Modified Rodrigues Parameters Kalman Filter

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{\sigma },\mathbf{b}{]}^T$，噪声为$\eta ={\left[{\mathbf{\eta }}_{\omega },{\mathbf{\eta }}_b\right]}^T$。
修正罗德里格参数的微分运动学方程为
$$\dot{\mathbf{\sigma }}=\frac{1}{4}\mathbf{B}(\mathbf{\sigma })\mathbf{\sigma }=\frac{1}{4}\left[\left(1-{\mathbf{\sigma }}^T\mathbf{\sigma }\right)I+2{\mathbf{\sigma }}^{\times }+2\mathbf{\sigma }{\mathbf{\sigma }}^T\right]\mathbf{\omega }\text{\hspace{1em}(2)}$$

根据式(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}{4}[\mathbf{B}(\mathbf{\sigma })]\left({\mathbf{\omega }}^{\ast }-\mathbf{b}\right)\\ 0\end{array}\right]\\ \mathbf{g}(\mathbf{x},\mathbf{\eta })& =\left[\begin{array}{c}-\frac{1}{4}[\mathbf{B}(\mathbf{\sigma })]{\mathbf{\eta }}_{\omega }\\ {\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}=\mathbf{x}-\bar{\mathbf{x}}$，对式在当前估计$\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}(1/2)\left[\overline{\mathbf{\sigma }}{\bar{\mathbf{\omega }}}^T-\overline{\mathbf{\omega }}{\overline{\mathbf{\sigma }}}^T-{\overline{\mathbf{\omega }}}^{\times }+{\overline{\mathbf{\omega }}}^T\overline{\mathbf{\sigma }}\mathbf{I}\right]& -(1/4)\mathbf{B}(\overline{\mathbf{\sigma }})\\ 0& 0\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}-(1/4)\mathbf{B}(\overline{\mathbf{\sigma }})& 0\\ 0& \mathbf{I}\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)}$$

  测量残差为测量与估计的姿态之差
$$y_k={\mathbf{\sigma }}^{\ast }-\left[H_k\right]{\overline{\mathbf{\sigma }}}_k$$

其中，$\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}& {\bar{x}}_k={\bar{x}}_k+\left[K_k\right]y_k\\ & \left[P_k\right]=\left(\left[I_{3\times 3}\right]-\left[K_k\right]\left[P_k\right]\right)\left[P_k\right]{\left(\left[I_{3\times 3}\right]-\left[K_k\right]\left[P_k\right]\right)}^{-1}\left[H_k\right]\left[P_k\right]{\left[H_k\right]}^T\end{array}$$

  具体操作的时候，在测量更新这一步，在公式(2)中选择测量${\mathbf{\sigma }}^{\ast }$或其影子集${\mathbf{\sigma }}^{\ast S}$中使残差$y_k$的范数较小。为了进一步缩小范围，只有当$\Vert {\mathbf{\sigma }}^{\ast }\Vert >\frac{1}{3}$时，才需要对二者对应的残差进行比较。(因为如果$\Vert {\mathbf{\sigma }}^{\ast }\Vert <\frac{1}{3}$,，影子集对应的残差范数肯定较大，不予考虑。）

2.仿真结果

（图1 姿态误差）

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

心得，评注

1. 对于状态和方差的一步预测即公式(5),(9)，利用简单的欧拉积分即可。
2. 对于(7)式的计算是关键，涉及到对向量$\mathbf{\sigma }$求偏导。按照张贤达矩阵分析与应用中的矩阵函数的求导方法。令
   $${\mathbf{f}}_1(\mathbf{x})=\frac{1}{4}\mathbf{B}(\mathbf{\sigma })\left({\mathbf{\omega }}^{\ast }-\mathbf{b}\right)=\frac{1}{4}\mathbf{B}(\mathbf{\sigma })\bar{\mathbf{\omega }}$$

展开之后得，
$${\mathbf{f}}_1(\mathbf{x})=\frac{1}{4}[-\mathbf{\omega }{\mathbf{\sigma }}^T\mathbf{\sigma }-2{\mathbf{\omega }}^{\times }\mathbf{\sigma }+2\mathbf{\sigma }{\mathbf{\sigma }}^T\mathbf{\omega }]$$
两边对$\mathbf{\sigma }$求微分，得
$$\begin{array}{r}d{\mathbf{f}}_1(\mathbf{x})=\frac{1}{4}[-\mathbf{\omega }d({\mathbf{\sigma }}^T)\mathbf{\sigma }-\mathbf{\omega }{\mathbf{\sigma }}^{T}d\mathbf{\sigma }-2{\mathbf{\omega }}^{\times }d\mathbf{\sigma }+2d\mathbf{\sigma }{\mathbf{\sigma }}^T\mathbf{\omega }+2\mathbf{\sigma }d({\mathbf{\sigma }}^T)\mathbf{\omega }]\\ =\frac{1}{4}[-({\mathbf{\sigma }}^T\otimes \mathbf{\omega })d\mathbf{\sigma }--\mathbf{\omega }{\mathbf{\sigma }}^{T}d\mathbf{\sigma }-2{\mathbf{\omega }}^{\times }d\mathbf{\sigma }+2{\mathbf{\omega }}^T\mathbf{\sigma }{I}_{3}d\mathbf{\sigma }+2({\mathbf{\omega }}^T\otimes \mathbf{\sigma })]\\ =\frac{1}{2}[-\mathbf{\omega }{\mathbf{\sigma }}^T-{\mathbf{\omega }}^{\times }+{\mathbf{\omega }}^T\mathbf{\sigma }{I}_3+\mathbf{\sigma }{\mathbf{\omega }}^T]d\mathbf{\sigma }\end{array}$$

因此，
$$\frac{\partial {\mathbf{f}}_1}{\partial {\mathbf{\sigma }}^T}{|}_{\mathbf{x}=\overline{\mathbf{x}}}=\frac{1}{2}[-\bar{\mathbf{\omega }}{\bar{\mathbf{\sigma }}}^T-{\bar{\mathbf{\omega }}}^{\times }+{\bar{\mathbf{\omega }}}^T\bar{\mathbf{\sigma }}{I}_3+\bar{\mathbf{\sigma }}{\bar{\mathbf{\omega }}}^T]$$

即式(7)成立。

3. EKF可以这样来看待：状态由确定性部分和随机量两部分组成。状态的一步预测估计，是在当前最优估计的基础上按照微分运动微分方程进行积分。这一部分没有任何的随机量。随机量就是在当前最优估计附近的一个小扰动。通过状态的导数函数$\mathbf{f}(x)$在当前最优估计处泰勒展开，可获得该扰动其的线性运动方程。通过积分连续里卡提方程，即可获得协方差的更新。