扩展KalmanKalmanKalman滤波的缺陷

存在正反馈

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

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

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

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,utet+nt\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,utet+nt

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

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

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

yt=h(xt)+sty_t=h(x_t)+s_tyt=h(xt)+st

其中sts_tst是观测噪声。设实际观测与估计观测的误差为zt=yt−y^tz_t=y_t-\hat{y}_tzt=yt−y^t，有

zt=yt−y^t=h(xt)−y^t+st≈h(x^t)+Hx^t(xt−x^t)−y^t+st=Hx^tet+st\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^tet+st

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

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

非一致性

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

不变扩展卡尔曼滤波(IEKF)(IEKF)(IEKF)

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

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

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

R˙t=Rt[w~t−ntw]×v˙t=Rt(a~t−nta)+gp˙t=vt\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˙tv˙tp˙t=Rt[w~t−ntw]×=Rt(a~t−nta)+g=vt

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

χt=(Rtvtpt010001)\chi_t= \begin{pmatrix} R_t & v_t & p_t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix}χt=⎝⎛Rt00vt10pt01⎠⎞

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

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

误差传递

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

ηt=χ^tχt−1=(R^tRtTv^t−R^tRtTvtp^t−R^tRtTpt010001)\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^tRtT00v^t−R^tRtTvt10p^t−R^tRtTpt01⎠⎞

令

R^tRtT=ηRtv^t−R^tRtTvt=ξvtp^t−R^tRtTpt=ξpt\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^tRtTv^t−R^tRtTvtp^t−R^tRtTpt=ηRt=ξvt=ξpt

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

ηt˙≈([ξ˙Rt]×ξ˙vtξ˙pt000000)=ddtΛ(ξRtξvtξpt)\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ξ˙vt00ξ˙pt00⎠⎞=dtdΛ⎝⎛ξRtξvtξpt⎠⎞

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

[ξ˙Rt]×≈η˙Rt=R^˙tRtT+R^tR˙tT=R^t[w~t]×RtT−R^t[w]×RtT=R^t[w~t−wt]×RtT=R^t[ntw]×RtT=R^t[ntw]×R^tTR^tRtT=[R^tntw]×ηRt≈[R^tntw]×(I+[ξRt]×)≈[R^tntw]×\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^˙tRtT+R^tR˙tT=R^t[w~t]×RtT−R^t[w]×RtT=R^t[w~t−wt]×RtT=R^t[ntw]×RtT=R^t[ntw]×R^tTR^tRtT=[R^tntw]×ηRt≈[R^tntw]×(I+[ξRt]×)≈[R^tntw]×

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

同理有

ξ˙vt=v^˙t−η˙Rtvt−ηRtv˙t=R^ta~t+g−[R^tntw]×vt−R^tRtT(Rtat+g)=R^t(a~t−at)+g−[R^tntw]×vt−R^tRtTg≈R^tnta+g−[R^tntw]×vt−(I+[ξRt]×)g=[g]×ξRt+([vt]×R^t)ntw+R^tnta\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−η˙Rtvt−ηRtv˙t=R^ta~t+g−[R^tntw]×vt−R^tRtT(Rtat+g)=R^t(a~t−at)+g−[R^tntw]×vt−R^tRtTg≈R^tnta+g−[R^tntw]×vt−(I+[ξRt]×)g=[g]×ξRt+([vt]×R^t)ntw+R^tnta

以及

ξ˙pt=p^˙t−η˙Rtpt−ηRtp˙t=v^t−[R^tntw]×pt−R^tRtTvt=(v^t−R^tRtTvt)−[R^tntw]×pt=ξvt+([pt]×R^t)ntw\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−η˙Rtpt−ηRtp˙t=v^t−[R^tntw]×pt−R^tRtTvt=(v^t−R^tRtTvt)−[R^tntw]×pt=ξvt+([pt]×R^t)ntw

综上有

