不变扩展卡尔曼滤波（三）：一个更通用的框架

同步更新于github page。

复杂系统下的不变扩展卡尔曼滤波

由前面的文章可知，不变性要靠合理的状态误差设计来实现。但在更复杂的系统中，为所有状态量找到一个满足不变性的状态误差是很困难的，甚至一些状态是不可能被表示为群运算的。在存在这类状态量的系统中，有没有可能保留InEKF的主要优点呢（主要指一致性方面的优点）？答案是肯定的。

考虑这样一类系统，其系统方程如下：

$$\begin{array}{rl}{\dot{\Theta }}_t& =g({\Theta }_t)\\ {\dot{\chi }}_t& =f({\chi }_t,u_t,{\Theta }_t)\\ y_t& =h({\chi }_t,{\Theta }_t)\end{array}$$

抛开矢量${\Theta }_t$不看，上式就是之前讨论的普通系统方程，引入矢量${\Theta }_t$后，系统的变化不仅依赖当前时刻的状态${\chi }_t$和输入$u_t$，还依赖另外一个随着时间变化的量${\Theta }_t$。在普通的EKF中，可以将${\Theta }_t$也融入到需要估计的状态量${\chi }_t$中。这里之所以分开写，是为了在InEKF中区分可以被描述为群元素的状态量${\chi }_t$，和不能被描述为群元素的量${\Theta }_t$。

以一个完整的IMU系统为例，系统方程为：

$$\begin{array}{rl}{\dot{R}}_t& =R_t[{\tilde{w}}_t-b_t^w-n_t^w{]}_{\times }\\ {\dot{v}}_t& =R_t({\tilde{a}}_t-b_t^a-n_t^a)+g\\ {\dot{p}}_t& =v_t\\ {\dot{b}}_t^w& =n_t^{bw}\\ {\dot{b}}_t^a& =n_t^{ba}\end{array}$$

其中$b_t^w,b_t^a$分别为IMU的gyro bias和accelerator bias，他们满足Random Walk模型，即随时间的变化率是一个满足高斯分布的白噪声。

由之前的文章可知，$R_t,v_t,p_t$能够构成一个群元素并且使得最终导出的误差系统方程满足不变性（系统矩阵与状态无关），但如果将两个bias也放入群中，就很难找到一个状态误差使其满足不变性了。也就是说，$R_t,v_t,p_t$就是上文中的${\chi }_t$，而$b_t^w,b_t^a$就是上文中的${\Theta }_t$。

为了处理这类系统，可以将状态误差定义为如下形式：

$$e_t=({\hat{\chi }}_t{\chi }_t^{-1},{\hat{\Theta }}_t-{\Theta }_t)=({\eta }_t,{\zeta }_t)$$

即右不变误差与普通线性误差的笛卡尔积，是一种混合误差。

混合误差的系统方程

现在需要推导在混合系统误差下，该误差的系统方程是怎样的。直接以上文的IMU系统为例，令

$${\chi }_t=\left(\begin{array}{ccc}{R}_t& v_t& p_t\\ 0& 1& 0\\ 0& 0& 1\end{array}\right),{\eta }_t={\hat{\chi }}_t{\chi }_t^{-1}$$

以及

$${\Theta }_t=\left(\begin{array}{c}{b}_t^w\\ b_t^a\end{array}\right),{\zeta }_t=\left(\begin{array}{c}{\hat{b}}_t^w-b_t^w\\ {\hat{b}}_t^a-b_t^a\end{array}\right)=\left(\begin{array}{c}{\zeta }_t^w\\ {\zeta }_t^a\end{array}\right)$$

推导方式模仿第二篇文章中普通系统的推导，也就是计算：

$${\dot{e}}_t=\left(\begin{array}{c}{\dot{\eta }}_t\\ \\ {\dot{\zeta }}_t\end{array}\right)$$

其中${\dot{\eta }}_t$的定义和上一篇文章中一样

