kalmanfilter.m

kalman滤波

%VD arithmetic
clear;close all;clc;
T=2;       %雷达扫描周期   
number=1;     %滤波次数,可调
N1=400/T;N2=600/T;N3=610/T;N4=660/T;N5=900/T;    %扫描次数
N=900/T;
x=zeros(N5,1);    
y=zeros(N5,1);
vx=zeros(N5,1);       %x,y方向的速度
vy=zeros(N5,1); 
x(1)=2000;y(1)=10000;
vx(1)=0;vy(1)=-15;
ax=0;ay=0;      %x,y方向的加速度
var=100;        %初始误差矩阵设置值(最大误差等级)
for i=1:N5-1    
    if(i>N1-1&i<=N2-1)
        ax=0.075;ay=0.075;             %减速慢转弯 
        vx(i+1)=vx(i)+ax*T;
        vy(i+1)=vy(i)+ax*T;
    elseif(i>N2-1&i<=N3-1)
        ax=0;ay=0;
        vx(i+1)=vx(i);
        vy(i+1)=vy(i);
    elseif(i>N3-1&i<=N4-1)
        ax=-0.3;ay=-0.3;       %加速快转弯 
        vx(i+1)=vx(i)+ax*T;
        vy(i+1)=vy(i)+ay*T;
    else
        ax=0;ay=0;
        vx(i+1)=vx(i);
        vy(i+1)=vy(i);
    end
    x(i+1)=x(i)+vx(i)*T+0.5*ax*T^2;     %x,y为真值
    y(i+1)=y(i)+vy(i)*T+0.5*ay*T^2;
end
a_x=0;a_y=0;     %x,y方向的加速度置0
n_x=100*randn(N,1);      %误差
n_y=100*randn(N,1); 
z_x=x+n_x;                %(z_x,z_y)为观测样本值=真值+噪声
z_y=y+n_y;       
%%%%%%%%%%%%%%%%%%%%%%%%滤波100次,滤波循环,实际为number=1 %%%%%%%%%%%%
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);
    xk1_s(1:2)=xk_s(1:2);      %(xk1_s,yk1_s)为预测值
    yk1_s(1:2)=yk_s(1:2); 
    I=[1,0,0,0,0,0;0,1,0,0,0,0;0,0,1,0,0,0;0,0,0,1,0,0;0,0,0,0,1,0;0 0 0 0 0 1];   %标准矩阵I
    Ak=[1,T,0.5*T*T,0,0,0;0,1,T,0,0,0;0,0,1,0,0,0;0,0,0,1,T,0.5*T*T;0,0,0,0,1,T;0,0,0,0,0,1]; %状态转移矩阵       %状态变量之间的增益矩阵Ak
    Ck=[1 0 0 0 0 0;0 0 0 1 0 0];       %测量转移矩阵
    Rk=[10000 0;0 10000];       %测量噪声协方差矩阵
    Pk=[var^2  var^2/T 2*var^2/(T^2) 0  0  0      %初始误差矩阵设置
       var^2/T  2*var^2/(T^2) 0 0 0 var^2
       2*var^2/(T^2) 0  0  0  var^2  var^2/T
       0  0  0 var^2 var^2/T  2*var^2/(T^2)
       0  0  var^2 var^2/T  2*var^2/(T^2) 0
       0  var^2 var^2/T  2*var^2/(T^2) 0 0];   %噪声均方误差的协方差阵
