kalmanfiltertotrackingsimulink.m

些程序是一个雷达跟踪一目标的仿真图形,可实现kalman滤波的估计轨迹与真实轨迹的误差,并分别可绘出X,Y方向的跟踪误差,对初学都来说,是一个非常好的学习例子

clc;
clear;
T=5;           %雷达扫描周期    
number=100;    %滤波次数
%%%%%%%%%%%%%%%%%%%%%%%%
N=800/T;   
x=zeros(N,1);
y=zeros(N,1);
x(1)=-2000;
y(1)=500;     
v_x=10;v_y=0;    
a_x=0;a_y=0;  
var=100;       %
I=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];
%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:N-1    
    x(i+1)=x(i)+v_x*T+0.5*a_x*T^2;
    y(i+1)=y(i)+v_y*T+0.5*a_y*T^2;
end;
n_x=100*randn(N,1);
n_y=100*randn(N,1); 
z_x=x+n_x;
z_y=y+n_y;        %(z_x,z_y)为观测样本值=真值+噪声
%%%%%%%%%%%%%%%%%%%%%%%%滤波100次,滤波循环
for m=1:number
    xk_s(1)=z_x(1);    %赋初值
    yk_s(1)=z_y(1);
    xk_s(2)=z_x(2);
    yk_s(2)=z_y(2);
Ak=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态变量之间的增益矩阵Ak
Ck=[1 0 0 0;0 0 1 0];
Rk=[10000 0;0 10000];
Pk=[var^2 var^2/T 0 0
    var^2/T 2*var^2/(T^2) 0 0
    0 0 var^2 var^2/T
    0 0 var^2/T 2*var^2/(T^2)];%噪声的均方误差阵
v_x=(z_x(2)-z_x(1))/T;       
v_y=(z_y(2)-z_y(1))/T;
xk=[z_x(1);v_x;z_y(1);v_y];  %将距离和速度做为估计量
%%%%%%%%%%%%%%%%%%%%%%%%Kalman滤波开始,估计循环
for r=3:N
    yk=[z_x(r);z_y(r)];
    Pk1=Ak*Pk*Ak';%(未考虑噪声)k时刻滤波的均方误差矩阵
    Hk=Pk1*Ck'*inv(Ck*Pk1*Ck'+Rk); %增益方程
    xk=Ak*xk+Hk*(yk-Ck*Ak*xk);     %递推公式
    Pk=(I-Hk*Ck)*Pk1;%滤波后的均方误差矩阵
    xk_s(r)=xk(1,1);      
    yk_s(r)=xk(3,1);       %(xk_s,yk_s)为估计值
    temp_x(m,r)=xk_s(r);
    temp_y(m,r)=yk_s(r);      
end
end
e_x=0;e_y=0;
eq_x=0;eq_y=0;
e_y1=0;
e_x1=N:1;e_y1=N:1;
%计算滤波的均值,计算滤波误差的均值
for i=1:N
    for j=1:number
        e_x=e_x+x(i)-temp_x(j,i);
        e_y=e_y+y(i)-temp_y(j,i);  %误差=真实值-滤波估计值
    end;
    e_x1(i)=e_x/number;
    e_y1(i)=e_y/number;     %位置估计误差均值
    e_x=0;eq_x=0;e_y=0;eq_y=0;
end
%%%%%%%%%%%%%%%%%%%%%%%作图
%figure(1);
%plot(x,y,'r-',z_x,z_y,'g:',xk_s,yk_s,'b-.');
%legend('真实轨迹','观测样本','估计轨迹');
figure(2);
plot(e_x1);          
legend('x方向平均误差');
figure(3);
plot(e_y1);          
%legend('y方向平均误差');