$$\begin{array}{r}{\dot{\eta }}_t\approx \left(\begin{array}{ccc}[{\dot{\xi }}_{R_t}{]}_{\times }& {\dot{\xi }}_{v_t}& {\dot{\xi }}_{p_t}\\ 0& 0& 0\\ 0& 0& 0\end{array}\right)=\frac{d}{dt}\Lambda \left(\begin{array}{c}{\xi }_{R_t}\\ {\xi }_{v_t}\\ {\xi }_{p_t}\end{array}\right)\end{array}$$

仅在${\xi }_{R_t},{\xi }_{v_t},{\xi }_{p_t}$的计算中有少许不同，分别为

$$\begin{array}{rl}[{\dot{\xi }}_{R_t}{]}_{\times }\approx {\dot{\eta }}_{R_t}& ={\dot{\hat{R}}}_t{R}_t^T+{\hat{R}}_t{\dot{R}}_t^T\\ & ={\hat{R}}_t[{\tilde{w}}_t-{\hat{b}}_t^w{]}_{\times }{R}_t^T-{\hat{R}}_t[w_t{]}_{\times }{R}_t^T\\ & ={\hat{R}}_t[{\tilde{w}}_t-{\hat{b}}_t^w-w_t{]}_{\times }{R}_t^T(\text{其中}{\tilde{w}}_t=w_t+b_t^w+n_t^w)\\ & ={\hat{R}}_t[n_t^w-{\zeta }_t^w{]}_{\times }{R}_t^T\\ & ={\hat{R}}_t[n_t^w-{\zeta }_t^w{]}_{\times }{\hat{R}}_t^T{\hat{R}}_t{R}_t^T\\ & =[{\hat{R}}_t(n_t^w-{\zeta }_t^w){]}_{\times }{\eta }_{R_t}\\ & \approx [{\hat{R}}_t(n_t^w-{\zeta }_t^w){]}_{\times }(I+[{\xi }_{R_t}{]}_{\times })\\ & \approx [{\hat{R}}_t(n_t^w-{\zeta }_t^w){]}_{\times }\end{array}$$

以及

$$\begin{array}{rl}{\dot{\xi }}_{v_t}& ={\dot{\hat{v}}}_t-{\dot{\eta }}_{R_t}{v}_t-{\eta }_{R_t}{\dot{v}}_t\\ & ={\hat{R}}_t({\tilde{a}}_t-{\hat{b}}_t^a)+g-[{\hat{R}}_t(n_t^w-{\zeta }_t^w){]}_{\times }{v}_t-{\hat{R}}_t{R}_t^T(R_t{a}_t+g)\\ & ={\hat{R}}_t({\tilde{a}}_t-{\hat{b}}_t^a-a_t)+g-[{\hat{R}}_t(n_t^w-{\zeta }_t^w){]}_{\times }{v}_t-{\hat{R}}_t{R}_t^{T}g(\text{其中}{\tilde{a}}_t=a_t+b_t^a+n_t^a)\\ & \approx {\hat{R}}_t(n_t^a-{\zeta }_t^a)+g-[{\hat{R}}_t(n_t^w-{\zeta }_t^w){]}_{\times }{v}_t-(I+[{\xi }_{R_t}{]}_{\times })g\\ & =[g{]}_{\times }{\xi }_{R_t}+([v_t{]}_{\times }{\hat{R}}_t)(n_t^w-{\zeta }_t^w)+{\hat{R}}_t(n_t^a-{\zeta }_t^a)\end{array}$$

以及

$$\begin{array}{rl}{\dot{\xi }}_{p_t}& ={\dot{\hat{p}}}_t-{\dot{\eta }}_{R_t}{p}_t-{\eta }_{R_t}{\dot{p}}_t\\ & ={\hat{v}}_t-[{\hat{R}}_t(n_t^w-{\zeta }_t^w){]}_{\times }{p}_t-{\hat{R}}_t{R}_t^T{v}_t\\ & =({\hat{v}}_t-{\hat{R}}_t{R}_t^T{v}_t)-[{\hat{R}}_t(n_t^w-{\zeta }_t^w){]}_{\times }{p}_t\\ & ={\xi }_{v_t}+([p_t{]}_{\times }{\hat{R}}_t)(n_t^w-{\zeta }_t^w)\end{array}$$

