KF、EKF、ESKF的区别与联系

背景

滤波:去除噪声还原真实数据的一种数据处理方法,被广泛应用于对信号准确度有需求的众多领域,例如军事、航天、通信等等。
常用滤波方法:以低通滤波、带通滤波、高通滤波为代表的按照频率滤波的方式;以卡尔曼滤波、粒子滤波为代表的贝叶斯滤波系列。此外,常用的还有滑动窗口滤波、均值滤波和中值滤波等方法。

在这里插入图片描述

贝叶斯滤波系列

贝叶斯滤波:基于贝叶斯公式推导得到的一种滤波理论框架,其本身不是一种具体的实现方法,基于其理论产生了一系列滤波方法,例如卡尔曼滤波系列和粒子滤波。
卡尔曼滤波:一种针对线性系统、将噪声假设为高斯噪声的贝叶斯滤波具体实现形式,具有实现简单、效率高等优点。
贝叶斯公式:
贝叶斯公式
卡尔曼滤波的五条公式:
在这里插入图片描述
在这里插入图片描述

KF、EKF、ESKF的原理与区别

贝叶斯滤波

首先确定存在系统方程:
x k = f ( x k − 1 , u k , w k ) x_k= f(x_{k-1},u_k,w_k) xk=f(xk1,uk,wk)
以及观测方程:
y k = h ( x k , v k ) y_k=h(x_k,v_k) yk=h(xk,vk)
其中,x表示系统状态,y表示观测量,w表示系统传递噪声,v表示观测误差。
根据贝叶斯公式:
在这里插入图片描述
在这里插入图片描述

结合已有系统得到贝叶斯公式如下:
p ( x k ∣ y 1 : k ) = p ( y k ∣ x k , y 1 : k − 1 ) p ( x k , y 1 : k − 1 ) p ( y 1 : k ) = p ( y k ∣ x k , y 1 : k − 1 ) p ( x k ∣ y 1 : k − 1 ) p ( y k ∣ y 1 : k − 1 ) = p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) ∫ p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) d x k \begin{aligned} p(x_k|y_{1:k})&=\cfrac{p(y_k|x_k,y_{1:k-1})p(x_k,y_{1:k-1})}{p(y_{1:k})}\\ &=\cfrac{p(y_k|x_k,y_{1:k-1})p(x_k|y_{1:k-1})}{p(y_k|y_{1:k-1})}\\ &=\cfrac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})dx_k} \end{aligned} p(xky1:k)=p(y1:k)p(ykxk,y1:k1)p(xk,y1:k1)=p(yky1:k1)p(ykxk,y1:k1)p(xky1:k1)=p(ykxk)p(xky1:k1)dxkp(ykxk)p(xky1:k1)
以上推导过程假设了系统具有马尔可夫性,因此 p ( y k ∣ x k , y 1 : k − 1 ) = p ( y k ∣ x k ) p(y_k|x_k,y_{1:k-1})=p(y_k|x_k) p(ykxk,y1:k1)=p(ykxk)
p ( x k ∣ y 1 : k − 1 ) p(x_k|y_{1:k-1}) p(xky1:k1)先验概率密度,通过上一次的系统状态结合系统方程计算得到。
p ( y k ∣ x k , y 1 : k − 1 ) p(y_k|x_k,y_{1:k-1}) p(ykxk,y1:k1)似然概率密度,由观测方程计算得到
p ( x k ∣ y 1 : k ) p(x_k|y_{1:k}) p(xky1:k)后验概率密度,即我们所需要得到的结果
其中先验概率密度可以进一步展开为:
p ( x k ∣ y 1 : k − 1 ) = ∫ ( x k , x k − 1 ∣ y 1 : k − 1 ) d x k − 1 = ∫ p ( x k ∣ x k − 1 , y 1 : k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1 = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1 \begin{aligned} p(x_k|y_{1:k-1})&=\int (x_k,x_{k-1}|y_{1:k-1})dx_{k-1}\\ &=\int p(x_k|x_{k-1},y_{1:k-1})p(x_{k-1}|y_{1:k-1})dx_{k-1}\\ &=\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})dx_{k-1} \end{aligned} p(xky1:k1)=(xk,xk1y1:k1)dxk1=p(xkxk1,y1:k1)p(xk1y1:k1)dxk1=p(xkxk1)p(xk1y1:k1)dxk1
以上公式中包含积分运算,对于一般的非线性、非高斯系统,很难得到后验概率的解析解,因此,需要结合具体的假设完成其实现形式。

