vkt

详细总结卡尔曼滤波原理+具体案例分析

详细总结卡尔曼滤波原理+具体案例分析

1状态观测器

在⼯程上，部分状态参数测量的成本过⾼，或是⽤现有仪器⽆法测得，此时需要引⼊状态观测器。状态观测器以原系统输

⼊、输出为输⼊，输出估计的状态变量。

原系统状态描述：

{xk=Axk−1+Buk−1yk=Cxk

状态观测器状态描述：

{x~k=Ax~k−1+Buk−1+f[yk−1−y~k−1]y~k=Cx~k

定义误差向量，则两系统状态⽅程相减得

其通解为矩阵指数函数，所以当特征值⼩于0时，误差向量各分量均趋于0，即状态观测器能直接估计原系统状

态。

本质上，状态观测器是针对状态空间⽅程描述的确定系统，从错误的系统状态估计值不断收敛到正确的系统状态估计值的数

学模型。

2状态滤波器

实际的系统往往是随机过程，状态描述为：

{xk=Axk−1+Buk−1+wk−1yk=Cxk+vk

其中称为过程噪声，主要由系统运⾏过程中的外⼒导致，如旋翼飞⾏器运动过程中受到的阵风；称为测量噪声，主要

由系统测量过程中测量仪器误差、精度不⾜导致。

本质上，状态滤波器是针对随机状态空间⽅程描述的随机系统，从不确定观测中提取信息，得到系统状态最优估计的数学模

型。当与为⾼斯⽩噪声且相互独⽴时，状态滤波器为卡尔曼滤波器。

状态滤波器的核⼼是通过贝叶斯原理不断调整滤波器增益矩阵，以减⼩随机⼲扰，使估计的系统状态趋近于真实状态；⽽状

态观测器的核⼼是通过极点配置确定观测器增益矩阵，使估计的状态跟踪当前系统的状态。

⼀般地，状态滤波器性能优于状态观测器，但在实际系统中，若随机噪声⾮⾼斯噪声，或有若⼲⾮线性环节，按常规噪声模

型建⽴的状态滤波器可能发散，此时则更需要状态观测器的稳定性。

3卡尔曼滤波器

曾提到贝叶斯⽅法的思考模式：

e=kx−kx

~

k

系统误差传递⽅程e=

k

A−fCe()k−1

A−fCek

wkvk

wkvk

参数先验信息πθ+()样本观测数据X=后验分布Pθ∣X()

即在得到新的样本信息之前，⼈们对模型的认知是先验分布，在得到新的样本信息后，⼈们对模型的认知为后验分

布。

由此引出卡尔曼滤波器。考虑⼀个随机系统：

{xk=Axk−1+Buk−1+wk−1zk=Cxk+vk

其中、，且与相互独⽴。

忽略噪声可建⽴该系统的先验数学模型(噪声仅满⾜随机分布，⽆法建模)：

}状态预测⽅程{x^k−=Ax^k−1+Buk−1z^k−=Cx^k

基于上述系统，从贝叶斯⽅法引出卡尔曼滤波器状态更新⽅程：

其中是后验状态估计，即经过⼀次修正后，当前系统状态的最优估计值；为系统状态的先验估计；为当前状态的测

量样本值；为当前状态的先验测量值；代表实际系统与估计系统间随机误差的影响；权衡先验模型与实测数

据间对后验分布的关系，称为卡尔曼增益，显然，越⼩表明实际与预测越接近，则后验分布越趋近于先验分布，

越⼤表明预测越不可信，需要通过实测来⼤幅修正先验分布。

下⾯要确定使后验模型的修正效果最佳，即越接近真实值。定义误差向量

{后验误差向量ek=xk−x^k先验误差向量ek−​=xk−x^k−​

定义后验误差协⽅差矩阵为:

right]Pk=E(ekekT)=⎣⎢⎢⎢⎡​σe12σe2σe1⋮σenσe1σe1σe2σe22⋮σenσe2……⋱…σe1σenσe2σen⋮σen2⎦⎥⎥⎥⎤​

同理也有先验误差协⽅差矩阵。根据最⼩⽅差估计原理，设损失函数为，即要求

将展开为：

上⾯运⽤了状态更新⽅程与状态预测⽅程，考虑到与相互独⽴，则进⼀步：

πθ()X

Pθ∣X()

wN0,Qk()vN0,Rk()wkvk

状态更新⽅程:=+Kz−x^kx^k

−

k(kz^k

−

)

x^kx^k

−

zk

z^k

−

z−kz^k

−

Kk

z−kz^k

−

z−kz^k

−

Kkx^kxk

Pk

−

trP(k)

K=kargmintrP[(k)]

Pk

ek

−

vk

令，即

将代⼊后验误差协⽅差矩阵表达式，即得

要更新，则只需要确定先验误差协⽅差矩阵。同样地，将展开为

考虑到与相互独⽴，进⼀步得到

⾄此，得到卡尔曼滤波的五⼤基本公式，其中核⼼⽅程为基于最⼩⽅差估计的卡尔曼增益调整⽅程，具体⼯作流程如图所

⽰。

P=kI−KCEeeI−KC+(k)(k

−

k

−

T)(k)T

KEvvKk(kk

T

)k

T

=I−KCPI−KC+(k)k

−

(k)T

KRKkk

T

=∂Kk

∂trP(k)

0

卡尔曼增益调整⽅程K=k

CPC+Rk

−

T

PCk

−

T

Kk

协⽅差更新⽅程P=I−KHPk(k)k

−

KkPk

−

Pk

−

P=k

−

Eee(k

−

k

−T

)

=Ex−x−[(kx^k

−

)(kx^k

−

)T]

=EAe+wAe+w[(k−1k−1)(k−1k−1)T]

=AEeeA+(k−1k−1

T)TEww(k−1k−1

T)

ekwk

协⽅差预测⽅程P=APA+Qk

−

k−1

T

4具体案例：船舶GPS定位

有⼀船舶出港沿某直线⽅向航⾏，辅助北⽃卫星进⾏定位和测速。假设

①船舶加速度=机动加速度+随机加速度，其中符合⾼斯分布；

②GPS观测噪声符合⾼斯分布。

要求⽤卡尔曼滤波器估计真实运动轨迹。

解决⽅案

⾸先建⽴随机系统的真实模型。以码头出发点为原点，采样周期(雷达扫描周期)为，⽤表⽰船舶在采样时刻处的真

实位置，⽤表⽰在时刻处的GPS定位观测值，则真实系统输出⽅程为。

记在时刻处船舶速，加速度为，由匀加速公式有：

其中。

定义在采样时刻系统状态为船舶的位置和速度，即

right]^Tx(k)=[s(k)s˙(k)]T可得到船舶运动的状态空间模型

⎩⎪⎪⎨⎪⎪⎧​[s(k+1)s˙(k+1)]=[10T1][s(k)s˙(k)]+[0.5TT]u(k)+[0.5TT]w(k)z(k)=[10][s(k)s˙(k)]+v(k)

at()ut()wt()wt()

vt()

Ts(k)kT

zk()kTz(k)=s(k)+v(k)

kTks˙()ak()

sk+1=()sk+()kT+s˙()akT

2

1

()2

⇒k+1=s˙()k+s˙()Tak()

a(k)=u(k)+w(k)

kTxk()

在不考虑机动⽬标的动⼒因素即时，将匀速直线运动的船舶系统扩展到四维，状态包含⽔平和纵向的位置和速

度，则系统⽅程可化为

right]=left[right]left[right]+left[right]left[right]z(k)=left[right]left[right]+left[right]

⎣⎢⎢⎡​x(k)x˙(k)y(k)y˙(k)⎦⎥⎥⎤​=⎣⎢⎢⎡​1000T100001000T1⎦⎥⎥⎤​⎣⎢⎢⎡​x(k−1)x˙(k−1)y(k−1)y˙(k−1)⎦⎥⎥⎤​+⎣⎢⎢⎡​0.5T2T0.5T2

T⎦⎥⎥⎤​⎣⎢⎢⎡​w1(k)w2(k)w3(k)w4(k)⎦⎥⎥⎤​z(k)=[10000100]⎣⎢⎢⎡​x(k)x˙(k)y(k)y˙(k)⎦⎥⎥⎤​+[v1(k)v2(k)]

现假设初始位置为(-100m，200m)，⽔平运动初速度为2m/s，垂直运动初速度为20m/s，雷达扫描周期，则系统状

态空间进⼀步化为

{x(k+1)=Ax(k)+Bu(k)+Γw(k)z(k)=Hx(k)+v(k)

其中

right]A=⎣⎢⎢⎡​1011⎦⎥⎥⎤​，,。

设过程噪声，其四个分量独⽴同分布，则

right]Q∗=E(w4×1w4×1T)=σw2⎣⎢⎢⎡​1111⎦⎥⎥⎤​

考虑噪声矩阵的加权得到过程噪声协⽅差矩阵

right]Q=Q∗⋅Γ=σw2⎣⎢⎢⎡​0.510.51⎦⎥⎥⎤​