ddtΛ(ξRtξvtξpt)=([R^tntw]×[g]×ξRt+([vt]×R^t)ntw+R^tntaξvt+([pt]×R^t)ntw000000)=Λ[(000[g]×000I0)(ξRtξvtξpt)+(R^tntw([vt]×R^t)ntw+R^tnta([pt]×R^t)ntw)]分离状态项和噪声项=Λ[(000[g]×000I0)(ξRtξvtξpt)+(R^t00[v^t]×R^tR^t0[p^t]×R^t0R^t)(ntwnta0)]\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^tntw]×00[g]×ξRt+([vt]×R^t)ntw+R^tnta00ξvt+([pt]×R^t)ntw00⎠⎞=Λ[⎝⎛0[g]×000I000⎠⎞⎝⎛ξRtξvtξpt⎠⎞+⎝⎛R^tntw([vt]×R^t)ntw+R^tnta([pt]×R^t)ntw⎠⎞] 分离状态项和噪声项=Λ[⎝⎛0[g]×000I000⎠⎞⎝⎛ξRtξvtξpt⎠⎞+⎝⎛R^t[v^t]×R^t[p^t]×R^t0R^t000R^t⎠⎞⎝⎛ntwnta0⎠⎞]

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

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

其中

Ft=(000[g]×000I0)Adχ^t=(R^t00[v^t]×R^tR^t0[p^t]×R^t0R^t)\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}FtAdχ^t=⎝⎛0[g]×000I000⎠⎞=⎝⎛R^t[v^t]×R^t[p^t]×R^t0R^t000R^t⎠⎞

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

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

Pt=ΦtPt−1ΦtT+Adχ^tQtAdχ^tTP_t=\Phi_tP_{t-1}\Phi_t^T + Ad_{\hat{\chi}_t}Q_tAd_{\hat{\chi}_t}^TPt=ΦtPt−1ΦtT+Adχ^tQtAdχ^tT

其中Φt\Phi_tΦt是通过FtF_{t}Ft积分后得到的状态转移矩阵，Qt=E(ntntT)Q_t=E(n_tn_t^T)Qt=E(ntntT)是系统噪声的协方差矩阵。由于系统矩阵是常量，积分可以变得非常简单，甚至直接写出解析式。

状态更新

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

yt1=RtT(m1−pt)+st1yt2=RtT(m2−pt)+st2⋮ytk=RtT(mk−pt)+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}yt1yt2ytk=RtT(m1−pt)+st1=RtT(m2−pt)+st2⋮=RtT(mk−pt)+stk

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

(yi01)=(RtT−RtTvt−RtTpt010001)(mi01)+(sti00)\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}⎝⎛yi01⎠⎞=⎝⎛RtT00−RtTvt10−RtTpt01⎠⎞⎝⎛mi01⎠⎞+⎝⎛sti00⎠⎞

注意等号右边第一项的矩阵是状态矩阵的逆，令Yti=(yti,0,1)T,Mi=(mi,0,1)T,Sti=(sti,0,0)TY_t^i=(y_t^i, 0, 1)^{T}, M^i=(m_i, 0, 1)^T, S_t^i=(s_t^i, 0, 0)^TYti=(yti,0,1)T,Mi=(mi,0,1)T,Sti=(sti,0,0)T有

Yti=χt−1Mi+StiY_t^i=\chi_t^{-1}M^i+S_t^iYti=χt−1Mi+Sti

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

Zti=Yti−χ^t−1MiZ_t^i=Y_t^i-\hat{\chi}_t^{-1}M^iZti=Yti−χ^t−1Mi

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

Zti=χ^tYti−MiZ_t^i=\hat{\chi}_tY_t^i-M^iZti=χ^tYti−Mi

这样就可以得到

Zti=χ^t(χt−1Mi+Sti)−Mi=χ^tχt−1Mi−Mi+χ^tSti=ηtMi−Mi+χ^tSti\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−1Mi+Sti)−Mi=χ^tχt−1Mi−Mi+χ^tSti=ηtMi−Mi+χ^tSti

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

(ηtM1−M1+χ^tSt1⋮ηtMk−Mk+χ^tStk)\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}⎝⎜⎛ηtM1−M1+χ^tSt1⋮ηtMk−Mk+χ^tStk⎠⎟⎞

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

