vd.m

kalman滤波的程序及ppt

function VD
clear;
T=2;            %雷达扫描周期
num=50;         %滤波次数
%**************************************************************************
N1=400/T; N2=600/T; N3=610/T; N4=660/T; N5=900/T;
x=zeros(N5,1); y=zeros(N5,1);
vx=zeros(N5,1); y=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;
    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=var*randn(N5,1); ny=var*randn(N5,1);
    %加入噪声
    zx=x+nx; zy=y+ny;
    %kalman滤波初始化
    %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);
    yks(1)=zy(1);
    xks(2)=zx(2);
    yks(2)=zy(2);
    rex(m,1)=xks(1); rex(m,2)=xks(2);
    rey(m,1)=yks(1); rey(m,2)=yks(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))/T;
    vy=(zy(2)-zy(1))/T;
    xk=[zx(1),vx,zy(1),vy]';
    %kalman滤波开始
    for r=3:N5
        if(u<=40)               %非机动模型，赋初值
            if(low==0)
                [o,h,g,q,perr,xk]=lmode_initial(T,r,zx,zy,vkxs,vkys,perr2);
                z=2:1;
                high=0;
                low=1;
                ua=0;
            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);             %kalman滤波标准式
            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,g,h,q,perr,xk]=hmode_initial(T,r,e,zx,zy,xk1s,yk1s,vkxs,vkys,perr11,perr12,perr22);
                high=1; low=0;
                for i=r-5:r-1
                    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);             %kalman滤波标准式
                    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);             %kalman滤波标准式
            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);
                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;
xks=0; yks=0;
ex1=N5:1; ey1=N5:1;
qx=N5:1; qy=N5:1;
xks1=N5:1; yks1=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;
        xks=xks+rex(j,i);
        yks=yks+rey(j,i);
    end
    ex1(i)=ex/num;
    ey1(i)=ex/num;
    qx(i)=(eqx/num-(ex1(i)^2))^0.5;
    qy(i)=(eqy/num-(ey1(i)^2))^0.5;
    xks1(i)=xks/num;
    yks1(i)=yks/num;
    ex=0; eqx=0; ey=0; eqy=0;xks=0; yks=0;
end
%绘图
figure(1);
plot(x,y,'k-',zx,zy,'g:',xks1,yks1,'r-');
legend('真实轨迹','观测样本','估计轨迹');
%figure(2);
%plot(zx,zy); legend('观测样本');
%figure(3);
%plot(xks,yks); legend('估计轨迹');
figure(2);
plot(ex1); legend('x方向平均误差');
figure(3);
plot(qx); legend('x方向滤波曲线的标准差曲线');