观测噪声，其两个分量独⽴同分布，则观测噪声协⽅差矩阵

right]R=σv2[11]

依此建⽴卡尔曼滤波器模型。

设置最优估计初值

right]^Tx^(1)=[−1002−20020]T、先验协⽅差矩阵，并以此开始迭代，得到实验图像如图所⽰。

u(k)=0

T=1s

B=Γ=⎣

⎢

⎢

⎡0.5

1

0.5

1⎦

⎥

⎥

⎤

H=[

1

0

0

0

0

1

0

0]

w∼N(0,0.1)2

Γ

v∼N(0,10)2

P=1

−⎣

⎢

⎢

⎡1

0

0

0

0

1

0

0

0

0

1

0

0

0

0

1⎦

⎥

⎥

⎤

在使⽤卡尔曼滤波器时，应⾄少保证系统模型或系统测量⾄少⼀个有⾜够的精度，否则卡尔曼滤波器⽆法从中提取到正确的

估计信息。这是因为卡尔曼滤波调整的是对先验模型值与仪器测量值间的信任权重，若⼆者都不准确则卡尔曼跟踪的数值亦

不准确。为验证这⼀结论，下⾯保持测量噪声不变，调整过程噪声⽅差，如图所⽰。

注：需要案例源码可以在评论区留邮箱