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

同步更新于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 (\begin{array}{c}[{\dot{\xi }}_{R_t}{]}_{\times }\\ 0\\ 0\end{array}\end{array}$$