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

文章同步更新于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)$$

令

$$\begin{array}{rl}{\hat{R}}_t{R}_t^T& ={\eta }_{R_t}\\ {\hat{v}}_t-{\hat{R}}_t{R}_t^T{v}_t& ={\xi }_{v_t}\\ {\hat{p}}_t-{\hat{R}}_t{R}_t^T{p}_t& ={\xi }_{p_t}\end{array}$$

设${\eta }_{R_t}$对应的李代数为${\xi }_{R_t}$，即${\eta }_{R_t}=\text{Exp}({\xi }_{R_t})$，由于${\eta }_{R_t}$是小量，用指数函数的一阶泰勒近似为${\eta }_{R_t}\approx I+[{\xi }_{R_t}{]}_{\times }$，则

$$\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)$$

即${\dot{\eta }}_t$就是群$S{E}_2(3)$在幺元处的切空间，其中

$$\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{]}_{\times }{R}_t^T-{\hat{R}}_t[w{]}_{\times }{R}_t^T\\ & ={\hat{R}}_t[{\tilde{w}}_t-w_t{]}_{\times }{R}_t^T\\ & ={\hat{R}}_t[n_t^w{]}_{\times }{R}_t^T\\ & ={\hat{R}}_t[n_t^w{]}_{\times }{\hat{R}}_t^T{\hat{R}}_t{R}_t^T\\ & =[{\hat{R}}_t{n}_t^w{]}_{\times }{\eta }_{R_t}\\ & \approx [{\hat{R}}_t{n}_t^w{]}_{\times }(I+[{\xi }_{R_t}{]}_{\times })\\ & \approx [{\hat{R}}_t{n}_t^w{]}_{\times }\end{array}$$

上面最后一步忽略了高阶小量$[{\hat{R}}_t{n}_t^w{]}_{\times }[{\xi }_{R_t}{]}_{\times }$。

同理有

$$\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+g-[{\hat{R}}_t{n}_t^w{]}_{\times }{v}_t-{\hat{R}}_t{R}_t^T(R_t{a}_t+g)\\ & ={\hat{R}}_t({\tilde{a}}_t-a_t)+g-[{\hat{R}}_t{n}_t^w{]}_{\times }{v}_t-{\hat{R}}_t{R}_t^{T}g\\ & \approx {\hat{R}}_t{n}_t^a+g-[{\hat{R}}_t{n}_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+{\hat{R}}_t{n}_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{]}_{\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{]}_{\times }{p}_t\\ & ={\xi }_{v_t}+([p_t{]}_{\times }{\hat{R}}_t)n_t^w\end{array}$$

综上有

$$\begin{array}{rl}\frac{d}{dt}\Lambda \left(\begin{array}{c}{\xi }_{R_t}\\ {\xi }_{v_t}\\ {\xi }_{p_t}\end{array}\right)& =\left(\begin{array}{ccc}[{\hat{R}}_t{n}_t^w{]}_{\times }& [g{]}_{\times }{\xi }_{R_t}+([v_t{]}_{\times }{\hat{R}}_t)n_t^w+{\hat{R}}_t{n}_t^a& {\xi }_{v_t}+([p_t{]}_{\times }{\hat{R}}_t)n_t^w\\ 0& 0& 0\\ 0& 0& 0\end{array}\right)\\ & =\Lambda [\left(\begin{array}{ccc}0& 0& 0\\ [g{]}_{\times }& 0& 0\\ 0& I& 0\end{array}\right)\left(\begin{array}{c}{\xi }_{R_t}\\ {\xi }_{v_t}\\ {\xi }_{p_t}\end{array}\right)+\left(\begin{array}{c}{\hat{R}}_t{n}_t^w\\ ([v_t{]}_{\times }{\hat{R}}_t)n_t^w+{\hat{R}}_t{n}_t^a\\ ([p_t{]}_{\times }{\hat{R}}_t)n_t^w\end{array}\right)]\text{分离状态项和噪声项}\\ & =\Lambda [\left(\begin{array}{ccc}0& 0& 0\\ [g{]}_{\times }& 0& 0\\ 0& I& 0\end{array}\right)\left(\begin{array}{c}{\xi }_{R_t}\\ {\xi }_{v_t}\\ {\xi }_{p_t}\end{array}\right)+\left(\begin{array}{ccc}{\hat{R}}_t& 0& 0\\ [{\hat{v}}_t{]}_{\times }{\hat{R}}_t& {\hat{R}}_t& 0\\ [{\hat{p}}_t{]}_{\times }{\hat{R}}_t& 0& {\hat{R}}_t\end{array}\right)\left(\begin{array}{c}{n}_t^w\\ n_t^a\\ 0\end{array}\right)]\end{array}$$

