1、理论部分
卡尔曼滤波使用的准则是线性最小方差估计(LMMSE),因此,经典卡尔曼滤波适用于线性高斯系统,系统模型如下:
W和V分别代表过程噪声和量测噪声,数学期望为0,方差分别为Q和R,X代表系统状态。本文假定已有一定的线性系统基础,因此不对上图中公式做具体介绍。并且本文着重介绍公式的由来、公式为什么是这种形式,至于其中的物理意义之类的就不多说了。
首先给出卡尔曼滤波的5条公式:
正式计算时每个循环中都顺序执行上述公式,即可实现卡尔曼滤波,下面说下由来。
首先,第四个公式代表卡尔曼滤波的校正,等式右边由两项组成,第一项代表由上一时刻的最优估计值递推估计的本时刻状态值,即:本时刻的经验估计值。第二项代表估计值与观测值之间的误差,两项相加,代表,由本时刻观测值与估计值之间的误差对估计值进行修正,K为修正作用的大小。这种形式,从物理意义上来说,是比较合理的,通过使用系统的经验估计值以及观测值,综合得出系统的估计值。
举个例子,上一时刻房间温度最优值为26度,由于温度变化缓慢,则可以根据经验估计本次温度也为26度,即四式右边第一项,而此时温度计读数为28度,那么本时刻温度应该是多少?不知道温度计精度的情况下,可以对两个数据求平均,为27度,即T=26+0.5*(28-26)=27。这里0.5就是本例子中的修正系数,那么假如温度计精度很高,将修正系数增大一些,比如增大到0.9,那么结果T=26+0.9*(28-26)=27.8,可以看到滤波结果很接近温度计读数,增大到1则滤波结果就是温度计读数,相反,如果温度计精度很差,那么减小修正系数,则可以使滤波结果更接近等式右边第一项得估计值。因此,合理调整K的大小,可以得到很好的滤波结果,卡尔曼的精髓就在于,能够动态调整K的大小,得到最优的估计解。
那么如何得到K呢?引入正交投影定理的概念
用图表示更加形象一些
X为待估计状态,这里假设x的估计解有x=Az+b的形式,定理内容为:状态x在观测z上的正交投影就是x在z上的线性最小方差估计,由定理可以得出
这里E为求数学期望。
由此可以得到K的求解公式
全部带入即可求得修正系数K的表达式为
可以看到,K的求解又引入了P(k+1|k)项,继续计算
这里,又引入了P(k|k)项
作变形
至此,卡尔曼滤波的公式推导完毕,过程看似全部都是密密麻麻的公式,实际上就是利用:过程噪声、观测噪声的数学期望为0,以及正交投影定理的两个公式,代入一步步写出即可,并不需要其他高端的知识。
公式中P代表方差,P(k+1|k)为k时刻转移至k+1时刻的方差,为“预测方差”,P(k|k)为估计方差,^为估计值,~为误差值。
2、实践部分
以一个例子说明卡尔曼滤波的用法。Kalman滤波在船舶GPS定位系统中的应用。
假定用GPS观测船舶的位置,可以观测到横向和纵向的位置值。船舶沿某方向匀速直线行驶,将由海风海浪引起的随机加速度认为是和观测不相关的随机白噪声,初始位置为(-100m,200m),水平速度2m/s,垂直速度20m/s,GPS观测周期为1秒,系统方程为
代码:
function main
clc;
clear;
close all;
T=1;
N=80/T;
X=zeros(4,N);
X(:,1)=[-100,2,200,20];
Z=zeros(2,N);
Z(:,1)=[X(1,1),X(3,1)];
delta_w=1e-3;
Q=delta_w*diag([0.5,1,0.5,1]) ;
R=50*eye(2);
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
H=[1,0,0,0;0,0,1,0];
%------------------------------------------------------------------%
for t=2:N
X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);
Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1);
end
Xkf=zeros(4,N);
Xkf(:,1)=X(:,1);
% Xkf(:,1) = [-50,2,110,20];
P0=eye(4);
for i=2:N
Xn=F*Xkf(:,i-1);
P1=F*P0*F'+Q;
K=P1*H'*inv(H*P1*H'+R);
Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);
P0=(eye(4)-K*H)*P1;
end
for i=1:N
Err_Observation(i)=RMS(X(:,i),Z(:,i));
Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));
end
figure
hold on;box on;
plot(X(1,:),X(3,:),'-g');
plot(Z(1,:),Z(2,:),'-b.');
plot(Xkf(1,:),Xkf(3,:),'-r+');
legend('真实轨迹','观测轨迹','滤波轨迹')
figure
hold on; box on;
plot(Err_Observation,'-ko','MarkerFace','g')
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
legend('滤波前误差','滤波后误差')
%------------------------------------------------------------------%
function dist=RMS(X1,X2);
if length(X2)<=2
dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end
%------------------------------------------------------------------%