zt≈((I+Λ(ξt))M1−M1+χ^tSt1⋮(I+Λ(ξt))Mk−Mk+χ^tStk)=(Λ(ξt)M1+χ^tSt1⋮Λ(ξt)Mk+χ^tStk)\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+χ^tSt1⋮(I+Λ(ξt))Mk−Mk+χ^tStk⎠⎟⎞=⎝⎜⎛Λ(ξt)M1+χ^tSt1⋮Λ(ξt)Mk+χ^tStk⎠⎟⎞

将Mk,Stk,χ^t,ξtM^k, S_t^k, \hat{\chi}_t,\xi_tMk,Stk,χ^t,ξt的具体表达式带入上式计算后，每三行就有两行是全零，为了使计算和书写更紧凑，可以将ztz_tzt中全是零的行去掉，得到z~t\tilde{z}_tz~t，即

z~t=([ξRt]×m1+ξpt+R^tst1⋮[ξRt]×mk+ξpt+R^tstk)=(−[m1]×03×3I3×3⋮−[mk]×03×3I3×3)(ξRtξvtξpt)+(R^tst1⋮R^tstk)=Htξt+st\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_{3\times3} & I_{3\times3} \\ \vdots \\ -[m_k]_\times & 0_{3\times3} & I_{3\times3} \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^tst1⋮[ξRt]×mk+ξpt+R^tstk⎠⎟⎞=⎝⎜⎛−[m1]×⋮−[mk]×03×303×3I3×3I3×3⎠⎟⎞⎝⎛ξRtξvtξpt⎠⎞+⎝⎜⎛R^tst1⋮R^tstk⎠⎟⎞=Htξt+st

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

x^t+=x^t+Kt[yt−h(x^t)]=x^t+Ktzt\hat{x}_t^+=\hat{x}_t+K_t[y_t-h(\hat{x}_t)]=\hat{x}_t+K_tz_tx^t+=x^t+Kt[yt−h(x^t)]=x^t+Ktzt

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

et+=et+Ktzte_t^+=e_t+K_tz_tet+=et+Ktzt

其中KtK_tKt是卡尔曼增益。

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

ηt+=χ^t+χt−1=χ^t+χ^t−1χ^tχt−1=χ^t+χ^t−1ηt\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

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

χ^t+χ^t−1=Exp(Ktzt)\hat{\chi}_t^+\hat{\chi}_t^{-1}=\text{Exp}(K_tz_t)χ^t+χ^t−1=Exp(Ktzt)

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

ηt+=Exp(Ktzt)ηt\eta_t^+=\text{Exp}(K_tz_t)\eta_tηt+=Exp(Ktzt)ηt

以及状态的更新方程

χ^t+=Exp(Ktzt)χ^t\hat{\chi}_t^+=\text{Exp}(K_tz_t)\hat{\chi}_tχ^t+=Exp(Ktzt)χ^t

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

ηt+=Exp(Ktzt)ηt\begin{aligned} \eta_t^+ &= \text{Exp}(K_tz_t)\eta_t \end{aligned}ηt+=Exp(Ktzt)ηt

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

I+Λ(ξt+)=[I+Λ(Ktzt)][I+Λ(ξt)]I+\Lambda(\xi_t^+)=[I + \Lambda(K_tz_t)][I+\Lambda(\xi_t)]I+Λ(ξt+)=[I+Λ(Ktzt)][I+Λ(ξt)]

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

ξt+=ξt+Ktzt\begin{aligned} \xi_t^+ &= \xi_t+K_t z_t \end{aligned}ξt+=ξt+Ktzt

上式与普通EKFEKFEKF中的误差更新方程et+=et+Ktzte_t^+=e_t+K_tz_tet+=et+Ktzt有相同的形式，所以增益KtK_{t}Kt的计算和普通的EKFEKFEKF完全相同。上式也可以用更紧凑的z~t\tilde{z}_tz~t代替，对应的更紧凑的增益记为K~t\tilde{K}_tK~t，则

K~t=PtHtT(HtPtHtT+Vt)−1\tilde{K}_t = P_tH_t^T(H_tP_tH_t^T+V_t)^{-1}K~t=PtHtT(HtPtHtT+Vt)−1