卡尔曼滤波(KF,Kalman Filter)

基础假设:
在这里插入图片描述

基于基础假设后,线性化的系统方程和观测方程具化为:
x k = F k x k − 1 + B k u k + w k y k = H k x k + v k x_{k}=F_kx_{k-1}+B_ku_{k}+w_k \\[3mm] y_k=H_kx_k+v_k xk=Fkxk1+Bkuk+wkyk=Hkxk+vk
先验概率密度具化为:
p ( x k ∣ y 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1 p(x_k|y_{1:k-1})=\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})dx_{k-1} p(xky1:k1)=p(xkxk1)p(xk1y1:k1)dxk1
其中:
p ( x k ∣ x k − 1 ) ∼ N ( F k x k − 1 + B k u k , W k ) p ( x k − 1 ∣ y 1 : k − 1 ) ∼ N ( μ k − 1 , P k − 1 ) p(x_k|x_{k-1})\sim N(F_kx_{k-1}+B_ku_{k},W_k)\\[3mm] p(x_{k-1}|y_{1:k-1})\sim N(\mu_{k-1},P_{k-1})\\[3mm] p(xkxk1)N(Fkxk1+Bkuk,Wk)p(xk1y1:k1)N(μk1,Pk1)
代入积分得到(积分过程忽略):
p ( x k ∣ y 1 : k − 1 ) ∼ N ( μ ^ k , P ^ k ) μ ^ k = F k μ k − 1 + B k u k P ^ k = F k P k − 1 F k T + W k p(x_k|y_{1:k-1})\sim N(\hat \mu_k, \hat P_k)\\[3mm] \hat \mu _k=F_k\mu_{k-1}+B_ku_k\\[3mm] \hat P_k=F_kP_{k-1}F_k^T+W_k p(xky1:k1)N(μ^k,P^k)μ^k=Fkμk1+BkukP^k=FkPk1FkT+Wk
于是就得到了先验概率分布。后验概率密度为:
p ( x k ∣ y 1 : k ) = p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) ∫ p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) d x k \begin{aligned} p(x_k|y_{1:k})=\cfrac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})dx_k} \end{aligned} p(xky1:k)=p(ykxk)p(xky1:k1)dxkp(ykxk)p(xky1:k1)
其中:
p ( y k ∣ x k ) ∼ N ( H k x k , V k ) p(y_k|x_{k})\sim N(H_kx_{k},V_k)\\[3mm] p(ykxk)N(Hkxk,Vk)
代入积分计算得到(积分过程忽略):
p ( x k ∣ y 1 : k − 1 ) ∼ N ( μ k , P k ) μ k = μ ^ k + K ( y k − H k μ ^ k ) P k = P ^ k − K H k P ^ k K = P ^ k H k T ( H k P ^ k H k T + V t ) − 1 p(x_k|y_{1:k-1})\sim N(\mu_k, P_k)\\[3mm] \mu _k=\hat \mu_k+K(y_k-H_k\hat \mu_k)\\[3mm] P_k=\hat P_{k}-KH_k\hat P_{k}\\[3mm] K=\hat P_kH_k^T(H_k\hat P_kH_k^T+V_t)^{-1} p(xky1:k1)N(μk,Pk)μk=μ^k+K(ykHkμ^k)Pk=P^kKHkP^kK=P^kHkT(HkP^kHkT+Vt)1
于是就得到了卡尔曼滤波的五个主要公式。

一个小例子:
有一辆小车,它从一个起点出发,你在之后需要每时每刻知道它的位置,小车上装有一个GPS可以对位置进行测量,但是GPS的测量不可能完全准确,它是有误差的,此时如何才能获取更精准的小车位置呢(如下图所示)?
在这里插入图片描述
首先确定系统方程:
x t = x t − 1 + ( T t − T t − 1 ) u t + w t (3) x_{t}=x_{t-1}+(T_t-T_{t-1})u_{t}+w_t \tag{3} xt=xt1+(TtTt1)ut+wt(3)
和观测方程
z t = x t + v t (1) z_t=x_t+v_t \tag{1} zt=xt+vt(1)
式中 x t x_t xt为t时刻小车真实位置,z为GPS测量值,T为时间,u为速度,w为传递噪声,v为观测噪声,符合标准正态分布,它反应的是测量与实际真值的误差,可以预先测量得到。直接套用KF的五个公式如下:

x ^ t = x t − 1 + ( T t − T t − 1 ) u t p ^ t = p t − 1 + w t K = p t − 1 + w t p t − 1 + w t + v t x t = x ^ t + K ( z t − x ^ t ) p t = p ^ t − K p ^ t = v t ( p t − 1 + w t ) p t − 1 + w t + v t \hat x_t=x_{t-1}+(T_t-T_{t-1})u_t\\[3mm] \hat p_t=p_{t-1}+w_t\\[3mm] K=\cfrac{p_{t-1}+w_t}{p_{t-1}+w_t+v_t}\\[3mm] x_t=\hat x_t+K(z_t-\hat x_t)\\[3mm] p_t=\hat p_t-K\hat p_t=\cfrac{v_t(p_{t-1}+w_t)}{p_{t-1}+w_t+v_t} x^t=xt1+(TtTt1)utp^t=pt1+wtK=pt1+wt+vtpt1+wtxt=x^t+K(ztx^t)pt=p^tKp^t=pt1+wt+vtvt(pt1+wt)
由此就得到t时刻的系统状态。

扩展卡尔曼滤波(EKF,Extended Kalman Filter)

卡尔曼滤波基于系统是线性系统的这一个假设,对其在实际的应用产生了很大的局限,因此扩展卡尔曼滤波推翻了这一假设,并提出了新的假设:

假设系统是连续变化的,即系统在一个迭代周期内变化很小

基于以上假设,可以将非线性方程在局部线性化,对其进行一阶泰勒展开,由于 p ( x k − 1 ∣ y 1 : k − 1 ) ∼ N ( μ k − 1 , P k − 1 ) p(x_{k-1}|y_{1:k-1})\sim N(\mu_{k-1},P_{k-1}) p(xk1y1:k1)N(μk1,Pk1),于是系统方程和观测方程变为:
x k = f ( μ k − 1 , u k , w k ) + d f d x k − 1 ( x k − 1 − μ k − 1 ) y k = h ( μ ^ k , v k ) + d h d x k ( x k − μ ^ k ) F k = d f d x k − 1 H k = d h d x k x_k=f(\mu_{k-1},u_k,w_k)+\cfrac{df}{dx_{k-1}}(x_{k-1}-\mu_{k-1})\\[3mm] y_k=h(\hat \mu_{k},v_k)+\cfrac{dh}{dx_{k}}(x_{k}-\hat \mu_{k})\\[3mm] F_k=\cfrac{df}{dx_{k-1}}\\[3mm] H_k=\cfrac{dh}{dx_{k}} xk=f(μk1,uk,wk)+dxk1df(xk1μk1)yk=h(μ^k,vk)+dxkdh(xkμ^k)Fk=dxk1dfHk=dxkdh
剩下的计算步骤与卡尔曼滤波一致。

Error-State卡尔曼滤波(ESKF)

本部分主要参考Quaternion kinematics for the error-state Kalman filter,下文提到文献均指代这个。由于ESKF整个推导过程极为繁琐,因此本文旨在尽可能简化的抽取梗概,从大体上理解ESKF,再结合参考文献中的细节,即可了解全貌。

直接法滤波与间接法滤波
直接法滤波:模型系统方程直接描述系统状态,不存在转化过程。一般使用的KF与EKF都属于直接法滤波。
间接法滤波:模型系统方程描述系统误差,需要通过转换得到系统状态。ESKF(Error-State Kalman Filter)是一种典型的间接法滤波,其预测和更新过程都是针对系统的误差状态,再将修正后误差状态修正系统状态。
流程如下:
在这里插入图片描述ESKF的核心在于将状态分解为两个部分组成:
X t = X + δ X (1) X_t=X+\delta X\tag1 Xt=X+δX(1)
其中 X t X_t Xt为系统状态真值, X X X为Nominal state, δ X \delta X δX为Error state,以下所有公式中均符合这个定义,结合下图理解可能更直观一点:
在这里插入图片描述

predict过程