而${\zeta }_t$由于是普通的线性误差，推导就比较简单了，即

$${\dot{\zeta }}_t=\left(\begin{array}{c}{\dot{\zeta }}_t^w\\ {\dot{\zeta }}_t^a\end{array}\right)=\left(\begin{array}{c}-n_t^{bw}\\ -n_t^{ba}\end{array}\right)$$

综上，模仿上一篇文章的处理方式，可以得到整个系统状态误差的系统方程：

$$\frac{d}{dt}\left(\begin{array}{c}{\xi }_{R_t}\\ {\xi }_{v_t}\\ {\xi }_{p_t}\\ {\zeta }_t^w\\ {\zeta }_t^a\end{array}\right)=\left(\begin{array}{ccccc}0& 0& 0& -{\hat{R}}_t& 0\\ [g{]}_{\times }& 0& 0& -[{\hat{v}}_t]{\hat{R}}_t& -{\hat{R}}_t\\ 0& I& 0& -[{\hat{p}}_t]{\hat{R}}_t& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right)\left(\begin{array}{c}{\xi }_{R_t}\\ {\xi }_{v_t}\\ {\xi }_{p_t}\\ {\zeta }_t^w\\ {\zeta }_t^a\end{array}\right)+\left(\begin{array}{ccccc}{\hat{R}}_t& 0& 0& 0& 0\\ [{\hat{v}}_t{]}_{\times }{\hat{R}}_t& {\hat{R}}_t& 0& 0& 0\\ [{\hat{p}}_t{]}_{\times }{\hat{R}}_t& 0& {\hat{R}}_t& 0& 0\\ 0& 0& 0& -I& 0\\ 0& 0& 0& 0& -I\end{array}\right)\left(\begin{array}{c}{n}_t^w\\ n_t^a\\ 0\\ n_t^{bw}\\ n_t^{ba}\end{array}\right)$$

将其写为更紧凑的形式，容易看出带有bias的IMU系统与之前不带bias的IMU系统之间的联系

$$\frac{d}{dt}\left(\begin{array}{c}{\xi }_t\\ {\zeta }_t\end{array}\right)=\left(\begin{array}{cc}{F}_{{\chi }_t}& F_{{\chi }_t,{\Theta }_t}\\ 0& F_{{\Theta }_t}\end{array}\right)\left(\begin{array}{c}{\xi }_t\\ {\zeta }_t\end{array}\right)+\left(\begin{array}{cc}A{d}_{{\hat{\chi }}_t}& 0\\ 0& -I\end{array}\right)\left(\begin{array}{c}{n}_t\\ n_t^b\end{array}\right)$$

其中$F_{{\chi }_t}$就是上一篇文章中，不带bias的IMU系统的系统矩阵，$A{d}_{{\hat{\chi }}_t}$是其噪声矩阵。带有bias的IMU系统，只是在原来的系统矩阵上进行了增广。这个结论适用于所有满足本文开头所定义的系统形式。

可以看到，在这种混合误差形式下，系统矩阵不再是一个常量了，每个时刻的系统矩阵与当前状态的估计值有关。但是，这种联系只通过bias或噪声等小量引入。可以证明对于这样的系统方程，一致性仍然能够保证。同时大量的实验也表明，这种“非完美”的InEKF在鲁棒性和收敛性上仍然优于传统的EKF。

混合状态的更新

之前已经知道，观测误差$z_t$与状态误差${\xi }_t$的线性关系为

$${\tilde{z}}_t=H_t{\xi }_t+s_t$$

其中$s_t$是观测噪声。

现在设系统的观测量不变（即${\Theta }_t$是无法直接观测到的），但是状态误差变成了$({\xi }_t,{\zeta }_t)$，显然线性关系应该变为

