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

扩展卡尔曼滤波的缺陷

存在正反馈

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

$\begin{aligned} \dot{x}_{t}&=f(x_t, u_t) + n_t \end{aligned}$x˙t​​=f(xt​,ut​)+nt​​

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

$\begin{aligned} \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{aligned}$e˙t​​=x˙t​−x^˙t​=f(xt​,ut​)−x^˙t​+nt​≈f(x^t​,ut​)+Fx^t​,ut​​(xt​−x^t​)−x^˙t​+nt​=Fx^t​,ut​​(xt​−x^t​)+nt​=Fx^t​,ut​​et​+nt​​

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

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

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

$y_t=h(x_t)+s_t$yt​=h(xt​)+st​

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

$\begin{aligned} 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{aligned}$zt​​=yt​−y^​t​=h(xt​)−y^​t​+st​≈h(x^t​)+Hx^t​​(xt​−x^t​)−y^​t​+st​=Hx^t​​et​+st​​

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

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

非一致性

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

不变扩展卡尔曼滤波

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

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

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

$\begin{aligned} \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{aligned}$R˙t​v˙t​p˙​t​​=Rt​[w~t​−ntw​]×​=Rt​(a~t​−nta​)+g=vt​​

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

$\chi_t= \begin{pmatrix} R_t & v_t & p_t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix}$χt​=⎝⎛​Rt​00​vt​10​pt​01​⎠⎞​

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

$\begin{aligned} \eta_t&=\chi_t^{-1}\hat{\chi}_t \\ \eta_t&=\hat{\chi}_t\chi_t^{-1} \end{aligned}$ηt​ηt​​=χt−1​χ^​t​=χ^​t​χt−1​​

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

误差传递

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

$\eta_t=\hat{\chi}_t\chi_t^{-1}= \begin{pmatrix} \hat{R}_tR_t^T & \hat{v}_t-\hat{R}_tR_t^Tv_t & \hat{p}_t-\hat{R}_tR_t^Tp_t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix}$ηt​=χ^​t​χt−1​=⎝⎛​R^t​RtT​00​v^t​−R^t​RtT​vt​10​p^​t​−R^t​RtT​pt​01​⎠⎞​

令

$\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​RtT​vt​p^​t​−R^t​RtT​pt​​=ηRt​​=ξvt​​=ξpt​​​

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

$\dot{\eta_t}\approx \begin{pmatrix} [\dot{\xi}_{R_t}]_\times & \dot{\xi}_{v_t} & \dot{\xi}_{p_t} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix} = \frac{d}{dt}\Lambda \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix}$ηt​˙​≈⎝⎛​[ξ˙​Rt​​]×​00​ξ˙​vt​​00​ξ˙​pt​​00​⎠⎞​=dtd​Λ⎝⎛​ξRt​​ξvt​​ξpt​​​⎠⎞​

即$\dot{\eta}_t$η˙​t​就是群$SE_2(3)$SE2​(3)在幺元处的切空间，其中

$\begin{aligned} [\dot{\xi}_{R_t}]_\times\approx\dot{\eta}_{R_t}&=\dot{\hat{R}}_tR_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}_tR_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{aligned}$[ξ˙​Rt​​]×​≈η˙​Rt​​​=R^˙t​RtT​+R^t​R˙tT​=R^t​[w~t​]×​RtT​+R^t​[w]×​RtT​=R^t​[w~t​−wt​]×​RtT​=R^t​[ntw​]×​RtT​=R^t​[ntw​]×​R^tT​R^t​RtT​=[R^t​ntw​]×​ηRt​​≈[R^t​ntw​]×​(I+[ξRt​​]×​)≈[R^t​ntw​]×​​

上面最后一步忽略了高阶小量$[\hat{R}_t n_t^w]_\times [\xi_{R_t}]_\times$[R^t​ntw​]×​[ξRt​​]×​。

同理有