根据IMU中值积分模型,可直接得到X_t 的一阶导数:
X ˙ t = U t ( X t , u m , i ) (2) \dot X_t=U_t(X_t,u_m,i)\tag2 X˙t=Ut(Xt,um,i)(2)
u m , i u_m,i um,i 为IMU的测量值和各类噪声,下文不重复标注了,上式对应文献中式235。
提取所有状态量主成分,构建一个Nominal state的动力学模型:
X ˙ = U ( X , u m ) (3) \dot X=U(X,u_m)\tag3 X˙=U(X,um)(3)
这个模型是自行构建的,理论上是可以根据需要调整的,注意观测,这个模型是完全不受噪声分量影响的,因为噪声分量的影响都放到了Error-state的动力学模型中。上式对应文献中式237。
有(1)(2)(3)就可以推导得到Error-state的动力学模型了,方法是对(1)进行求导,将(2)(3)带入进去求解即可,其中对于速度分量和旋转分量的推导有一些麻烦,因为设计对旋转量的求导,其他都好推。此时得到:
δ X ˙ = U δ ( X , δ x , u m , i ) (4) \dot{\delta X}=U_{\delta}(X,\delta x,u_m,i)\tag4 δX˙=Uδ(X,δx,um,i)(4)
上式对应文献中式261。
对(3)(4)积分可得到离散时间下的系统递推方程为:
X k + 1 = f ( X k , u m ) δ X k + 1 = f δ ( X k , δ X k , u m , i ) (5) X_{k+1}=f(X_{k},u_m)\\[3mm] \delta X_{k+1}=f_{\delta}(X_k,\delta X_{k},u_m,i)\tag5 Xk+1=f(Xk,um)δXk+1=fδ(Xk,δXk,um,i)(5)
积分过程参考文献4.6,上式对应文献中260与261。有了系统递推方程后,剩下的部分就跟EKF基本一模一样了,将Error-state方程线性化为:
δ X x + 1 = F δ ( X , u m ) δ X k + F i i (6) \delta X_{x+1}=F_{\delta}(X,u_m)\delta X_k+F_ii\tag 6 δXx+1=Fδ(X,um)δXk+Fii(6)
这里 F δ F_{\delta} Fδ F i F_i Fi f δ f_{\delta} fδ δ X 、 i \delta X、i δXi的雅可比矩阵,实际上根据该文献的推导,在推导误差系统方程过程中,大量的高阶项已经被忽略,这里已经是一个线性模型,不需要额外的求导操作了。这就是predict部分最核心的公式了,有了这个线性模型,predict部分就退化为一般针对Error-state的EKF,套用EKF的predict公式即可:
δ X ^ x + 1 = F δ ( X , u m ) δ X ^ k P ^ k + 1 = F δ P ˇ k F δ T + F i Q i F i T (7) \delta \hat X_{x+1}=F_{\delta}(X,u_m)\delta \hat X_k\\[3mm] \hat P_{k+1}=F_{\delta}\check P_kF_{\delta}^T+F_iQ_iF_i^T \tag 7 δX^x+1=Fδ(X,um)δX^kP^k+1=FδPˇkFδT+FiQiFiT(7)

update过程

假设观测方程为:
Y = h ( X t ) + v (8) Y=h(X_t)+v\tag 8 Y=h(Xt)+v(8)
套用EKF的update公式如下:
K = P ^ k + 1 H T ( H P ^ k + 1 H T + V ) − 1 δ X ˇ k + 1 = K ( Y − h ( X ^ t , k + 1 ) ) P ˇ k + 1 = ( I − K H ) P ^ k + 1 (9) K=\hat P_{k+1}H^T(H\hat P_{k+1}H^T+V)^{-1}\\[3mm] \delta \check X_{k+1}=K(Y-h(\hat X_{t,k+1}))\\[3mm] \check P_{k+1}=(I-KH)\hat P_{k+1} \tag 9 K=P^k+1HT(HP^k+1HT+V)1δXˇk+1=K(Yh(X^t,k+1))Pˇk+1=(IKH)P^k+1(9)
整个过程其实跟EKF完全一样,ESKF与EKF的唯一不同是,式子中的H矩阵是h相对于Error-state的雅可比矩阵。换一种更容易理解的方式,因为 X t = X ⊕ δ X X_t=X\oplus\delta X Xt=XδX ,Nominal state已知, h ( X t ) h(X_t) h(Xt)自然就退化为针对Error-state的方程,结合(7)就是一个完全的EKF了。
剩下唯一一个问题就是求解H矩阵,这里直接采用链式法则求解即可:
H ≜ d h d X t ∣ X d X t d δ X ∣ X (10) H\triangleq \frac{dh}{dX_t}_{|X}\frac{dX_t}{d\delta X}_{|X}\tag{10} HdXtdhXdδXdXtX(10)
具体的计算过程参考该文献吧,这里不展开了。

