机动目标跟踪_张泽兵_05040056

(1) 算法描述

在该问题中，机动目标经历三个阶段：初始匀速直线阶段、匀速圆周运动阶段、返回匀速直线阶段。在此过程中线速度大小v 保持不变。

图1

如图1所示：θ为轨迹切线与横轴正向夹角。在初始匀速阶段和返回匀速直线阶段θ分别为0，π。在匀速圆周运动阶段θ从0到π均匀变化。 由

2/mv r ma =及/w v r =得

/w a v =（w 为角速度）

所以容易得到：

/wt vt r θ==

cos x v v θ=

sin y v v θ=-

状态变量

[,,,,]T x y s r r v a θ=

状态方程为：

[][1][1]cos x x x x r n r n v T r n v T θ=-+=-+

[][1][1]sin y y y x r n r n v T r n v T θ=-+=--

[][1][1]a

n n wT n T v

θθθ=-+=-+

[][1][]v v n v n u n =-+

[][1][]a a n a n u n =-+

即

[]([1])[]s n a s n u n =-+,其中[][0,0,0,[],[]]'v a u n u n u n =

所以状态转换矩阵为

211,0,sin ,cos ,0210,1,cos ,sin ,02

0,0,1,,/0,0,0,1,00,0,0,0,1v T T v T T a a A T T v

s v θθθθ??-????

??--???????==-???

????????????

假设[]v u n 和[]a u n 不相关，方差分别为2

v σ、2

a

σ，因此得驱动噪

声

220,0,0,0,00,0,0,0,00,0,0,,00,0,0,0,v a Q σσ????

?

?=????????

观测矢量

[][][][]x y r n x n w n r n ??=+????

其中[][][]x y u n w n u n ??

=????

所以观测矩阵

1,0,0,0,00,1,0,0,0H ??

=????

，

假设[]x u n 、[]y u n 不相关且方差分别为2

x σ和2

y σ

观测噪声

22,00,x y C σσ??=??????

总结上述式子，得到这个问题的扩展卡尔曼滤波方程为

??[|1]([1|1])s

n n a s n n -=--

[|1][1|1]T M n n AM n n A Q -=--+ 1[][|1]([|1])T T K n M n n H C HM n n H -=-+-

???[|][|1][]([][|1])s

n n s n n K n x n Hs n n =-+-- [|]([])[|1]M n n I K n H M n n =--

(2) 滤波初始化

由问题描述，x σ=y σ=100，

初始位置为[-20000;0] ,也可以有一定的偏离，经过一段时间后依然能够收敛。不妨取[-10000;2000].初始速度v=300m/s ，θ=0，a=0 所以初始状态为

[1][10000,0,0,300,0]T s -=-

对驱动噪声，取v σ=30；a σ=2； (3) 仿真分析

采用MATLAB 编写仿真程序，利用蒙特卡罗方法对跟踪滤波器进行仿真分析，次数为10次。以下给出仿真图和结果分析。 滤波轨迹和滤波均值轨迹： 见图2。

图2 而当M=1时

可以看出性能比之M=10时较差，尤其表现在拐弯处。X、Y方向滤波估计误差均值及误差标准差：

见图3、4。

图3

图4 附Matlab源代码：

function main()

%%@project:飞行器跟踪模拟

%@author:fantasy

%@date:2006.5.10

%@descripition:主函数

%产生观测数据

total=3*60;%总的时间长度

global T;%采样周期

T=1;

N=total/T;%数据长度

a=20;

var_rx=100;

var_ry=100;

X=[];%观测数据

X_ideal=[];%理想数据

for i=1:N

[rx,ry]=track(i*T,20);

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

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=5;

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);

function [s,M]=Karlman(s_forward,M_forward,X,a,Q,C,H) %卡尔曼滤波

%@author:fantasy

%@date:2006.5.15

%参数说明

% X--观测数据矢量

% A--状态矩阵

% Q--驱动噪声协方差

% C--观测噪声协方差

% h--观测方程句柄

% s--输出数据矢量

% s_foward--前次输出矢量

% M--前次预测矩阵

global T;

%预测

s=feval(a,s_forward);

%状态转换矩阵

% A=[1,0,-s(4)/2*sin(s(3)/2)*T,cos(s(3)/2)*T,0;... % 0,1,-s(4)/2*cos(s(3)/2)*T,-sin(s(3)/2)*T,0;... % 0,0,1,-s(5)*T/(s(4))^2,T/s(4);...

% 0,0,0,1,0;...

% 0,0,0,0,1];

A=[1,0,-s(4)*sin(s(3))*T,cos(s(3))*T,0;...

0,1,-s(4)*cos(s(3))*T,-sin(s(3))*T,0;...

0,0,1,-s(5)*T/(s(4))^2,T/s(4);...

0,0,0,1,0;...

0,0,0,0,1];

%最小预测MSE矩阵

M=M_forward;

M=A*M*A'+Q;

%卡尔曼增益矩阵

K=M*H'*inv(C+H*M*H');

%修正

s=s+K*(X-H*s);

%最小MSE矩阵

M=M-K*H*M;