Modified Rodrigues Parameters Kalman Filter

1.系统模型

1.1 陀螺模型

$\begin{aligned} \boldsymbol{\omega^{*}} &=\boldsymbol{\omega}+\boldsymbol{b}+\boldsymbol{\eta_{\omega}}\\ \boldsymbol{\dot{b}}&=\boldsymbol{\eta_{b}} \end{aligned}\tag{1}$

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

1.2 运动学模型

待估计状态为 $\boldsymbol{x}=[\boldsymbol{\sigma}, \boldsymbol{b}]^{T}$ ，噪声为 $\eta=\left[\boldsymbol{\eta}_{\omega}, \boldsymbol{\eta}_{b}\right]^{T}$ 。
修正罗德里格参数的微分运动学方程为
$\dot{\boldsymbol{\sigma}}=\frac{1}{4} \boldsymbol{B}(\boldsymbol{\sigma}) \boldsymbol{\sigma}=\frac{1}{4}\left[\left(1-\boldsymbol{\sigma}^{T} \boldsymbol{\sigma}\right) I+2 \boldsymbol{\sigma}^{\times}+2 \boldsymbol{\sigma} \boldsymbol{\sigma}^{T}\right] \boldsymbol{\omega}\tag{2}$

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

其中，
$\begin{aligned} \boldsymbol{f}(\boldsymbol{x}) &=\left[\begin{array}{c} \frac{1}{4}[\boldsymbol{B}(\boldsymbol{\sigma})]\left(\boldsymbol{\omega}^{*}-\boldsymbol{b}\right) \\ \mathbf{0} \end{array}\right] \\ \boldsymbol{g}(\boldsymbol{x}, \boldsymbol{\eta}) &=\left[\begin{array}{c} -\frac{1}{4}[\boldsymbol{B}(\boldsymbol{\sigma})] \boldsymbol{\eta}_{\omega} \\ \boldsymbol{\eta}_{b} \end{array}\right] \end{aligned}\tag{4}$

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

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

其中
$[F] =\left.\frac{\partial \boldsymbol{f}}{\partial \boldsymbol{x}}\right|_{\boldsymbol{x}=\overline{\boldsymbol{x}}}=\left[\begin{array}{cc} (1 / 2)\left[\overline{\boldsymbol{\sigma}} \bar{\boldsymbol{\omega}}^{T}-\overline{\boldsymbol{\omega}} \overline{\boldsymbol{\sigma}}^{T}-\overline{\boldsymbol{\omega}}^{\times}+\overline{\boldsymbol{\omega}}^{T} \overline{\boldsymbol{\sigma}} \boldsymbol{I}\right] & -(1 / 4) \boldsymbol{B}(\overline{\boldsymbol{\sigma}}) \\ 0 & \mathbf{0} \end{array}\right]\tag{7}$

$[G]=\left.\frac{\partial \boldsymbol{g}}{\partial \boldsymbol{\eta}}\right|_{\boldsymbol{x}=\overline{\boldsymbol{x}}, \boldsymbol{\eta}=\mathbf{0}}=\left[\begin{array}{cc} -(1 / 4) \boldsymbol{B}(\overline{\boldsymbol{\sigma}}) & \mathbf{0} \\ \mathbf{0} & \boldsymbol{I} \end{array}\right]\tag{8}$

式中， $\boldsymbol{\bar{\omega}}=\boldsymbol{\omega^*}-\boldsymbol{\bar{b}}$ 。则状态协方差按照如下的里卡提微分方程传播
$[\dot{P}]=[F][P]+[P][F]^{T}+[G][Q][G]^{T}\tag{9}$

测量残差为测量与估计的姿态之差
$y_{k}=\boldsymbol{\sigma}^{*}-\left[H_{k}\right] \overline{\boldsymbol{\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{aligned} &\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{aligned}$

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