gaikalman.m

kalman滤波的程序及ppt

function kalman
clc;
clear;
T=2;                %雷达扫描周期
num=50;            %滤波次数
%*******************产生真实轨迹*******************************************
N=800/T;
x=zeros(N,1); y=zeros(N,1);
vx=zeros(N,1); vy=zeros(N,1);
x(1)=-2000; y(1)=500;
vx=10; vy=0;
ax=0; ay=0;
var=100;
%产生真实轨迹
for i=1:N-1
    x(i+1)=x(i)+vx*T+0.5*ax*T^2;
    y(i+1)=y(i)+vy*T+0.5*ay*T^2;
end

nx=zeros(N,1); ny=zeros(N,1);
nx=100*randn(N,1); ny=100*randn(N,1);
zx=x+nx; zy=y+ny;
%滤波50次
for m=1:num
    z=2:1;
    xks(1)=zx(1);
    yks(1)=zy(1);
    xks(2)=zx(2);
    yks(2)=zy(2);
    rex(m,1)=xks(1);
    rey(m,1)=yks(1);
    rex(m,2)=xks(2);
    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:N
        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);
        rex(m,r)=xks(r);
        rey(m,r)=yks(r);
    end
end
ex=0; ey=0;
eqx=0; eqy=0;
ey1=0;
ex1=N:1; ey1=N:1;
rex1=0;rey1=0;
rex2=N:1;rey2=N:1;
qx=N:1;qy=N:1;
%计算滤波的均值
for i=1:N
    for j=1:num
        rex1=rex1+rex(j,i);
        rey1=rey1+rey(j,i);
    end
    rex2(i)=rex1/num;
    rey2(i)=rey1/num;
    rex1=0;
    rey1=0;
end
%计算滤波误差的均值,标准差
for i=1:N
    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; ey=0; eqy=0;
end
%绘图
figure(3);
plot(x,y,'k-',zx,zy,'g:',rex2,rey2,'r-.');
legend('真实轨迹','观测样本','估计轨迹');
figure(4);
plot(ex1);
legend('x方向平均误差');