令${\xi }_t=({\xi }_{R_t}^T,{\xi }_{v_t}^T,{\xi }_{p_t}^T{)}^T$，以及$n_t=(n_t^w,n_t^a,0)$，可得误差的线性系统方程

$${\dot{\xi }}_t=F_t{\xi }_t+A{d}_{{\hat{\chi }}_t}{n}_t$$

其中

$$\begin{array}{rl}{F}_t& =\left(\begin{array}{ccc}0& 0& 0\\ [g{]}_{\times }& 0& 0\\ 0& I& 0\end{array}\right)\\ A{d}_{{\hat{\chi }}_t}& =\left(\begin{array}{ccc}{\hat{R}}_t& 0& 0\\ [{\hat{v}}_t{]}_{\times }{\hat{R}}_t& {\hat{R}}_t& 0\\ [{\hat{p}}_t{]}_{\times }{\hat{R}}_t& 0& {\hat{R}}_t\end{array}\right)\end{array}$$

与普通EKF的误差系统方程相比，最大不同就是系统矩阵$F_t$是一个常量！和输入以及状态估计值均无关！而噪声项前面的矩阵，刚好是当前状态矩阵的伴随。基于这样的系统方程，可以证明其收敛性还有一致性都有较好的保证。

后面的事情就和普通EKF一样了，根据系统方程估计下一时刻的状态（使用RK4积分等方法），然后对误差系统方程积分，得到状态转移矩阵，并传递协方差矩阵，即

$$P_t={\Phi }_t{P}_{t-1}{\Phi }_t^T+A{d}_{{\hat{\chi }}_t}{Q}_{t}A{d}_{{\hat{\chi }}_t}^T$$

其中${\Phi }_t$是通过$F_t$积分后得到的状态转移矩阵，$Q_t=E(n_t{n}_t^T)$是系统噪声的协方差矩阵。由于系统矩阵是常量，积分可以变得非常简单，甚至直接写出解析式。这也算是InEKF的另一个优点吧。

状态更新

假设地图中有很多landmark，用$m_1,m_2,...,m_k$表示，无人机上的传感器可以观测到这些landmark相对于自身的位置，分别用$y_t^1,y_t^2,...,y_t^k$表示，设每个观测的噪声为$s_t^1,s_t^2,...,s_t^k$，则观测方程就是

$$\begin{array}{rl}{y}_t^1& =R_t^T(m_1-p_t)+s_t^1\\ y_t^2& =R_t^T(m_2-p_t)+s_t^2\\ & ⋮\\ y_t^k& =R_t^T(m_k-p_t)+s_t^k\end{array}$$

拿其中一个观测来举例，我们可以发现观测方程可以写为如下形式

$$\begin{array}{rl}\left(\begin{array}{c}{y}_i\\ 0\\ 1\end{array}\right)& =\left(\begin{array}{ccc}{R}_t^T& -R_t^T{v}_t& -R_t^T{p}_t\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{m}_i\\ 0\\ 1\end{array}\right)+\left(\begin{array}{c}{s}_t^i\\ 0\\ 0\end{array}\right)\end{array}$$

注意等号右边第一项的矩阵是状态矩阵的逆，令$Y_t^i=(y_t^i,0,1),M^i=(m_i,0,1),S_t^i=(s_t^i,0,0)$（由于不方便书写，所以都写为行向量的形式，实际上它们都是列向量），有

$$Y_t^i={\chi }_t^{-1}{M}^i+S_t^i$$

现在我们定义观测误差。如果定义为

$$Z_t^i=Y_t^i-{\hat{\chi }}_t^{-1}{M}^i$$

那么就与普通的EKF无异。考虑到我们的状态量是一个群元素，可以定义观测误差为（这样定义的目的在后面的推导中会变清晰）

$$Z_t^i={\hat{\chi }}_t{Y}_t^i-M^i$$

这样就可以得到

$$\begin{array}{rl}{Z}_t^i& ={\hat{\chi }}_t({\chi }_t^{-1}{M}^i+S_t^i)-M^i\\ & ={\hat{\chi }}_t{\chi }_t^{-1}{M}^i-M^i+{\hat{\chi }}_t{S}_t^i\\ & ={\eta }_t{M}^i-M^i+{\hat{\chi }}_t{S}_t^i\end{array}$$

将所有的观测误差按照列向量的方式堆叠起来，有

$$z_t=\left(\begin{array}{c}{\eta }_t{M}^1-M^1+{\hat{\chi }}_t{S}_t^1\\ ⋮\\ {\eta }_t{M}^k-M^k+{\hat{\chi }}_t{S}_t^k\end{array}\right)$$

和EKF一样，现在需要求出观测误差与状态误差之间的线性关系。利用状态误差的一阶近似${\eta }_t\approx I+\Lambda ({\xi }_t)$，有

