Quaternion based Kalman Filter for attitude estimation

1.系统模型

1.1 陀螺模型

$$\begin{array}{rl}{\mathbf{\omega }}^{\ast }& =\mathbf{\omega }+\mathbf{b}+{\mathbf{\eta }}_{\mathbf{\omega }}\\ \dot{\mathbf{b}}& ={\mathbf{\eta }}_{\mathbf{b}}\end{array}\text{\hspace{1em}(1)}$$

其中，$\omega$是真实的角速度，$b$是偏置，${\omega }^{\ast }$是角速度的测量值。${\eta }_{\omega }$和${\eta }_b$是零均值白噪声。

1.2 运动学模型

  待估计状态为$\mathbf{x}=[\mathbf{q},\mathbf{b}{]}^T$，噪声为$\eta ={\left[{\mathbf{\eta }}_{\omega },{\mathbf{\eta }}_b\right]}^T$。
四元数的微分运动学方程为
$$\dot{\mathbf{q}}=\frac{1}{2}\left[\begin{array}{l}\mathbf{\omega }\\ 0\end{array}\right]\otimes \mathbf{q}\text{\hspace{1em}(2)}$$

根据式()，可得待估计状态的动力学方程
$$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x})+\mathbf{g}(\mathbf{x},\mathbf{\eta })\text{\hspace{1em}(3)}$$

其中，
$$\begin{array}{rl}\mathbf{f}(\mathbf{x})& =\left[\begin{array}{c}\frac{1}{2}\left[\begin{array}{l}{\mathbf{\omega }}^{\ast }-\mathbf{b}\\ 0\end{array}\right]\otimes \mathbf{q}\\ 0\end{array}\right]\\ \mathbf{g}(\mathbf{x},\mathbf{\eta })& =\left[\begin{array}{c}\frac{1}{2}\left[\begin{array}{l}-{\mathbf{\eta }}_{\omega }\\ 0\end{array}\right]\otimes \mathbf{q}\\ {\mathbf{\eta }}_b\end{array}\right]\end{array}\text{\hspace{1em}(4)}$$

  状态估计$\bar{\mathbf{x}}$的传播利用无噪声的非线性运动方程：
$$\dot{\bar{\mathbf{x}}}=f(\bar{\mathbf{x}})\text{\hspace{1em}(5)}$$

  EKF中，状态的协方差矩阵$[P]$表明了状态的一阶不确定度。利用$\delta \mathbf{x}=[\delta \mathbf{\theta },\delta \mathbf{b}]$，对式在当前估计$\bar{\mathbf{x}}$处进行线性化，得到
$$\delta \dot{\mathbf{x}}=[F]\delta \mathbf{x}+[G]\mathbf{\eta }\text{\hspace{1em}(6)}$$

其中
$$[F]={\frac{\partial \mathbf{f}}{\partial \mathbf{x}}|}_{\mathbf{x}=\overline{\mathbf{x}}}=\left[\begin{array}{cc}-{\overline{\mathbf{\omega }}}^{\times }& -{\mathbf{I}}_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right]\text{\hspace{1em}(7)}$$

$$[G]={\frac{\partial \mathbf{g}}{\partial \mathbf{\eta }}|}_{\mathbf{x}=\overline{\mathbf{x}},\mathbf{\eta }=0}=\left[\begin{array}{cc}-{\mathbf{I}}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right]\text{\hspace{1em}(8)}$$

$$[Q]=\left[\begin{array}{cc}{\sigma }_{\omega }^2{\mathbf{I}}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\sigma }_b^2{\mathbf{I}}_{3\times 3}\end{array}\right]\text{\hspace{1em}(8)}$$

式中，$\bar{\mathbf{\omega }}={\mathbf{\omega }}^{\ast }-\bar{\mathbf{b}}$。则状态协方差按照如下的里卡提微分方程传播
$$[\dot{P}]=[F][P]+[P][F{]}^T+[G][Q][G{]}^T\text{\hspace{1em}(9)}$$

  测量残差为测量与估计的姿态之差
$${\mathbf{y}}_{\mathbf{k}}=\delta {\mathbf{\theta }}^{\ast }$$

其中，$\left[H_k\right]=\left[I_{3\times 3},0\right]$。
  滤波增益为
$$\left[K_k\right]=\left[P_k\right]{\left[H_k\right]}^T{\left(\left[H_k\right]\left[P_k\right]{\left[H_k\right]}^T+\left[{\Lambda }_{\sigma }\right]\right)}^{-1}$$

  状态和协方差按照以下公式进行更新
$$\begin{array}{rl}& \delta {\mathbf{x}}_k=[\delta \mathbf{\theta },\delta \mathbf{b}]=\left[K_k\right]{\mathbf{y}}_{\mathbf{k}}\\ & \left[P_k\right]=\left(\left[I_{6\times 6}\right]-\left[K_k\right]\left[H_k\right]\right)\left[P_k\right]{\left(\left[I_{3\times 3}\right]-\left[K_k\right]\left[H_k\right]\right)}^{-1}+\left[H_k\right]\left[P_k\right]{\left[H_k\right]}^T\end{array}$$
则$$\begin{gathered} {\hat{\mathbf{b}}}^{+}=\bar{\mathbf{b}}+\delta \mathbf{b} \\ {\hat{\mathbf{q}}}^{+}=\left[\begin{array}{l}\frac{1}{2}\delta \mathbf{\theta }\\ \sqrt{1-\frac{1}{4}\delta {\mathbf{\theta }}^T\delta \mathbf{\theta }}\end{array}\right]\otimes \bar{\mathbf{q}} \end{gathered}$$

2仿真结果

（图1 陀螺偏置估计结果）

（图2 姿态误差）

心得，评注

1. 编程一定要认真，尤其是在现有程序上进行改动的时候，一定要考虑周全。
   
   这个xplus(5:7)在mrp的导航里是xplus(4:6)，在修改成quaternion后一直没改过来，导致程序有问题

参考文献：
optimal estimation of dynamics system
Fundamentals of spacecraft attitude determine and control