kalman3.m

kalman滤波用于目标二维运动情况下的蒙特卡罗法仿真跟踪滤波器

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%目标水平运动
%初始位置 x0=-2000m y0=500m
% V=10m/s  扫描周期T=5s deltaa=0 deltax=100m
%蒙特卡咯法仿真跟踪滤波器
%仿真次数100
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;
T=5;    %雷达扫描周期
num=100  %滤波次数
%************************************轨迹*************
N=800/T;
%目标位置向量x y
x=zeros(N,1);y=zeros(N,1);
%目标速度向量 vx vy
vx=zeros(N,1);vy=zeros(N,1);
x(1)=-2000;
y(1)=500;
vx=10; vy=0;
%加速度向量ax ay
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 ny
nx=zeros(N,1);ny=zeros(N,1);
nx=100*randn(N,1);
ny=100*randn(N,1);
%混合信号向量zx zy
zx=x+nx;zy=y+ny;  
%**************************滤波100次***********************
for m=1:num
    %测量值z=[zx;zy]
    z=2:1;
    %估计出的目标轨迹xks yks
    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*var/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 Start
    for r=3:N
        z=[zx(r);zy(r)];
        %Kalman滤波方程
        xk1=o*xk;     %一步预测方程
        perr1=o*perr*o';
        k=perr1*h'*inv(h*perr1*h'+q); %滤波增益方程
        xk=xk1+k*(z-h*xk1);   %状态估计计算方程
        perr=(eye(4)-k*h)*perr1;  %估计均方误差方程
        
        %分别提取出x y 的位置和速度分量
        xks(r)=xk(1,1);
        yks(r)=xk(3,1);
        vkxs(r)=xk(2,1);
        vkys(r)=xk(4,1);
        
        %目标的位置量
        xkls(r)=xk(1,1);
        ykls(r)=xk(3,1);
        
        perr11(r)=perr(1,1);
        perr12(r)=perr(1,2);
        perr22(r)=perr(2,2);
        %记录第m次的目标位置估计量
        rex(m,r)=xks(r);
        rey(m,r)=yks(r);
    end
end 
%误差信号ex ey
ex=0;ey=0;
eqx=0;eqy=0;
ey1=0;
ex1=N:1;ey1=N:1;
%计算误差均值
for i=1:N
    for j=1:num
        ex=ex+x(i)-rex(j,i);
        ey=ey+y(i)-rey(j,i);
    end 
    ex1(i)=ex/num;
    ey1(i)=ey/num;
    ex=0;eqx=0;ey=0;eqy=0;
end 
%绘图
figure(1);
plot(x,y,'K-',zx,zy,'g:',xks,yks,'r-');
legend('真实轨迹','观测样本','估计轨迹');
figure(2);
plot(ex1);
legend('X方向平均误差');
figure(3);
plot(ey1);
legend('Y方向平均误差');