%     v_x=(z_x(2)-z_x(1))/T;       %x,y方向初始速度   
%     v_y=(z_y(2)-z_y(1))/T;
%     xk=[z_x(1);v_x;a_x;z_y(1);v_y;a_y];     %将距离和速度做为估计量，可加加速度形成6矢量(状态方程)
%%%%%%%%%%%%%%%%%%%%%%%%Kalman滤波开始,估计循环 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    a_x=0;a_y=0;     %x,y方向的加速度置0
    v_x=(z_x(2)-z_x(1))/T;       %x,y方向初始速度   
    v_y=(z_y(2)-z_y(1))/T;
    xk=[z_x(1);v_x;a_x;z_y(1);v_y;a_y];     %将距离和速度做为估计量，可加加速度形成6矢量(状态方程)
    for r=3:N1-1
        xk1=Ak*xk;          %预测值
        yk=[z_x(r);z_y(r)];     %(z_x,z_y)为测量值(测量方程)
        Pk1=Ak*Pk*Ak';    %(未考虑状态噪声)k时刻滤波的均方误差矩阵
        Hk=Pk1*Ck'*inv(Ck*Pk1*Ck'+Rk);   %增益方程,Hk为增益矩阵,inv矩阵求反函数
        xk=xk1+Hk*(yk-Ck*xk1);       %递推公式
        Pk=(I-Hk*Ck)*Pk1;   %滤波后的均方误差矩阵
        xk_s(r)=xk(1,1);      %(xk_s,yk_s)为估计值
        yk_s(r)=xk(4,1);  
        xk1_s(r)=xk1(1,1);      %(xk1_s,yk1_s)为预测值
        yk1_s(r)=xk1(4,1); 
        temp_x(m,r)=xk_s(r);      % m为循环次数，估计值
        temp_y(m,r)=yk_s(r);  
        temp1_x(m,r)=xk1_s(r);      % m为循环次数，预测值
        temp1_y(m,r)=yk1_s(r);      
    end
    a_x=0.075;a_y=0.075;     %x,y方向的加速度置0
    v_x=(xk_s(N1-1)-xk_s(N1-2))/T;       %x,y方向初始速度   
    v_y=(yk_s(N1-1)-yk_s(N1-2))/T;
    xk=[z_x(N1-1);v_x;a_x;z_y(N1-1);v_y;a_y];     %将距离和速度做为估计量，可加加速度形成6矢量(状态方程)
    for r=N1:N2-1
        xk1=Ak*xk;          %预测值
        yk=[z_x(r);z_y(r)];     %(z_x,z_y)为测量值(测量方程)
        Pk1=Ak*Pk*Ak';    %(未考虑状态噪声)k时刻滤波的均方误差矩阵
        Hk=Pk1*Ck'*inv(Ck*Pk1*Ck'+Rk);   %增益方程,Hk为增益矩阵,inv矩阵求反函数
        xk=xk1+Hk*(yk-Ck*xk1);       %递推公式
        Pk=(I-Hk*Ck)*Pk1;   %滤波后的均方误差矩阵
        xk_s(r)=xk(1,1);      %(xk_s,yk_s)为估计值
        yk_s(r)=xk(4,1);  
        xk1_s(r)=xk1(1,1);      %(xk1_s,yk1_s)为预测值
        yk1_s(r)=xk1(4,1); 
        temp_x(m,r)=xk_s(r);      % m为循环次数，估计值
        temp_y(m,r)=yk_s(r);  
        temp1_x(m,r)=xk1_s(r);      % m为循环次数，预测值
        temp1_y(m,r)=yk1_s(r);      
    end
    a_x=0;a_y=0;     %x,y方向的加速度置0
    v_x=(xk_s(N2-1)-xk_s(N2-2))/T;       %x,y方向初始速度   
    v_y=(yk_s(N2-1)-yk_s(N2-2))/T;
    xk=[z_x(N2-1);v_x;a_x;z_y(N2-1);v_y;a_y];     %将距离和速度做为估计量，可加加速度形成6矢量(状态方程)
    for r=N2:N3-1
        xk1=Ak*xk;          %预测值
        yk=[z_x(r);z_y(r)];     %(z_x,z_y)为测量值(测量方程)
        Pk1=Ak*Pk*Ak';    %(未考虑状态噪声)k时刻滤波的均方误差矩阵
        Hk=Pk1*Ck'*inv(Ck*Pk1*Ck'+Rk);   %增益方程,Hk为增益矩阵,inv矩阵求反函数
        xk=xk1+Hk*(yk-Ck*xk1);       %递推公式
        Pk=(I-Hk*Ck)*Pk1;   %滤波后的均方误差矩阵
        xk_s(r)=xk(1,1);      %(xk_s,yk_s)为估计值
        yk_s(r)=xk(4,1);  
        xk1_s(r)=xk1(1,1);      %(xk1_s,yk1_s)为预测值
        yk1_s(r)=xk1(4,1); 
        temp_x(m,r)=xk_s(r);      % m为循环次数，估计值
        temp_y(m,r)=yk_s(r);  
        temp1_x(m,r)=xk1_s(r);      % m为循环次数，预测值
        temp1_y(m,r)=yk1_s(r);      
    end
    a_x=-0.3;a_y=-0.3;     %x,y方向的加速度置0
    v_x=(xk_s(N3-1)-xk_s(N3-2))/T;       %x,y方向初始速度   
    v_y=(yk_s(N3-1)-yk_s(N3-2))/T;
    xk=[z_x(N3-1);v_x;a_x;z_y(N3-1);v_y;a_y];     %将距离和速度做为估计量，可加加速度形成6矢量(状态方程)
    for r=N3:N4-1
        xk1=Ak*xk;          %预测值
        yk=[z_x(r);z_y(r)];     %(z_x,z_y)为测量值(测量方程)
        Pk1=Ak*Pk*Ak';    %(未考虑状态噪声)k时刻滤波的均方误差矩阵
        Hk=Pk1*Ck'*inv(Ck*Pk1*Ck'+Rk);   %增益方程,Hk为增益矩阵,inv矩阵求反函数
        xk=xk1+Hk*(yk-Ck*xk1);       %递推公式
        Pk=(I-Hk*Ck)*Pk1;   %滤波后的均方误差矩阵
        xk_s(r)=xk(1,1);      %(xk_s,yk_s)为估计值
        yk_s(r)=xk(4,1);  
        xk1_s(r)=xk1(1,1);      %(xk1_s,yk1_s)为预测值
        yk1_s(r)=xk1(4,1); 
        temp_x(m,r)=xk_s(r);      % m为循环次数，估计值
        temp_y(m,r)=yk_s(r);  
        temp1_x(m,r)=xk1_s(r);      % m为循环次数，预测值
        temp1_y(m,r)=yk1_s(r);      
    end
    a_x=0;a_y=0;     %x,y方向的加速度置0
    v_x=(xk_s(N4-1)-xk_s(N4-2))/T;       %x,y方向初始速度   
    v_y=(yk_s(N4-1)-yk_s(N4-2))/T;
    xk=[z_x(N4-1);v_x;a_x;z_y(N4-1);v_y;a_y];     %将距离和速度做为估计量，可加加速度形成6矢量(状态方程)
    for r=N4:N5
        xk1=Ak*xk;          %预测值
        yk=[z_x(r);z_y(r)];     %(z_x,z_y)为测量值(测量方程)
        Pk1=Ak*Pk*Ak';    %(未考虑状态噪声)k时刻滤波的均方误差矩阵
        Hk=Pk1*Ck'*inv(Ck*Pk1*Ck'+Rk);   %增益方程,Hk为增益矩阵,inv矩阵求反函数
        xk=xk1+Hk*(yk-Ck*xk1);       %递推公式
        Pk=(I-Hk*Ck)*Pk1;   %滤波后的均方误差矩阵
        xk_s(r)=xk(1,1);      %(xk_s,yk_s)为估计值
        yk_s(r)=xk(4,1);  
        xk1_s(r)=xk1(1,1);      %(xk1_s,yk1_s)为预测值
        yk1_s(r)=xk1(4,1); 
        temp_x(m,r)=xk_s(r);      % m为循环次数，估计值
        temp_y(m,r)=yk_s(r);  
        temp1_x(m,r)=xk1_s(r);      % m为循环次数，预测值
        temp1_y(m,r)=yk1_s(r);      
    end