$\begin{aligned} \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}_tR_t^T(R_ta_t+g) \\ &=\hat{R}_t(\tilde{a}_t-a_t) + g-[\hat{R}_t n_t^w]_\times v_t-\hat{R}_tR_t^Tg \\ &\approx \hat{R}_tn_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}_tn_t^a \end{aligned}$ξ˙​vt​​​=v^˙t​−η˙​Rt​​vt​−ηRt​​v˙t​=R^t​a~t​+g−[R^t​ntw​]×​vt​−R^t​RtT​(Rt​at​+g)=R^t​(a~t​−at​)+g−[R^t​ntw​]×​vt​−R^t​RtT​g≈R^t​nta​+g−[R^t​ntw​]×​vt​−(I+[ξRt​​]×​)g=[g]×​ξRt​​+([vt​]×​R^t​)ntw​+R^t​nta​​

以及

$\begin{aligned} \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}_tR_t^Tv_t \\ &=(\hat{v}_t-\hat{R}_tR_t^Tv_t)-[\hat{R}_t n_t^w]_\times p_t \\ &=\xi_{v_t}+([p_t]_\times \hat{R}_t) n_t^w \end{aligned}$ξ˙​pt​​​=p^​˙​t​−η˙​Rt​​pt​−ηRt​​p˙​t​=v^t​−[R^t​ntw​]×​pt​−R^t​RtT​vt​=(v^t​−R^t​RtT​vt​)−[R^t​ntw​]×​pt​=ξvt​​+([pt​]×​R^t​)ntw​​

综上有

$\begin{aligned} \frac{d}{dt}\Lambda \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix}&= \begin{pmatrix} [\hat{R}_t n_t^w]_\times & [g]_\times \xi_{R_t}+([v_t]_\times \hat{R}_t) n_t^w+\hat{R}_tn_t^a & \xi_{v_t}+([p_t]_\times \hat{R}_t) n_t^w \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix} \\ &=\Lambda\Bigg[ \begin{pmatrix} 0 & 0 & 0 \\ [g]_\times & 0 & 0 \\ 0 & I & 0 \end{pmatrix} \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix}+ \begin{pmatrix} \hat{R}_tn_t^w \\ ([v_t]_\times \hat{R}_t) n_t^w+\hat{R}_tn_t^a \\ ([p_t]_\times \hat{R}_t) n_t^w \end{pmatrix} \Bigg] \ \ \ \ \text{分离状态项和噪声项} \\ &=\Lambda\Bigg[ \begin{pmatrix} 0 & 0 & 0 \\ [g]_\times & 0 & 0 \\ 0 & I & 0 \end{pmatrix} \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix}+ \begin{pmatrix} \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{pmatrix} \begin{pmatrix} n_t^w \\ n_t^a \\ 0 \end{pmatrix} \Bigg] \end{aligned}$dtd​Λ⎝⎛​ξRt​​ξvt​​ξpt​​​⎠⎞​​=⎝⎛​[R^t​ntw​]×​00​[g]×​ξRt​​+([vt​]×​R^t​)ntw​+R^t​nta​00​ξvt​​+([pt​]×​R^t​)ntw​00​⎠⎞​=Λ[⎝⎛​0[g]×​0​00I​000​⎠⎞​⎝⎛​ξRt​​ξvt​​ξpt​​​⎠⎞​+⎝⎛​R^t​ntw​([vt​]×​R^t​)ntw​+R^t​nta​([pt​]×​R^t​)ntw​​⎠⎞​]    分离状态项和噪声项=Λ[⎝⎛​0[g]×​0​00I​000​⎠⎞​⎝⎛​ξRt​​ξvt​​ξpt​​​⎠⎞​+⎝⎛​R^t​[v^t​]×​R^t​[p^​t​]×​R^t​​0R^t​0​00R^t​​⎠⎞​⎝⎛​ntw​nta​0​⎠⎞​]​

令$\xi_t=(\xi_{R_t}^T, \xi_{v_t}^T, \xi_{p_t}^T)^T$ξt​=(ξRt​T​,ξvt​T​,ξpt​T​)T，以及$n_t=(n_t^w, n_t^a, 0)$nt​=(ntw​,nta​,0)，可得误差的线性系统方程

