trace.m

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

function R=Trace(X)
%@project:飞行器跟踪模拟
%@author:fantasy
%@date:2006.5.10
%@parameter:
%    X:观测数据
%    R:输出坐标

%观测时间间隔
global T;

%观测矩阵
H=[1,0,0,0,0;...
   0,1,0,0,0];

%位移测量误差
var_rx=100;
var_ry=100;
var_rx2=var_rx^2;
var_ry2=var_ry^2;

%观测噪声协方差矩阵
C=[var_rx2,0;...
   0,var_ry2];

%驱动噪声协方差矩阵
var_v=30;
var_a=1;
var_v2=var_v^2;
var_a2=var_a^2;

Q=zeros(5,5);
Q(4,4)=var_v2;
Q(5,5)=var_a2;

%初始状态
s0=[-10000,2000,0,300,0]';

%Kalman滤波跟踪
N=size(X,2);%观测数据长度
s=s0;
a=@traverse;
M=Q;
Xplus=[];%修正后的航迹
for icurrent=1:N
    [s,M]=Karlman(s,M,X(:,icurrent),a,Q,C,H);
    Xplus=[Xplus;(s(1:2))'];
end

%可视化数据
% plot(X(1,:),X(2,:),'r.');
% axis('equal');
% hold on;
% plot(Xplus(:,1),Xplus(:,2));

R=Xplus';


function s_estimate=traverse(s)
%状态方程
%s=[rx,ry,theta,v,a]
global T;
s_estimate=zeros(5,1);
s_estimate(1)=s(1)+s(4)*cos(s(3))*T;
s_estimate(2)=s(2)-s(4)*sin(s(3))*T;
s_estimate(3)=s(3)+(s(5)/s(4))*T;
s_estimate(4)=s(4);
s_estimate(5)=s(5);