vd.m

这是我自己编写的一段VDkalman程序

%VD算法
clear;

     
T=2;%雷达扫描周期
num=50;%滤波次数
%***************产生真实轨迹**********************
 N1=400/T;N2=600/T;N3=610/T;N4=660/T;N5=900/T;N6=670/T;N7=720/T;
 x=zeros(N5,1);y=zeros(N5,1);
 vx=zeros(N5,1);vy=zeros(N5,1);
 x(1)=2000;y(1)=10000;
 vx(1)=0;vy(1)=15;
 ax=0;ay=0;
 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)+0.075*T;
         vy(i+1)=vy(i)+0.075*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)-0.3*T;
         vy(i+1)=vy(i)-0.3*T;
     elseif(i>N4-1&i<=N6-1)
          ax=0;ay=0;
         vx(i+1)=vx(i);
         vy(i+1)=vy(i);
     elseif(i>N6-1&i<=N7-1)
         ax=-0.3;ay=-0.3;
         vx(i+1)=vx(i)-0.3*T;
         vy(i+1)=vy(i)-0.3*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;
     y(i+1)=y(i)+vy(i)*T+0.5*ay*T^2;
     
 end
 rex=num:N5;
 rey=num:N5;
 %滤波50次
 for m=1:num
     %产生噪声
    
     nx=randn(N5,1);ny=randn(N5,1);
     %加入噪声
     zx=x+nx;
     zy=y+ny;
     %滤波初始化
     rex(m,1)=2000;
     rey(m,1)=10000;
     ki=0;
     low=1;high=0;
     u=0;ua=0;
     e=0.8;%加权衰减因子
     z=2:1;
     xks(1)=zx(1);xks(2)=zx(2);
     yks(1)=zy(1);yks(2)=zy(2);
     o=4:4;g=4:2;h=2:4;q=2:2;xk=4:1;perr=4:4;
     o=[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=[T/2 0; T/2 0;0 T/2;0 T/2];
     q=[10000 0;0 10000];
     perr=[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)];
     vx=(zx(2)-zx(1))/2;
     vy=(zy(2)-zy(1))/2;
     xk=[zx(1); vx; zy(1); vy];
    %kalman滤波开始 
    for r=3:N5
        if(u<=40)
            if(low==0)
                
                o=4:4; g=4:2;
                h=2:4; q=2:2;
                xk=4:1; perr=4:4;
                o=[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=[(T^2)/2 0;T 0;0 (T^2)/2;0 T];
                q=[10000 0;0 10000];
                xk=[zx(r-1);vkxs(r-1);zy(r-1);vkys(r-1)];
                perr=perr2(1:4,1:4);
                z=2:1;high=0;low=1;ua=0;
                
            end
            z=[zx(r);zy(r)];
            xk1=o*xk;
            perr1=o*perr*o';
            k=perr*h'*inv(h*perr1*h'+q);
            xk=xk1+k*(z-h*xk1);
            perr=(eye(4)-k*h)*perr1;
            xks(r)=xk(1,1);yks(r)=xk(3,1);vkxs(r)=xk(2,1);vkys(r)=xk(4,1);
            xk1s(r)=xk1(1,1);yk1s(r)=xk1(3,1);
            perr11(r)=perr(1,1);perr12(r)=perr(1,2);perr22(r)=perr(2,2);
            if(r>=20)
                v=z-h*xk1;
                w=h*perr*h'+q;
                p=v'*inv(w)*v;
                u=e*u+p;
                s(r-19)=u;
            end
        elseif(u>40)
            if(high==0)
            
            o=6:6;g=6:2;
             h=2:6;q=2:2;
             xk=6:1;perr=6:6;
             o=[1 T 0 0 0.5*T^2 0;0 1 0 0 T 0;0 0 1 T 0 0.5*T^2;
                0 0 0 1 0 T;0 0 0 0 1 0;0 0 0 0 0 1];
            
              h=[1 0 0 0 0 0; 0 0 1 0 0 0];
              jsx=0.5*(zx(r-1/(1-e))-xk1s(r-1/(1-e)));
              jsy=0.5*(zy(r-1/(1-e))-yk1s(r-1/(1-e)));
              vx=vkxs(r-1/(1-e)-1)+2*jsx;
               vy=vkys(r-1/(1-e)-1)+2*jsy;
             xk=[zx(r-5);vx;zy(r-5);vy;jsx;jsy];
             p111=10000;p112=10000;p115=5000;
             p155=0.25*(10000+perr11(r-6)+4*perr12(r-6)+perr22(r-6));
             p122=10000+perr11(r-6)+4*perr12(r-6)+perr12(r-6);
             p125=5000+0.5*perr11(r-6)+perr22(r-6)+1.5*perr12(r-6);
              perr=[p111 p112 0 0 p115 0;0 p122 0 0 p125 0;
                    0 0 100 0 0 0;0 0 0 100 0 0;
                    0 0 0 0 p155 0;0 0 0 0 0 100];
             high=1;low=0;
             for i=r-5:r-1
                 z=[zx(i);zy(i)];
                 xk1=o*xk;
                 perr1=o*perr*o';
                 k=perr1*h'*inv(h*perr1*h'+q);
                 xk=xk1+k*(z-h*xk1);
                 perr=(eye(6)-k*h)*perr1;
                  xks(r)=xk(1,1);yks(r)=xk(3,1);vkxs(r)=xk(2,1);vkys(r)=xk(4,1);
                  xk1s(r)=xk1(1,1);yk1s(r)=xk1(3,1);
             end
            end
           z=[zx(r);zy(r)];
          xk1=o*xk;
          perr1=o*perr*o';
          k=perr1*h'*inv(h*perr1*h'+q);
          xk=xk1+k*(z-h*xk1);
          perr=(eye(6)-k*h)*perr1;
          xks(r)=xk(1,1);yks(r)=xk(3,1);vkxs(r)=xk(2,1);vkys(r)=xk(4,1);
          xk1s(r)=xk1(1,1);yk1s(r)=xk1(3,1);
          ag=[xk(5,1);xk(6,1)];
          perr2=perr;
          ki=ki+1;
          pm=[perr(5,5) perr(5,6);perr(6,5) perr(6,6)];
          pa=ag'*inv(pm)*ag;
          sa(r)=pa;
          if(ki>5) %退出机动判断
            u1=sa(r-4)+sa(r-3)+sa(r-2)+sa(r-1)+sa(r);
            sb(r)=u1;
            if(u1<20)
                u=0;
            end
          end
        end
        rex(m,r)=xks(r);
        rey(m,r)=yks(r);
    end %结束一次滤波
 end
 ex=0;ey=0;
 eqx=0;eqy=0;
 ey1=0;
 ex1=N5:1;ey1=N5:1;
 qx=N5:1;qy=N5:1;
 %计算滤波均值和均方差
 for i=1:N5
     for j=1:num
         ex=ex+x(i)-rex(j,i);
         ey=ey+y(i)-rey(j,i);
         eqx=eqx+(x(i)-rex(j,i))^2;
          eqy=eqy+(y(i)-rey(j,i))^2;
     end
     ex1(i)=ex/num;
     ey1(i)=ey/num;
     qx(i)=(eqx/num-(ex1(i)^2))^0.5;
     qy(i)=(eqy/num-(ey1(i)^2))^0.5;
     ex=0;eqx=0;eqy=0;
     ey=0;
 end
 %绘图
 figure(1);
 plot(x,y,'k-',zx,zy,'g:',xks,yks,'r-.');
 legend('真实轨迹', '观测样本', '估计轨迹');
 figure(2);
 plot(zx,zy);legend('观测样本');
 figure(3);
 plot(xks,yks); legend('估计轨迹');
 figure(4);
 plot(ex1); legend('x方向平均误差');
figure(5);
plot(qx); legend('x方向滤波曲线的标准差曲线');