$$\begin{array}{rl}{z}_t& \approx \left(\begin{array}{c}(I+\Lambda ({\xi }_t))M^1-M^1+{\hat{\chi }}_t{S}_t^1\\ ⋮\\ (I+\Lambda ({\xi }_t))M^k-M^k+{\hat{\chi }}_t{S}_t^k\end{array}\right)\\ & =\left(\begin{array}{c}\Lambda ({\xi }_t)M^1+{\hat{\chi }}_t{S}_t^1\\ ⋮\\ \Lambda ({\xi }_t)M^k+{\hat{\chi }}_t{S}_t^k\end{array}\right)\end{array}$$

将$M^k,S_t^k,{\hat{\chi }}_t,{\xi }_t$的具体表达式带入上式计算后，每三行就有两行是全零，为了使计算和书写更紧凑，可以将$z_t$中全是零的行去掉，得到${\tilde{z}}_t$，即

$$\begin{array}{rl}{\tilde{z}}_t& =\left(\begin{array}{c}[{\xi }_{R_t}{]}_{\times }{m}_1+{\xi }_{p_t}+{\hat{R}}_t{s}_t^1\\ ⋮\\ [{\xi }_{R_t}{]}_{\times }{m}_k+{\xi }_{p_t}+{\hat{R}}_t{s}_t^k\end{array}\right)\\ & =\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)\left(\begin{array}{c}{\xi }_{R_t}\\ {\xi }_{v_t}\\ {\xi }_{p_t}\end{array}\right)+\left(\begin{array}{c}{\hat{R}}_t{s}_t^1\\ ⋮\\ {\hat{R}}_t{s}_t^k\end{array}\right)\\ & =H_t{\xi }_t+s_t\end{array}$$

下面考虑怎么根据观测误差更新InEKF的状态。回想一下普通EKF的更新方式

$${\hat{x}}_t^{+}={\hat{x}}_t+K_t[y_t-h({\hat{x}}_t)]={\hat{x}}_t+K_t{z}_t$$

等价于状态误差的更新（上式两边减去$x_t$）

$$e_t^{+}=e_t+K_t{z}_t$$

其中$K_t$是卡尔曼增益。

同理，根据InEKF的误差定义，更新后的误差

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

其中${\hat{\chi }}_t^{+}{\hat{\chi }}_t^{-1}$就是更新前后状态的差异，可以设

$${\hat{\chi }}_t^{+}{\hat{\chi }}_t^{-1}=\text{Exp}(K_t{z}_t)$$

就得InEKF的状态误差的更新方程

$${\eta }_t^{+}=\text{Exp}(K_t{z}_t){\eta }_t$$

以及状态的更新方程

$${\hat{\chi }}_t^{+}=\text{Exp}(K_t{z}_t){\hat{\chi }}_t$$

即InEKF采用的是“指数更新”。最后来看看增益$K_t$怎么算，由误差的更新方程以及观测误差的定义可得

$$\begin{array}{rl}{\eta }_t^{+}& =\text{Exp}(K_t{z}_t){\eta }_t\end{array}$$

利用指数函数的一阶近似$\text{Exp}(\xi )\approx I+\Lambda (\xi )$，有

$$I+\Lambda ({\xi }_t^{+})=[I+\Lambda (K_t{z}_t)][I+\Lambda ({\xi }_t)]$$

忽略高阶小量$\Lambda ()\cdot \Lambda ()$，有

$$\begin{array}{rl}{\xi }_t^{+}& ={\xi }_t+K_t{z}_t\end{array}$$

上式与普通EKF中的误差更新方程$e_t^{+}=e_t+K_t{z}_t$有相同的形式，所以增益$K_t$的计算和普通的EKF完全相同。上式也可以用更紧凑的${\tilde{z}}_t$代替，对应的更紧凑的增益记为${\tilde{K}}_t$，则

$${\tilde{K}}_t=P_t{H}_t^T(H_t{P}_t{H}_t^T+V_t{)}^{-1}$$

其中，$P_t$是状态误差${\xi }_t$的协方差矩阵，$H_t$是之前推出的${\tilde{z}}_t$与${\xi }_t$之间的观测矩阵，$V_t=E(s_t{s}_t^T)$是观测误差的协方差矩阵（注意，$s_t$中的每一项噪声都被${\hat{R}}_t$旋转过）。状态的更新自然就是

$$\begin{array}{rl}{\hat{\chi }}_t^{+}& =\text{Exp}({\tilde{K}}_t{\tilde{z}}_t){\hat{\chi }}_t\\ P_t^{+}& =(I-{\tilde{K}}_t{H}_t)P_t\end{array}$$

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
3. Invariant Kalman Filtering for Visual Inertial SLAM