机动目标跟踪_张泽兵_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('误差标准值值(米)');