不变扩展卡尔曼滤波（二）：原理与推导

文章同步更新于github pages，欢迎收藏关注。

扩展卡尔曼滤波的缺陷

存在正反馈

普通的扩展卡尔曼滤波（EKF），通过对动态方程的线性化，来估计状态与状态的协方差。比如有一个系统定义为

$$\begin{array}{rl}{\dot{x}}_t& =f(x_t,u_t)+n_t\end{array}$$

其中$x_t$是系统状态，$u_t$是输入，$n_t$是系统噪声。假设某个时刻对状态的估计为${\hat{x}}_t$，并定义状态误差为$e_t=x_t-{\hat{x}}_t$，则根据EKF，对$f$在${\hat{x}}_t$处线性化（一阶泰勒展开），并注意到$f({\hat{x}}_{t_n},u_t)={\dot{\hat{x}}}_{t_{n+1}}$，有

$$\begin{array}{rl}{\dot{e}}_t& ={\dot{x}}_t-{\dot{\hat{x}}}_t\\ & =f(x_t,u_t)-{\dot{\hat{x}}}_t+n_t\\ & \approx f({\hat{x}}_t,u_t)+F_{{\hat{x}}_t,u_t}(x_t-{\hat{x}}_t)-{\dot{\hat{x}}}_t+n_t\\ & =F_{{\hat{x}}_t,u_t}(x_t-{\hat{x}}_t)+n_t\\ & =F_{{\hat{x}}_t,u_t}{e}_t+n_t\end{array}$$

上式是状态误差的线性传递方程，因此可以使用标准的卡尔曼滤波来估计协方差。

但是这里存在一个问题，上面误差传递方程中，系统矩阵$F$依赖当前状态的估计量。在有噪声引入时，状态估计量是无法预测的，这就导致很难对上述系统方程做收敛性分析，实际上，任何EKF都没有收敛性保证。当状态估计值与真值差距较大时，直接导致依赖状态估计值的$F$也有较大偏差，使用这样的$F$继续传递误差后，又进一步放大误差，整个误差传递系统形成正反馈，最终导致滤波器发散。

实际上，在滤波器进行状态更新时也有类似问题。设系统的观测方程为

$$y_t=h(x_t)+s_t$$

其中$s_t$是观测噪声。设实际观测与估计观测的误差为$z_t=y_t-{\hat{y}}_t$，有

$$\begin{array}{rl}{z}_t& =y_t-{\hat{y}}_t\\ & =h(x_t)-{\hat{y}}_t+s_t\\ & \approx h({\hat{x}}_t)+H_{{\hat{x}}_t}(x_t-{\hat{x}}_t)-{\hat{y}}_t+s_t\\ & =H_{{\hat{x}}_t}{e}_t+s_t\end{array}$$

上式即观测误差与状态误差之间的线性近似关系。同样的，$H_{{\hat{x}}_t}$与状态估计值有关，当真实状态与估计值差距较大时，利用上式进行更新可能起不到正面效果。

从上面的分析可知，普通EKF对初值的准确性要求挺高的，如果状态初值设定得与实际情况差距较大，很难让滤波器收敛。

非一致性

EKF还会导致另外一个问题，即所谓的不一致性。每当新的观测到来时，EKF会更新当前状态的估计，但是用于计算新状态的量，都是通过旧状态的线性化得来的。使用EKF的更新方法会出现不一致性，导致本来不该观测到信息的方向上观测到信息，在SLAM问题中这种现象比较明显，表现为较大的drift，以及过于乐观的协方差估计。因此，有很多针对该问题的方法，比如OC（Observability Constrain），FEJ（First Estimateed Jacobian）以及Robocentric等，这些方法都是对现有EKF框架的修补，而且使用起来都不容易。最后引用一张[1]中的图更直观的描述这个问题。

不变扩展卡尔曼滤波

不变扩展卡尔曼滤波（简写为InEKF），可以较好的解决上面EKF存在的两个问题，而且有严格的数学推导作为保证。这里不介绍InEKF的收敛性和一致性的推导（实在有点复杂*_*!），主要关注其思想和用法。

InEKF的想法还是比较简单的——通过改变状态误差的定义方法，实现误差传递矩阵$F$与状态估计值的独立。在EKF中，我们的状态误差直接定义为两个状态的差，即$e_t=x_t-{\hat{x}}_t$，这太过于粗暴了，我们面对的是非线性系统，误差怎么都不应该是线性的形式。

现在我们以一个简单的问题为例，说明InEKF的用法。假设我们有一架无人机，上面装了IMU以及双目相机等传感器，我们打算对无人机的位姿进行估计。假设IMU的bias为零（之后会讨论有bias的情况），则IMU的系统方程为

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

其中${\tilde{w}}_t,{\tilde{a}}_t$是IMU的测量值，$n_t^w,n_t^a$分别是gyro和acc的噪声，$g$是重力设为已知，待估计的量就是$R,v,p$，根据上一章李群的内容，我们发现这三个量刚好能构成一个$S{E}_2(3)$群，于是我们将InEKF的状态量写为

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

这里已经和EKF有很大不同了，一般EKF的状态量是个矢量，而InEKF的状态量是一个矩阵，而且构成群。既然是群，其误差也应该定义在群上，自然是不能再用减法了，设状态估计值为${\hat{\chi }}_t$，和真值的误差有两种形式

$$\begin{array}{rl}{\eta }_t& ={\chi }_t^{-1}{\hat{\chi }}_t\\ {\eta }_t& ={\hat{\chi }}_t{\chi }_t^{-1}\end{array}$$

第一种称为左不变误差（left-invariant），因为对${\chi }_t,\widehat{{\chi }_t}$都左乘一个相同的群元素后，误差不变。同理，第二种为右不变误差（right-invariant）。具体使用哪种误差，要看该误差能否实现状态传递矩阵与状态估计值的独立。在很多SLAM问题中，右不变误差可以满足要求，因此下文都使用右不变误差。

误差传递

仿照EKF，现在推导这个误差的系统方程。

$${\eta }_t={\hat{\chi }}_t{\chi }_t^{-1}=\left(\begin{array}{ccc}{\hat{R}}_t{R}_t^T& {\hat{v}}_t-{\hat{R}}_t{R}_t^T{v}_t& {\hat{p}}_t-{\hat{R}}_t{R}_t^T{p}_t\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)$$

令

R ^ t R t T = η R t v ^ t − R ^ t R t T v t = ξ v t p ^ t − R ^ t R t T p t = ξ p t \begin{aligned} \hat{R}_tR_t^T&=\eta_{R_t} \\ \hat{v}_t-\hat{R}_tR_t^Tv_t&=\xi_{v_t} \\ \hat{p}_t-\hat{R}_tR_t^Tp_t&=\xi_{p_t} \end{aligned} R^t​RtT​v^t​−R^t​R