卡尔曼滤波

1960年由Kalman和Bucy提出（空间技术的发展） 1) 是线性最小均方误差滤波

2) 把对信号的先验知识用信号的模型形式表示 3) 时域状态变量法

4) 递推形式的线性最小均方误差算法。

卡尔曼滤波建立在已知随机信号模型的基础上 ；它适用于时变非平稳随机序列。动态估计

4.5.1卡尔曼滤波的基本概念

一个实际的系统可用如下形式表示：

设向量非平稳序列xk1和yk1用下面的动态方程描述：

xkk,k1xk1wk1ykCkxkvk(k0) （4—33）

xk是状态向量，yk是观测向量，wk是输入噪声，vk是观测噪声，k,k1是从k1时刻到k时刻的状态转移阵。上述动态方程可由系统的机理推导得来或由实验数据辨识得到。现假设已知。

有如下假设：

1）wk和vk为零均值白噪声即：

TE[wk]0,Cov[wk,wj]EwkwjQkkjTE[v]0,Cov[v,v]E(vvkjkj)Rkkjk

其中Qk对称半正定Rk对称正定，均为已知。 2）wk和vk不相关即

vkw,jv] Co[EwvTkj0(, k)j 3）初始状态x0是随机向量，且与wk、vk不相关，即

TE[x(0Cov[x0,wk]0Ex0wk)]  TE[x(0Cov[x0,vk]0Ex0vk)]卡尔曼滤波：——状态估计

在已知动态方程（4—33）（状态和观测方程）和样本观测数据yk，yk1，„

ˆk。 情况下，求随机序列样本——状态xk的估计值x由方程可看出：

1) 若无观测噪声可直接由yk求得xk， 2) 若无输入噪声可直接由xk1推出xk。

但实际是wk，vk均不为零，所以不能用（4—33）方程求。

卡尔曼通过对称为一步预测观测误差——新息的修正加之最小均方误差调整准则很好地解决了带有噪声的状态估计问题。

现代控制理论中的状态观测器是一种最简单的估计算法。它只适用于定常系统。卡尔曼也适用于时变系统，而且由于充分利用了噪声的统计特性，精度也高很多。

卡尔曼的递推思想与新息：递推计算和新息是卡尔曼滤波的基本思想，请看如：

yk1y1y2yk1k1

1yk(yk1yk)k1平均计算变成一种递推计算，大大减少了计算量，把k1估计看成是在k基

1(yk1yk)它的作用： 础上的修正，分析修正项

k11) yk1的估计看成是在yk基础上进行修正。 2) yk的估计可理解为对k1次估计的预测。

3) 修正项中yk1yk称为一步观测误差，若一步预测误差为0，说明预测很准不必修正，否则修正。

14) 是对修正的加权系数，k,修正

k15) 一步预测误差yk1yk是第k1的测量的新信息，它提供了yk预测无法预测的信息。称为新息（Innovation）。

卡尔曼滤波器正是应用了上述新息的递推计算原则。

4.5.2卡尔曼滤波的新息递推：

ˆ采用的是：过去状态对现状态的预测卡尔曼滤波对现状态Xk的估计Xkˆˆ(ykCkX)Xkk1，加上观测偏差kk1的适当加权修正Kk即：

ˆXˆˆXKk(ykCkX) kkk1kk1 （4—34）

ˆk1次对k次的预测估计。 Xkk1是基于

ˆˆ （4—35）Xk,k1X k1kk1ˆˆˆ所以 X kk,k1Xk1Kk(ykCkk,k1Xk1) （4—36） 预测 +修正 =加权·新息

其中Kk——修正加权称为增益矩阵，它决定着递推方向，质量。 如何确定Kk是卡尔曼滤波器的核心。把Kk建立成一种能够递推的结构形式是卡尔曼的独到之处！实时性能好！

只要增益矩阵Kk已知，就可以用（4—36）式递推地求得这一步的状态估计

