kalman滤波应用

目录

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

1 无噪牛顿系统

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

[rva]=[010001000][rva]
x=Ax

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

xk+1=Fxk

其中F=exp(AT) ,展开有:

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

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

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

ˆxk=Fˆx+k17Pk=FP+k1FT

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

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

yk=Hkxk+vk

其中

vkN(0,Rk)Rk=σ2

我们可以计算出Kalman增益:

Kk=PkHTk(HkPkHTk+Rk)1

KkHkRk带入,有:

Kk=[Pk,11Pk,12Pk,13]1Pk,11+σ2

估计误差协方差:

P+k=PkKkHkPk

如果我们把Pk 完整的写出来,并替换Hk,Kk,可得:

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

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

2 matlab实现

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

xk=Fk1xk1+Gk1uk1+wk1yk=Hkxk+vkE(wkwTj)=QkδkjE(vkvTj)=RkδkjE(wkvTj)=0

Kalman滤波器初始化为:

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

Kalman滤波器的整个过程为:

Pk=Fk1P+k1FTk1+Qk1Kk=PkHTk(HkPkHTk+Rk)1=P+kHTkR1kˆxk=Fk1ˆx+k1+Gk1uk1ˆx+k=ˆxk+Kk(ykHkˆxk)P+k=(IKkHk)Pk(IKkHk)T+KkRkKTk=[(Pk)1+HTkR1kHk]1=(IKkHk)Pk

这是个迭代的过程,在迭代过程中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= 是位置估计的方差。