main.m

利用Kalman Filter进行机动目标跟踪的例子

function main()
%%@project:飞行器跟踪模拟
%@author:fantasy
%@date:2006.5.10
%@descripition:主函数

%产生观测数据
total=3*60;%总的时间长度
global T;%采样周期
T=1;
N=total/T;%数据长度
a=20;
var_rx=400;
var_ry=400;

X=[];%观测数据
X_ideal=[];%理想数据

for i=1:N
    [rx,ry]=track(i*T,a);
    X_ideal=[X_ideal,[rx;ry]];
    rx=rx+var_rx*randn(1,1);
    ry=ry+var_ry*rand(1,1);
    X=[X,[rx;ry]];
end

X_filter=zeros(size(X));%滤波后数据
X_mean=X_filter;%蒙特卡洛平均数据
Error_var=zeros(size(X));
M=10;%蒙特卡洛仿真次数

for iCount=1:M
    X_filter=Trace(X);
    X_mean=X_mean+X_filter;
    Error_var=Error_var+(X_ideal-X_filter).^2;
end

X_mean=X_mean/M;
Error_var=Error_var/M;
Error_mean=X_ideal-X_mean;%误差均值
Error_var=sqrt(Error_var-Error_mean.^2);


plot(X_ideal(1,:),X_ideal(2,:),X(1,:),X(2,:),X_mean(1,:),X_mean(2,:));
axis equal;
legend('理想轨迹','观测轨迹','滤波轨迹');

figure;
k=1:N;
subplot(2,1,1),plot(k,Error_mean(1,:));title('x方向误差均值');xlabel('采样次数'),ylabel('误差均值(米)');
subplot(2,1,2),plot(k,Error_var(1,:));title('x方向误差标准值');xlabel('采样次数'),ylabel('误差标准值值(米)');

figure;
subplot(2,1,1),plot(k,Error_mean(2,:));title('y方向误差均值');xlabel('采样次数'),ylabel('误差均值(米)');
subplot(2,1,2),plot(k,Error_var(2,:));title('y方向误差标准值');xlabel('采样次数'),ylabel('误差标准值值(米)');

%@subfunction    
%理想航迹方程
function [x,y]=track(t,a)
%parameter:
%   t:时间
%   x:横轴位移
%   y:纵轴位移
%   a:转弯处加速度 
%   r:初始位置
%   v:初始速度

r=[-20000,0]';
v=300;
w=a/v;%角速度
t1=-r(1)/v;
t2=t1+pi/w;
D=v^2/a*2;%圆周运动直径
if t<=0
   [x,y]=r0';
elseif t>0&&t<=t1
    x=r(1)+v*t;
    y=r(2);
elseif t>t1&&t<=t2
    angel=(t-t1)*w;
    x=D/2*sin(angel);
    y=-D*(sin(angel/2))^2;
else
    x=-v*(t-t2);
    y=-D;
end