kalman滤波应用

目录

为加深对kalman滤波的理解。本文给出几个kalman滤波的例子。

1 无噪牛顿系统

假设一个牛顿系统，包含位置r，速度v和加速度a. 这个系统可以 描述为：

[⋅r⋅v⋅a]=[010001000][rva]
⋅x=Ax

这是连续系统的描述方式，离散化之后可以表示为：

xk+1=Fxk

其中F=exp(AT) ，展开有：

F=exp(AT)=I+AT+(AT)22!+…=[1T(AT)22!01T000]

其中T是离散化的采样时间。

这个系统的kalman滤波描述为：

ˆx−k=Fˆx+k−17P−k=FP+k−1FT

我们发现在(k−1)+到k−之间，估计误差的协方差是增加的。因 为在(k−1)+和k−之间我们没有收到任何观察值，估计误差增大也 是可以理解的。

现在假定我们对位置进行测量，测量方差为σ2:

yk=Hkxk+vk

其中

vk∼N(0,Rk)Rk=σ2

我们可以计算出Kalman增益：

Kk=P−kHTk(HkP−kHTk+Rk)−1

把Kk，Hk，Rk带入，有：

Kk=[P−k,11P−k,12P−k,13]1P−k,11+σ2

估计误差协方差：

P+k=P−k−KkHkP−k

如果我们把P−k 完整的写出来，并替换Hk,Kk，可得：

P+k=P−k−1P−k,11+σ2[P−k,1100P−k,1200P−k,1300]P−k=P−k−1P−k,11+σ2[(P−k,11)2P−k,11P−k,21P−k,11P−k,31P−k,12P−k,11(P−k,12)2P−k,12P−k,31P−k,13P−k,11P−k,13P−k,12(P−k,13)2]

通过式~(8)，我们将看到从k−到k+，协方差矩阵的迹 下降。当我们得到一个新的观测时，我们希望对系统的状态估计会有所改进。也 就是说，我们希望协方差降低。式~(8)告诉我们这个协方差的确降低 了。

2 matlab实现

在使用matlab实现之前，让我们再次总结一下离散时间kalman滤波器的过程。首 先，这个系统的描述为：

xk=Fk−1xk−1+Gk−1uk−1+wk−1yk=Hkxk+vkE(wkwTj)=Qkδk−jE(vkvTj)=Rkδk−jE(wkvTj)=0

Kalman滤波器初始化为：

ˆx+0=E(x0)P+0=E[(x0−ˆx+0)(x0−ˆx+0)T]

Kalman滤波器的整个过程为：

P−k=Fk−1P+k−1FTk−1+Qk−1Kk=P−kHTk(HkP−kHTk+Rk)−1=P+kHTkR−1kˆx−k=Fk−1ˆx+k−1+Gk−1uk−1ˆx+k=ˆx−k+Kk(yk−Hkˆx−k)P+k=(I−KkHk)P−k(I−KkHk)T+KkRkKTk=[(P−k)−1+HTkR−1kHk]−1=(I−KkHk)P−k

这是个迭代的过程，在迭代过程中k=1,2,…向前演进。

接下来是matlab代码实现： 

 1: % sampling interval
 2: T = 5;
 3: % position measurement standard deviation
 4: sigma = 30;
 5: R = sigma^2;
 6: % initial state estimate uncertainty
 7: P0 = [100 0 0; 0 10 0; 0 0 1];
 8: H = [1 0 0];
 9: % state transition matrix
10: F = [1 T T*T/2; 0 1 T; 0 0 1];
11: x = [1; 1; 1]; % initial state
12: xhat = x; % initial state estimate

T 是采样间隔。=sigma= 是位置估计的标准差。=R= 是位置估计的方差。