$\dot{\xi}_t=F_t\xi_t+Ad_{\hat{\chi}_t}n_t$ξ˙​t​=Ft​ξt​+Adχ^​t​​nt​

其中

$\begin{aligned} F_t&= \begin{pmatrix} 0 & 0 & 0 \\ [g]_\times & 0 & 0 \\ 0 & I & 0 \end{pmatrix} \\ Ad_{\hat{\chi}_t}&= \begin{pmatrix} \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{pmatrix} \end{aligned}$Ft​Adχ^​t​​​=⎝⎛​0[g]×​0​00I​000​⎠⎞​=⎝⎛​R^t​[v^t​]×​R^t​[p^​t​]×​R^t​​0R^t​0​00R^t​​⎠⎞​​

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

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

$P_t=\Phi_tP_{t-1}\Phi_t^T + Ad_{\hat{\chi}_t}Q_tAd_{\hat{\chi}_t}^T$Pt​=Φt​Pt−1​ΦtT​+Adχ^​t​​Qt​Adχ^​t​T​

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

状态更新

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

$\begin{aligned} 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\\ &\vdots \\ y_t^k &= R_t^T(m_k-p_t) + s_t^k \end{aligned}$yt1​yt2​ytk​​=RtT​(m1​−pt​)+st1​=RtT​(m2​−pt​)+st2​⋮=RtT​(mk​−pt​)+stk​​

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

$\begin{aligned} \begin{pmatrix} y_i \\ 0 \\ 1 \end{pmatrix}&= \begin{pmatrix} R_t^T & -R_t^Tv_t & -R_t^Tp_t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} m_i \\ 0 \\ 1 \end{pmatrix}+ \begin{pmatrix} s_t^i \\ 0 \\ 0 \end{pmatrix} \end{aligned}$⎝⎛​yi​01​⎠⎞​​=⎝⎛​RtT​00​−RtT​vt​10​−RtT​pt​01​⎠⎞​⎝⎛​mi​01​⎠⎞​+⎝⎛​sti​00​⎠⎞​​

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

$Y_t^i=\chi_t^{-1}M^i+S_t^i$Yti​=χt−1​Mi+Sti​

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

$Z_t^i=Y_t^i-\hat{\chi}_t^{-1}M^i$Zti​=Yti​−χ^​t−1​Mi

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

$Z_t^i=\hat{\chi}_tY_t^i-M^i$Zti​=χ^​t​Yti​−Mi

这样就可以得到

$\begin{aligned} 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}_tS_t^i \\ &=\eta_t M^i - M^i + \hat{\chi}_tS_t^i \end{aligned}$Zti​​=χ^​t​(χt−1​Mi+Sti​)−Mi=χ^​t​χt−1​Mi−Mi+χ^​t​Sti​=ηt​Mi−Mi+χ^​t​Sti​​

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

$z_t= \begin{pmatrix} \eta_t M^1 - M^1 + \hat{\chi}_tS_t^1 \\ \vdots \\ \eta_t M^k - M^k + \hat{\chi}_tS_t^k \end{pmatrix}$zt​=⎝⎜⎛​ηt​M1−M1+χ^​t​St1​⋮ηt​Mk−Mk+χ^​t​Stk​​⎠⎟⎞​

和EKF一样，现在需要求出观测误差与状态误差之间的线性关系。利用状态误差的一阶近似$\eta_t\approx I+\Lambda(\xi_t)$ηt​≈I+Λ(ξt​)，有

