(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]an 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 ,020,0,1,,/0,0,0,1,00,0,0,0,1v T T v T T a a A T T vs v θθθθ⎡⎤-⎢⎥⎢⎥⎢⎥--⎢⎥⎢⎥∂⎢⎥==-∂⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦假设[]v u n 和[]a u n 不相关，方差分别为2v σ、2aσ，因此得驱动噪声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 不相关且方差分别为2x σ和2y σ观测噪声22,00,x y C σσ⎡⎤=⎢⎥⎢⎥⎣⎦总结上述式子，得到这个问题的扩展卡尔曼滤波方程为ˆˆ[|1]([1|1])sn 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])sn 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]];endX_filter=zeros(size(X));%滤波后数据X_mean=X_filter;%蒙特卡洛平均数据Error_var=zeros(size(X));M=10;%蒙特卡洛仿真次数for iCount=1:MX_filter=Trace(X);X_mean=X_mean+X_filter;Error_var=Error_var+(X_ideal-X_filter).^2;endX_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<=t1x=r(1)+v*t;y=r(2);elseif t>t1&&t<=t2angel=(t-t1)*w;x=D/2*sin(angel);y=-D*(sin(angel/2))^2; elsex=-v*(t-t2);y=-D;endfunction 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;。