$${\tilde{z}}_t=(H_{{\chi }_t}{H}_{{\Theta }_t})\left(\begin{array}{c}{\xi }_t\\ {\zeta }_t\end{array}\right)+s_t$$

以带有bias的IMU系统为例，$H_{{\chi }_t}$就是上一篇文章中的观测矩阵$H_t$，即

$$H_{{\chi }_t}=\left(\begin{array}{ccc}-[m_1{]}_{\times }& 0_{3\times 3}& I_{3\times 3}\\ ⋮& & \\ -[m_k{]}_{\times }& 0_{3\times 3}& I_{3\times 3}\end{array}\right)$$

其中$m_k$是观测到的landmark在世界系下的坐标。而观测误差与bias的误差没有直接关系，即

$$H_{{\Theta }_t}=(0_{3k\times 3}{0}_{3k\times 3})$$

同理，线性状态误差的更新变为了：

$$\left(\begin{array}{c}{\xi }_t^{+}\\ {\zeta }_t^{+}\end{array}\right)=\left(\begin{array}{c}{\xi }_t\\ {\zeta }_t\end{array}\right)+\left(\begin{array}{c}{K}_t^{\xi }\\ K_t^{\zeta }\end{array}\right){\tilde{z}}_t$$

卡尔曼增益的计算都一样，即

$$K_t=\left(\begin{array}{c}{K}_t^{\xi }\\ K_t^{\zeta }\end{array}\right)=P_t{H}_t^T(H_t{P}_t{H}_t^T+V_t{)}^{-1}$$

只是现在的$H_t=(H_{{\chi }_t}{H}_{{\Theta }_t})$，状态的协方差矩阵$P_t$在维数上也有相应改变。

最终，混合状态的更新方式也采用混合的形式，能表示为群元素的部分采用指数更新，不能表示为群元素的部分采用普通的线性更新，即

$$\left(\begin{array}{c}{\hat{\chi }}_t^{+}\\ {\hat{\Theta }}_t^{+}\end{array}\right)=\left(\begin{array}{c}\text{Exp}(K_t^{\xi }{\tilde{z}}_t){\hat{\chi }}_t\\ {\hat{\Theta }}_t+K_t^{\zeta }{\tilde{z}}_t\end{array}\right)$$

总结

设系统方程满足如下形式：

$$\begin{array}{rl}{\dot{\Theta }}_t& =g({\Theta }_t)\\ {\dot{\chi }}_t& =f({\chi }_t,u_t,{\Theta }_t)\\ y_t& =h({\chi }_t,{\Theta }_t)\end{array}$$

其中${\chi }_t,{\Theta }_t$都是需要估计的量，但是${\chi }_t$是一个群元素（比如$S{E}_k(3)$中的元素），${\Theta }_t$是一个矢量，一般不可直接观测。则该系统在InEKF框架下，具有如下形式的系统矩阵$F_t$和观测矩阵$H_t$：

$$F_t=\left(\begin{array}{cc}{F}_{{\chi }_t}& F_{{\chi }_t,{\Theta }_t}\\ 0& F_{{\Theta }_t}\end{array}\right),H_t=(H_{{\chi }_t}{H}_{{\Theta }_t})$$

且卡尔曼增益

$$K_t=\left(\begin{array}{c}{K}_t^{\xi }\\ K_t^{\zeta }\end{array}\right)=P_t{H}_t^T(H_t{P}_t{H}_t^T+V_t{)}^{-1}$$

状态的更新方式为

$$\left(\begin{array}{c}{\hat{\chi }}_t^{+}\\ {\hat{\Theta }}_t^{+}\end{array}\right)=\left(\begin{array}{c}\text{Exp}(K_t^{\xi }{\tilde{z}}_t){\hat{\chi }}_t\\ {\hat{\Theta }}_t+K_t^{\zeta }{\tilde{z}}_t\end{array}\right)$$

References

1. Non-linear state error based extended Kalman flters with applications to navigation
2. Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation