wukalman.m

kalman滤波的程序及ppt

function kalman
clc;
clear;
T=2;                %雷达扫描周期
%*******************产生真实轨迹*******************************************
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;
%滤波初始化
z=2:1;
xks(1)=zx(1);
yks(1)=zy(1);
xks(2)=zx(2);
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))/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);
end
ex=N:1; ey=N:1;
%计算滤波误差
for i=1:N
    ex(i)=x(i)-xks(i);
    ey(i)=y(i)-yks(i);
end
%绘图
figure(3);
plot(x,y,'k-',zx,zy,'g:',xks,yks,'r-.');
legend('真实轨迹','观测样本','估计轨迹');
figure(4);
plot(ex);
legend('x方向平均误差');