KF、EKF、ESKF的区别与联系

目录

背景贝叶斯滤波系列

KF、EKF、ESKF的原理与区别贝叶斯滤波卡尔曼滤波（KF，Kalman Filter）扩展卡尔曼滤波（EKF，Extended Kalman Filter）Error-State卡尔曼滤波（ESKF）predict过程update过程

总结KF、EKF、ESKF的联系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(xk−1​,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(xk​∣y1:k​)​=p(y1:k​)p(yk​∣xk​,y1:k−1​)p(xk​,y1:k−1​)​=p(yk​∣y1:k−1​)p(yk​∣xk​,y1:k−1​)p(xk​∣y1:k−1​)​=∫p(yk​∣xk​)p(xk​∣y1:k−1​)dxk​p(yk​∣xk​)p(xk​∣y1:k−1​)​​ 以上推导过程假设了系统具有马尔可夫性，因此

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(yk​∣xk​,y1:k−1​)=p(yk​∣xk​)

p

(

x

k

∣

y

1

:

k

−

1

)

p(x_k|y_{1:k-1})

p(xk​∣y1:k−1​)：先验概率密度，通过上一次的系统状态结合系统方程计算得到。

p

(

y

k

∣

x

k

,

y

1

:

k

−

1

)

p(y_k|x_k,y_{1:k-1})

p(yk​∣xk​,y1:k−1​)：似然概率密度，由观测方程计算得到

p

(

x

k

∣

y

1

:

k

)

p(x_k|y_{1:k})

p(xk​∣y1: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(xk​∣y1:k−1​)​=∫(xk​,xk−1​∣y1:k−1​)dxk−1​=∫p(xk​∣xk−1​,y1:k−1​)p(xk−1​∣y1:k−1​)dxk−1​=∫p(xk​∣xk−1​)p(xk−1​∣y1:k−1​)dxk−1​​ 以上公式中包含积分运算，对于一般的非线性、非高斯系统，很难得到后验概率的解析解，因此，需要结合具体的假设完成其实现形式。

卡尔曼滤波（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​=Fk​xk−1​+Bk​uk​+wk​yk​=Hk​xk​+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(xk​∣y1:k−1​)=∫p(xk​∣xk−1​)p(xk−1​∣y1:k−1​)dxk−1​ 其中：

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(xk​∣xk−1​)∼N(Fk​xk−1​+Bk​uk​,Wk​)p(xk−1​∣y1:k−1​)∼N(μk−1​,Pk−1​) 代入积分得到（积分过程忽略）：

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(xk​∣y1:k−1​)∼N(μ^​k​,P^k​)μ^​k​=Fk​μk−1​+Bk​uk​P^k​=Fk​Pk−1​FkT​+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(xk​∣y1:k​)=∫p(yk​∣xk​)p(xk​∣y1:k−1​)dxk​p(yk​∣xk​)p(xk​∣y1:k−1​)​​ 其中：

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(yk​∣xk​)∼N(Hk​xk​,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(xk​∣y1:k−1​)∼N(μk​,Pk​)μk​=μ^​k​+K(yk​−Hk​μ^​k​)Pk​=P^k​−KHk​P^k​K=P^k​HkT​(Hk​P^k​HkT​+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​=xt−1​+(Tt​−Tt−1​)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​=xt−1​+(Tt​−Tt−1​)ut​p^​t​=pt−1​+wt​K=pt−1​+wt​+vt​pt−1​+wt​​xt​=x^t​+K(zt​−x^t​)pt​=p^​t​−Kp^​t​=pt−1​+wt​+vt​vt​(pt−1​+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(xk−1​∣y1:k−1​)∼N(μk−1​,Pk−1​)，于是系统方程和观测方程变为：

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(μk−1​,uk​,wk​)+dxk−1​df​(xk−1​−μk−1​)yk​=h(μ^​k​,vk​)+dxk​dh​(xk​−μ^​k​)Fk​=dxk−1​df​Hk​=dxk​dh​ 剩下的计算步骤与卡尔曼滤波一致。

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​+Fi​i(6) 这里

F

δ

F_{\delta}

Fδ​、

F

i

F_i

Fi​为

f

δ

f_{\delta}

fδ​对

δ

X

、

i

\delta X、i

δX、i的雅可比矩阵，实际上根据该文献的推导，在推导误差系统方程过程中，大量的高阶项已经被忽略，这里已经是一个线性模型，不需要额外的求导操作了。这就是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^k​P^k+1​=Fδ​Pˇk​FδT​+Fi​Qi​FiT​(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+1​HT(HP^k+1​HT+V)−1δXˇk+1​=K(Y−h(X^t,k+1​))Pˇk+1​=(I−KH)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}

H≜dXt​dh​∣X​dδXdXt​​∣X​(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、均值滤波：一般应用于滑动窗口滤波中，计算量小且能够起到一定平滑的作用，但当状态分布不均衡时会造成输出偏移。