ˆ)的重视程度，直接影值。而Kk的大小反映了对观测yk和新息(ykCkk,k1Xk1响到状态估计的准确性，是状态估计的关键。

确定Kk的准则：最小均方误差准则。

ˆk)T(xkxˆk)min （4—37） JE(xkxˆk)(xkxˆk)trP(xkx即 JtrE （4—38） kminKKTdef其中tr表示方阵迹，即对角元之和。Pk是状态估计的均方误差矩阵。 极小化J，即

求Kk。

选择Kk使J最小，得卡尔曼滤波器递推公式为：

Pkk1k,k1Pk1Tk,k1Qk1 （4—40）

TTCKPkk1CkRk KkPkk1CktrPk0， （4—39） Kk1 （4—41）

ˆˆˆ Xkk,k1Xk1Kk(ykCkk,k1Xk1) （4—42） Pk(IKKCK)Pkk1 （4—43）

Pkk1，KK，和PK的计算与观测yK无关，只要条件容许，可以事先计算好，ˆ求状态估计即可。 而实时滤波只要递推Xk卡尔曼滤波：是时域内不断地“预测——修正——递推”过程。

1）PK小说明预测较准，K也小减小修正，若在k1时刻输入噪声wk1很小

T一步预测误差Pkk1很小由 KkPkk1CCKPkk1CkRk0，再由

 Xkk,k1Xk1Kk(ykCkkk,X1k)1kkX,k1

Tk1既无须考虑观测量的修正，只用状态方程即可，这符合逻辑。

ˆ不准，ˆ)2）K较大，重视新息修正Kk(ykCkk,k1XPK大说明预测k,k1Xk1k1的作用，（加大实际观测与模型估计的修正作用）。

3）如果在k时刻观测噪声Rk很小（4—41）

TTTT11CKPkk1CkPkk1CkKkPkk1Ck(Ck)Pkk1Ck1Ck1 1再由（4—42）

 Xkk,k1Xk1Ck1(ykCkk,k1Xk1)Ck1yk

既无须考虑状态方程，只要用观测方程直接计算即可，这也是很符合逻辑的结论。

可以证明卡尔曼滤波是无偏的状态估计

4.5.3卡尔曼滤波举例

11xk1wk1xk举例：随机状态方程为：01yk10xkvk （例5-1）

w1kw1jw1kw2j00kjE[wk]0,Cov[wk,wj]Ewwww01其中 2k2j2k1jkE[vk]0,Cov[vk,vj][2(1)]kj（例5-2）

这里输入噪声wk是平稳白噪声，而观测噪声vk是非平稳白噪声。所以即使在稳态时，观测yk也是一个非平稳序列。为简单起见，初始的状态估计误差的协方差阵设为对角阵，即

2(x1x1)2)(x1x1)(x2x100P0E（例5-3） 2(xx010(2x2)x(2x1)x2)1求增益矩阵K1,K2,,K10和具体的卡尔曼滤波递推公式。 解 状态方程中的各参数矩阵是定常的，即

111）由式（例5-1）k,k1,cc1001k

002）由式（例5-2）噪声参数为： QkQ,Rk2(1)k 013）则由式（4—40） Pkk1k,k1Pk1Tk,k1Qk1

预测误差协方差阵的递推公式为：

111000 Pkk1Pk1Q （例5-4） P01k11101TTTCKPkk1CkRk4）则由式（4—41） KkPkk1Ck1

增益矩阵的递推公式为：

11TKkPkk1CCPkkCRkPkk110(1T1 （例5-5）

1k0P)2(1)kk105）则由式（4—43） Pk(IKKCK)Pkk1

估计误差协方差阵的递推公式为：

P[IKK(10)]Pkk1 （例5-6） k(IKKC)Pkk120106）令k=1 将（例5-3）代入（例5-4）求得P； 1010112021将P代入（例5-5）求得K1 1010212021将K1代入（例5-6）求得P110211021 131217）同理令k=2，3，4，5用（例5-4）（例5-5）（例5-6）递推公式可算得前5步的结果如下表 k 0 1 2 3 4 5 20108.166.719.0553.62.25.623Pkk1 10116.717.2454.212.22.732Kk 0.950.45 0.730.6 0.90.5 0.550.33 0.850.45 0.950.48 0.486.24前十步的增益矩阵见图， Pk 100 010k1kk2k1.00.80.60.40.2122.191.82 1.823.210.90.5 0.51.71.621 120.850.45 0.450.63k1kk2k3456710k卡尔曼滤波前十步的增益矩阵波动是由于观测噪声的方差是波动的

由图可见当k4时Kk即趋于稳定的周期序列，即这时有：

Tk为奇数5),(0.85,0.4Kk Tk为偶数3),(0.55,0.3根据这一特殊情况，滤波计算变得很容易，由式（4—42）

ˆˆˆ Xkk,k1Xk1Kk(ykCkk,k1Xk1)

k4是状态估计的递推公式为： XkXk1Kk(ykCXk1)

1111 XKk[yk(10)X]01k101k1110.85Xk1[yk(11)Xk1];k为奇数0.45010.5511X[yk(11)Xk1];k为偶数01k10.33分析讨论：

①上面已说过，增益矩阵反映了对新息的重视程度。由上式可知，当k为

Tk为奇数(0.85,0.45),奇数时增益矩阵较大Kk，这说明这时的新息较可Tk为偶数(0.55,0.33),信，这是由于当k为奇数时观测噪声较小的缘故（此时Rk2(1)k1，偶数时Rk2(1)k3）。

②由上表可以看到，估计误差的协方差PK和预测误差的协方差Pkk1，都逐渐变小，这说明估计越来越准，但当k为奇数时协方差更小，反映了这时由于观测噪声小。估计就更准确。

在Kalman滤波器的实现过程中，应注意几点：

1、首先是滤波方程中的系统状态空间模型应该与真实系统的模型基本一致； 2、保证待估计状态的可观测性要好，即各状态要得到充分的激励，并且状态之

间的相关性要小；

3、初始条件以及噪声协方差矩阵的选取要合理。 对于最后一点，具体来说：

1）预估状态初值x[1|0]不能偏离真值x[0]太远，否则滤波器或者不收敛，或者收敛速度太慢，所以滤波初值应根据经验和实际情况适当选取，不应该随便地全部选为零。

2）预估状态的初始协方差矩阵P(1|0)是对预估状态初值偏差的一种反映，这一偏差由两部分构成，一个是真值x[0]的不确定性，即x[0]本身也是服从某一分布（具有自身均值和方差）的随机向量，另一个是x[1|0]相对于x[0]均值的偏差（由于x[0]均值未知，所以该偏差必然存在）。综合上面两部分因素，P(1|0)不应过小，滤波过程初始阶段修正不足，滤波过程变长甚至发散。 但是从方程 ：

Pkk1k,k1Pk1Tk,k1Qk1 （4—40）

TTCKPkk1CkRk KkPkk1Ck1 （4—41）

ˆˆˆ Xkk,k1Xk1Kk(ykCkk,k1Xk1) （4—42） Pk(IKKCK)Pkk1 （4—43）

式中可以看出P(1|0)过大又会造成初始的滤波增益较大，也就是说对修正量中初始观测噪声加了比较大的权，导致滤波状态修正过量，初始滤波效果变差、滤波过程效率降低。

3）因为通常初始时刻P(1|0)远大于Q和R，所以在初始阶段滤波增益主要受P的影响。但是随着时间的推移，迭代过程的进行，P越来越小，这时滤波增益主

要取决于状态噪声Q和观测噪声R。如果Q过小则滤波增益基本为零，估值与观测量解耦；如果Q过大则稳态时估值对观测噪声的实时依赖性偏强，不确定性程度增加。R的作用与Q相反。

K(n)大，修正作用强 K(n)小，估计与观测解耦 滤波增益K(n)随时间变化 估值协方差P(n|n) 噪声协方差 Q (n|n), R (n|n)