文章同步更新于github pages,欢迎收藏关注。
扩展卡尔曼滤波的缺陷
存在正反馈
普通的扩展卡尔曼滤波(EKF),通过对动态方程的线性化,来估计状态与状态的协方差。比如有一个系统定义为
x ˙ t = f ( x t , u t ) + n t \begin{aligned} \dot{x}_{t}&=f(x_t, u_t) + n_t \end{aligned} x˙t=f(xt,ut)+nt
其中 x t x_t xt是系统状态, u t u_t ut是输入, n t n_t nt是系统噪声。假设某个时刻对状态的估计为 x ^ t \hat{x}_{t} x^t,并定义状态误差为 e t = x t − x ^ t e_{t}=x_{t}-\hat{x}_{t} et=xt−x^t,则根据EKF,对 f f f在 x ^ t \hat{x}_{t} x^t处线性化(一阶泰勒展开),并注意到 f ( x ^ t n , u t ) = x ^ ˙ t n + 1 f(\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 ( x t , u t ) − x ^ ˙ t + n t ≈ f ( x ^ t , u t ) + F x ^ t , u t ( x t − x ^ t ) − x ^ ˙ t + n t = F x ^ t , u t ( x t − x ^ t ) + n t = F x ^ t , u t e t + n t \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
上式是状态误差的线性传递方程,因此可以使用标准的卡尔曼滤波来估计协方差。
但是这里存在一个问题,上面误差传递方程中,系统矩阵 F F F依赖当前状态的估计量。在有噪声引入时,状态估计量是无法预测的,这就导致很难对上述系统方程做收敛性分析,实际上,任何EKF都没有收敛性保证。当状态估计值与真值差距较大时,直接导致依赖状态估计值的 F F F也有较大偏差,使用这样的 F F F继续传递误差后,又进一步放大误差,整个误差传递系统形成正反馈,最终导致滤波器发散。
实际上,在滤波器进行状态更新时也有类似问题。设系统的观测方程为
y t = h ( x t ) + s t y_t=h(x_t)+s_t yt=h(xt)+st
其中 s t s_t st是观测噪声。设实际观测与估计观测的误差为 z t = y t − y ^ t z_t=y_t-\hat{y}_t zt=yt−y^t,有
z t = y t − y ^ t = h ( x t ) − y ^ t + s t ≈ h ( x ^ t ) + H x ^ t ( x t − x ^ t ) − y ^ t + s t = H x ^ t e t + s 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^tet+st
上式即观测误差与状态误差之间的线性近似关系。同样的, H x ^ t 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 F与状态估计值的独立。在EKF中,我们的状态误差直接定义为两个状态的差,即 e t = x t − x ^ t e_{t}=x_{t}-\hat{x}_{t} et=xt−x^t,这太过于粗暴了,我们面对的是非线性系统,误差怎么都不应该是线性的形式。
现在我们以一个简单的问题为例,说明InEKF的用法。假设我们有一架无人机,上面装了IMU以及双目相机等传感器,我们打算对无人机的位姿进行估计。假设IMU的bias为零(之后会讨论有bias的情况),则IMU的系统方程为
R ˙ t = R t [ w ~ t − n t w ] × v ˙ t = R t ( a ~ t − n t a ) + g p ˙ t = v t \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}_t w~t,a~t是IMU的测量值, n t w , n t a n_t^w,n_t^a ntw,nta分别是gyro和acc的噪声, g g g是重力设为已知,待估计的量就是 R , v , p R,v,p R,v,p,根据上一章李群的内容,我们发现这三个量刚好能构成一个 S E 2 ( 3 ) SE_2(3) SE2(3)群,于是我们将InEKF的状态量写为
χ t = ( R t v t p t 0 1 0 0 0 1 ) \chi_t= \begin{pmatrix} R_t & v_t & p_t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix} χt=⎝⎛Rt00vt10pt01⎠⎞
这里已经和EKF有很大不同了,一般EKF的状态量是个矢量,而InEKF的状态量是一个矩阵,而且构成群。既然是群,其误差也应该定义在群上,自然是不能再用减法了,设状态估计值为 χ ^ 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),因为对 χ t , χ t ^ \chi_t, \hat{\chi_t} χt,χt^都左乘一个相同的群元素后,误差不变。同理,第二种为右不变误差(right-invariant)。具体使用哪种误差,要看该误差能否实现状态传递矩阵与状态估计值的独立。在很多SLAM问题中,右不变误差可以满足要求,因此下文都使用右不变误差。
误差传递
仿照EKF,现在推导这个误差的系统方程。
η t = χ ^ t χ t − 1 = ( R ^ t R t T v ^ t − R ^ t R t T v t p ^ t − R ^ t R t T p t 0 1 0 0 0 1 ) \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 ^ 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^tRtTv^t−R^tR