＜＜Indirect Kalman Filter for 3D Attitude Estimation＞＞学习笔记

1. 该文定义四元数为JPL标准 [虚部，实部]， 不同于Hamilton标准，符合左手系规则。

     i j = - k，

2. 左右乘法

3. 四元数微分

4. 四元数积分

四元数的积分主要用于INS中的姿态的递推。

本文采用左乘的形式进行位姿增量的计算：

 

4.1 零阶积分

在短的陀螺采样周期中，认为角速度恒定。构造等效旋转矢量：

则零阶积分可以为：

                              

当更新周期极短时(极小<10e-5)，使用LHopital法则进行更新：

                                

 

5. 四元数的误差模型

5.1 测量模型

首先陀螺仪的测量模型为： ， 噪声n为零均值的高斯白噪声。

陀螺的bias建模为随机游走模型，所谓的随机游走模型即为：k时刻的值=k-1时刻的值+零均值的随机噪声。

其噪声满足： 

 

5.2 连续时间的误差状态推导

考虑噪声的true state：

                                          

不考虑噪声的normal state：

                                                                 其中：  

误差定义：

               

连续时间的误差状态：

                                  

                                          其中：

代入可得：

               

忽略二次项得：

                                          最后： 

bias的误差等式为： 

则 整体的误差状态方程为：