其中，PtP_{t}Pt是状态误差ξt\xi _{t}ξt的协方差矩阵，HtH_{t}Ht是之前推出的z~t\tilde{z}_tz~t与ξt\xi _{t}ξt之间的观测矩阵，Vt=E(ststT)V_t=E(s_ts_t^T)Vt=E(ststT)是观测误差的协方差矩阵（注意，sts_{t}st中的每一项噪声都被R^t\hat{R}_tR^t旋转过）。状态的更新自然就是

χ^t+=Exp(K~tz~t)χ^tPt+=(I−K~tHt)Pt\begin{aligned} \hat{\chi}_t^+&=\text{Exp}(\tilde{K}_t\tilde{z}_t)\hat{\chi}_t \\ P_t^+&=(I-\tilde{K}_tH_t)P_t \end{aligned}χ^t+Pt+=Exp(K~tz~t)χ^t=(I−K~tHt)Pt

不变扩展卡尔曼滤波（IEKF）相关推荐

【camera-lidar-radar】基于卡尔曼滤波和扩展卡尔曼滤波的相机、激光雷达、毫米波雷达多传感器后融合
[camera-lidar-radar]基于卡尔曼滤波和扩展卡尔曼滤波的相机.激光雷达.毫米波雷达多传感器后融合代码下载地址(C++ and Python):下载地址红点和蓝点分别表示radar和 ...
扩展卡尔曼滤波EKF与多传感器融合
Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本.在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准.本文将简要介绍EKF,并介绍其在无人驾 ...
概率机器人总结——(扩展)卡尔曼滤波先实践再推导
概率机器人总结--卡尔曼滤波先实践再推导概率机器人总结--(扩展)卡尔曼滤波先实践再推导 (1)卡尔曼.扩展卡尔曼.粒子滤波到底什么关系? (2)实践--扩展卡尔曼滤波 (3)推导--卡尔曼滤波 ( ...
机器人学习--扩展卡尔曼滤波算法用于机器人定位
论文链接:leonard_ieeetroa19.pdf-互联网文档类资源-CSDN下载https://download.csdn.net/download/GGY1102/61742571 基于EKF ...
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
所有滤波问题其实都是求感兴趣的状态的后验概率分布,只是由于针对特定条件的不同,可通过求解递推贝叶斯公式获得后验概率的解析解(KF.EKF.UKF),也可通过大数统计平均求期望的方法来获得后验概率(PF ...
卫星轨道的估计问题(Matlab)(二):扩展卡尔曼滤波(EKF)对新问题的尝试
前言在前面的问题中我们已经考虑到了用微分方程来描述卫星运动轨迹的方法: r¨=rθ˙2−GMr−2θ¨=−2r−1r˙θ˙\ddot r = r\dot \theta^2-GMr^{-2}\\\dd ...
使用扩展卡尔曼滤波（EKF）进行AHRS九轴姿态融合
AHRS九轴姿态融合 EKF滤波卡尔曼滤波在做九轴姿态融合的过程中,这里介绍一种融合算法,基于EKF的九轴姿态融合算法: 首先表明,该算法并非自己想到的,算法原理参考了这篇论文:Dale E. S ...
飞控之扩展卡尔曼滤波(附matlab和C代码)
目录原理讲解: 数据融合效果: matlab代码: 飞控代码: 联系作者原理讲解: 从四元数基础到扩展卡尔曼滤波数据融合效果: ...
无人驾驶汽车系统入门（二）——高级运动模型和扩展卡尔曼滤波
前言:上一篇文章的最后我们提到卡尔曼滤波存在着一个非常大的局限性--它仅能对线性的处理模型和测量模型进行精确的估计,在非线性的场景中并不能达到最优的估计效果.所以之前为了保证我们的处理模型是线性的,我 ...
扩展卡尔曼滤波的理解与对加入高斯噪声的正弦信号进行滤波实例
文章目录卡尔曼滤波拓展卡尔曼滤波正弦波滤波卡尔曼滤波卡尔曼滤波是一个线性状态估似算法,对高斯噪声有良好的滤波效果,也可做多传感器融合算法,如九轴.具体推导网络上,网络上很多.珠玉在前我就不过 ...

不变扩展卡尔曼滤波（IEKF）