卡尔曼滤波及误差分析程序.txt

基于cvca模型的标准卡尔曼程序及误差分析

%.........卡尔曼滤波算法.........%
clear all;
close all;
randn('state',0);

%.........输入观测数据.........%
load track zt2;
load track_zhenshi zt1;
x=zt1;
z=zt2;

%.........卡尔曼滤波各参数.........%
steps=30;
T=1;
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];
G=[0.5*T^2,0
    T,0
    0,0.5*T^2
    0,T];

%.........初始值设置.........%
x0(:,1)=[300,10,300,10]';
P(:,:,1)=[10,0,0,0
          0,1,0,0
          0,0,10,0
          0,0,0,1];
Xp=zeros(4,steps);  %......4 * 1
Xn=[x0(:,1) zeros(4,steps-1)];
Pp=zeros(4,4*steps);%......4 * 4
Pn=[P(:,:,1) zeros(4,4*(steps-1))];

Kg=zeros(4,2*steps);%......4 * 2
Q=zeros(2,2*steps); %......2 * 2
R=zeros(2,2*steps); %......2 * 2
I=eye(4);

%.........卡尔曼滤波算法.........%
for k=1:(steps-1)
    i=2*k-1;
    j=4*k-3;
    Q(:,i:i+1)=5*eye(2);%...过程噪声协方差
    R(:,i:i+1)=1*eye(2);%...观测噪声协方差
    Xp(:,k+1)=F*Xn(:,k);
    Pp(:,j+4:j+7)=F*Pn(:,j:j+3)*F'+G*Q(:,i:i+1)*G';
    Kg(:,i:i+1)=Pp(:,j+4:j+7)*H'*inv(H*Pp(:,j+4:j+7)*H'+R(:,i:i+1));
    Xn(:,k+1)=Xp(:,k+1)+Kg(:,i:i+1)*(z(:,k+1)-H*Xp(:,k+1));   
    Pn(:,j+4:j+7)=(I-Kg(:,i:i+1)*H)*Pp(:,j+4:j+7);
end 

%.........经卡尔曼滤波后的误差.........%
for i=2:steps
    zzkf(i-1)=sqrt((Xn(3,i)-x(2,i))^2+(Xn(1,i)-x(1,i))^2);
    zzvx(i-1)=abs(Xn(2,i)-10);
    zzvy(i-1)=abs(Xn(4,i)-10);
end

%.........经卡尔曼滤波后的仿真及误差分析.........%
 figure(1)
 plot(z(1,1:30),z(2,1:30),'k+',Xn(1,:),Xn(3,:),'ro');
 axis([300 600 300 600]);
 xlabel('X 轴方向位移（单位：m）');
 ylabel('Y 轴方向位移（单位：m）');
 title('目标运动位置真实值/估计值')
 legend('目标运动位置真实值','目标位置估计值',2);
 
 figure(2)
 plot(zzkf,'g');
 axis([0 30 0 5]);
 xlabel('时间（单位：s）');
 ylabel('误差');
 legend('KF位置估计值误差',2); 
 title('KF估计值与真实值之间的位置误差分布')

 figure(3)
 plot([1:steps-1],zzvx,'r');
 axis([1 30 0 5]);
 xlabel('时间（单位：s）');
 ylabel('误差');
 legend('KF速度（X轴方向）估计值误差',2); 
 title('KF估计值与真实值之间的X轴速度误差分布')

 figure(4)
 plot([1:steps-1],zzvy,'r');
 axis([1 30 0 5]);
 xlabel('时间（单位：s）');
 ylabel('误差');
 legend('KF速度（Y轴方向）估计值误差',2); 
 title('KF估计值与真实值之间的Y轴速度误差分布')