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)=0Kalman滤波器初始化为:
ˆ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= 是位置估计的方差。