为什么使用ESKF?

1、Error-State的值一般趋近于0,可以避免一些一阶部分可能出现的奇点、应用于惯导系统时的万向锁问题等,提供了在所有时间段内的线性有效性保证。

2、Error-State的值一般很小,可以保证在泰勒展开式中的二次部分忽略不计,使得雅可比矩阵的计算非常快速且简单,有些系统中的雅可比矩阵甚至可以被认为是一个常数或者状态幅值。

3、Error-State动力系统通常比较缓慢,因为所有较大的分量被集成在了nominal-state当中,这就意味着我们可以使卡尔曼滤波的更新过程频率低于预测过程,如下所示。

在这里插入图片描述
.

总结

KF、EKF、ESKF的联系

1、本质上都属于贝叶斯滤波,是贝叶斯滤波的具化形式之一,遵循贝叶斯滤波基础理论。
2、实现流程和计算方法大体一致,只在细节上由差别。
3、由于都基于马尔可夫性假设,都存在无法考虑都历史状态从而可能使稳定性下降的问题。

KF、EKF、ESKF的区别

1、KF于EKF是直接法滤波的代表,直接处理系统状态;ESKF是一种间接法滤波,处理系统误差状态。
2、EKF可广泛应用于非线性系统中,ESKF目前主要在惯导系统应用较多,KF只能应用于线性系统。
3、EKF由于每次迭代都需要计算雅可比矩阵,计算复杂度最高,ESKF次之,KF计算最快,对于现代计算机而言计算消耗很小。

与其他滤波方法的区别

1、粒子滤波:本质上也是贝叶斯滤波的一种,相比较于KF系列,其应用范围更广,不基于非线性系统和高斯噪声的假设,直接用采样的方式逼近系统状态真值,缺点是需要样本越多,精度越高,但计算量也越大。
2、滑动窗口滤波:相比较于KF系列,会考虑一定时间内的历史系统状态,对于局部的系统状态跳变有更好的适应性,但会造成输出延迟等问题。
3、均值滤波:一般应用于滑动窗口滤波中,计算量小且能够起到一定平滑的作用,但当状态分布不均衡时会造成输出偏移。

  • 33
    点赞
  • 175
    收藏
    觉得还不错? 一键收藏
  • 7
    评论
KF(卡尔曼滤波)和EKF(扩展卡尔曼滤波)是常用的状态估计算法,都是基于贝叶斯滤波的思想。它们在解决传感器数据融合、机器人定位和导航等问题上有着重要的应用。 KF适用于线性的系统模型和高斯分布的噪声,通过递推的方式将信息从过去的时刻传播到当前时刻,同时利用测量数据对估计的状态进行修正,从而得到精确的估计结果。但KF的最大局限性在于缺乏对非线性系统模型和非高斯分布噪声的处理能力。 EKF是将KF推广到非线性系统的滤波方法,通过在KF的线性化过程中引入扩展项,使得对非线性系统的状态进行有效估计。其中,EKF通过在每一次迭代中对系统模型进行线性化来逼近非线性函数,然后使用KF来进行滤波。这种迭代的处理方法,在一定程度上解决了非线性系统模型的估计问题,使得EKF具有了更广泛的应用领域。 在Matlab中,KFEKF都有相应的函数实现。对于线性系统而言,KF函数可以直接调用进行滤波操作,而对于非线性系统,可以通过EKF函数进行滤波。这些函数提供了代码方便调用,在实际应用中可以方便地进行状态估计的实现。 综上所述,KFEKF在状态估计中有着重要的应用,KF适用于线性系统模型和高斯分布的噪声,而EKF则通过线性化迭代的方式对非线性系统进行估计。在Matlab中,KFEKF都有相应的函数实现,可以方便地进行状态估计的实现。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 7
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值