$\begin{aligned} z_t &\approx \begin{pmatrix} (I+\Lambda(\xi_t)) M^1 - M^1 + \hat{\chi}_tS_t^1 \\ \vdots \\ (I+\Lambda(\xi_t)) M^k - M^k + \hat{\chi}_tS_t^k \end{pmatrix} \\ &= \begin{pmatrix} \Lambda(\xi_t)M^1 + \hat{\chi}_tS_t^1 \\ \vdots \\ \Lambda(\xi_t)M^k + \hat{\chi}_tS_t^k \end{pmatrix} \\ \end{aligned}$zt​​≈⎝⎜⎛​(I+Λ(ξt​))M1−M1+χ^​t​St1​⋮(I+Λ(ξt​))Mk−Mk+χ^​t​Stk​​⎠⎟⎞​=⎝⎜⎛​Λ(ξt​)M1+χ^​t​St1​⋮Λ(ξt​)Mk+χ^​t​Stk​​⎠⎟⎞​​

上式计算后，每三行就有两行是全零，为了使计算和书写更紧凑，可以将$z_t$zt​中全是零的行去掉，得到$\tilde{z}_t$z~t​，即

$\begin{aligned} \tilde{z}_t&= \begin{pmatrix} [\xi_{R_t}]_\times m_1 + \xi_{p_t} + \hat{R}_t s_t^1 \\ \vdots \\ [\xi_{R_t}]_\times m_k + \xi_{p_t} + \hat{R}_t s_t^k \end{pmatrix}\\ &= \begin{pmatrix} -[m_1]_\times & 0 & 1 \\ \vdots \\ -[m_k]_\times & 0 & 1 \end{pmatrix} \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix} + \begin{pmatrix} \hat{R}_t s_t^1 \\ \vdots \\ \hat{R}_t s_t^k \end{pmatrix} \\ &=H_t\xi_t+s_t \end{aligned}$z~t​​=⎝⎜⎛​[ξRt​​]×​m1​+ξpt​​+R^t​st1​⋮[ξRt​​]×​mk​+ξpt​​+R^t​stk​​⎠⎟⎞​=⎝⎜⎛​−[m1​]×​⋮−[mk​]×​​00​11​⎠⎟⎞​⎝⎛​ξRt​​ξvt​​ξpt​​​⎠⎞​+⎝⎜⎛​R^t​st1​⋮R^t​stk​​⎠⎟⎞​=Ht​ξt​+st​​

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

$\hat{x}_t^+=\hat{x}_t+K_t[y_t-h(\hat{x}_t)]=\hat{x}_t+K_tz_t$x^t+​=x^t​+Kt​[yt​−h(x^t​)]=x^t​+Kt​zt​

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

$e_t^+=e_t+K_tz_t$et+​=et​+Kt​zt​

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

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

$\begin{aligned} \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{aligned}$ηt+​​=χ^​t+​χt−1​=χ^​t+​χ^​t−1​χ^​t​χt−1​=χ^​t+​χ^​t−1​ηt​​

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

$\hat{\chi}_t^+\hat{\chi}_t^{-1}=\text{Exp}(K_tz_t)$χ^​t+​χ^​t−1​=Exp(Kt​zt​)

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

$\eta_t^+=\text{Exp}(K_tz_t)\eta_t$ηt+​=Exp(Kt​zt​)ηt​

以及状态的更新方程

$\hat{\chi}_t^+=\text{Exp}(K_tz_t)\hat{\chi}_t$χ^​t+​=Exp(Kt​zt​)χ^​t​

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

$\begin{aligned} \eta_t^+ &= \text{Exp}(K_tz_t)\eta_t \end{aligned}$ηt+​​=Exp(Kt​zt​)ηt​​

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

$I+\Lambda(\xi_t^+)=[I + \Lambda(K_tz_t)][I+\Lambda(\xi_t)]$I+Λ(ξt+​)=[I+Λ(Kt​zt​)][I+Λ(ξt​)]

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

$\begin{aligned} \xi_t^+ &= \xi_t+K_t z_t \end{aligned}$ξt+​​=ξt​+Kt​zt​​

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

$\tilde{K}_t = P_tH_t^T(H_tP_tH_t^T+V_t)$K~t​=Pt​HtT​(Ht​Pt​HtT​+Vt​)

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

$\hat{\chi}_t^+=\text{Exp}(\tilde{K}_t\tilde{z}_t)\hat{\chi}_t$χ^​t+​=Exp(K~t​z~t​)χ^​t​

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