end
e_x=0;      %x方向的误差
e_y=0;      %y方向的误差
e1_x=0;  e1_y=0;
e2_x=0;  e2_y=0;
%计算滤波的均值,计算滤波误差的均值
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);  %误差=真实值-滤波估计值
        e1_x=e_x+x(i)-temp1_x(j,i);    %每次循环的误差相加
        e1_y=e_y+y(i)-temp1_y(j,i);  %误差=真实值-滤波估计值
        e2_x=e2_x+x(i)-z_x(i);      %x方向测量值误差
        e2_y=e2_y+y(i)-z_y(i);      %y方向测量值误差
    end
    e_x1(i)=e_x/number;     %平均误差
    e_y1(i)=e_y/number;     %位置估计误差均值
    e1_x1(i)=e1_x/number;     %平均误差
    e1_y1(i)=e1_y/number;     %位置估计误差均值
    e2_x1(i)=e2_x/number;     %x方向测量值平均误差
    e2_y1(i)=e2_y/number;     %y方向测量值误差均值
    e_x=0; e_y=0;
    e1_x=0; e1_y=0;
    e2_x=0;  e2_y=0;
end
%%%%%%%%%%%%%%%%%%%%%%%作图
figure(1);
plot(x,y,'k-',z_x,z_y,'g:',xk_s,yk_s,'b-',xk1_s,yk1_s,'r--');
legend('真实轨迹','观测样本','估计轨迹','预测轨迹',1);
xlabel('x');ylabel('y');
figure(2);
plot(1:N,e2_x1,'g:',1:N,e_x1,'r-',1:N,e1_x1,'b--');          
legend('x方向测量平均误差','x方向估计值平均误差','x方向预测值平均误差',1);
xlabel('N');ylabel('x');
figure(3);
plot(1:N,e2_y1,'g:',1:N,e_y1,'r-',1:N,e1_y1,'b--');   
legend('y方向测量平均误差','y方向估计值平均误差','y方向预测值平均误差');
xlabel('N');ylabel('y');
figure(4);
plot(1:N,x,'k-',1:N,z_x,'g:',1:N,xk_s,'b-',1:N,xk1_s,'r--');
legend('真实轨迹','观测样本','估计轨迹','预测轨迹',2);
xlabel('t');ylabel('x');
figure(5);
plot(1:N,y,'k-',1:N,z_y,'g:',1:N,yk_s,'b-',1:N,yk1_s,'r--');
legend('真实轨迹','观测样本','估计轨迹','预测轨迹',1);
xlabel('t');ylabel('y');