仿真题7.17
现有一个在二维平面内运动的目标，它从（60000m,40000m ）处，以
（-172m/s,246m/s ）的速度出发。

在400s 的运动过程中，目标运动速率保持为300m/s ，并在56~105s,182~245s,285~314s 和348~379s 期间分别以1g,-1.5g,3g 和-2.5g(g=9.8m/2s )的转弯速率进行机动，其余时间段则进行匀速运动。

系统在两个方向的观测噪声标准差为m y x 100==σσ。

采用IMM 算法实现对该目标的跟踪，其中的模型集合由具有不同转弯速率的协同转弯模型构成。

定义状态向量由目标在各方向的位置和速度分量构成，即
()()()[]
T y x n v n y n v n x n x )()(= 在协同转弯模型中，状态转移矩阵及状态噪声输入矩阵分别为
()()()⎥⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎢⎣⎡T T T T -T -T T --T =-ωωωωωωωωωωωωωcos 0sin 0)sin(1)
cos(10)sin(0cos 0)cos(10)sin(1)1,(n n F ()⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎣⎡T T T T =-Γ2/00002/1,22n n 其中，ω为转弯速率，T 为采样周期。

模型集合由7个协同转弯模型组成，转弯速率分别为
s s s s s s s /6.5,/74.3,/87.1,/0,/87.1,/74.3,/6.57654321 ====-=-=-=ωωωωωωω。

转速0ω对应模型的系统状态噪声标准差为1.8m/2s ，其余模型的系统状态噪声标准差为2.5m/2s 。

模型初始概率为{0.03，0.03，0.03，0.92，0.03，0.03，0.03}，转移概率矩阵为
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡=0.90.1000000.10.80.10000
00.10.80.1000000.10.80.1000000.10.80.1000000.10.80.1000000.10.9π
请给出：
（1）目标的真实运动轨迹。

（2）目标在两个方向的跟踪均方根误差曲线。

（3）第4、第5和第7个模型的模型概率变化曲线。

代码如下：
clear all;
clc;
N=3000;
gv=0.0332;
trials=100;
v=randn(N,1,trials)*sqrt(gv);
a1=-2;
a2=1.4;
a3=-0.4;
a4=0.0384;
u1=zeros(N,1,trials);
for m=1:trials
for i=1:N-4
u1(i+4,1,m)=-a1*u1(i+3,1,m)-a2*u1(i+2,1,m)-a3*u1(i+1,1,m)... -a4*u1(i,1,m)+v(i+4,1,m);
end
end
N2=2000;
Jmin=0.005;
W_esti=zeros(4,N2,trials);
W=zeros(4,N2+1);
e=zeros(N2,1);
for m=1:trials
P_esti=eye(4);
for n=5:N2
P_pre=P_esti;
U(:,n,m)=[u1(n-1,1,m);u1(n-2,1,m);u1(n-3,1,m);u1(n-4,1,m)]; A=(U(:,n,m))'*P_pre*U(:,n,m)+Jmin;
K=P_pre*U(:,n,m)/A;
alpha(n,m)=u1(n,1,m)-(U(:,n,m))'*W_esti(:,n,m);
W_esti(:,n+1,m)=W_esti(:,n,m)+K*alpha(n,m);
P_esti=P_pre-K*(U(:,n,m))'*P_pre;
end
W=W+W_esti(:,:,m);
e=e+abs(alpha(:,m)).^2;
end
W=1/trials*W;
e=1/trials*e;
h=1:N2+1;
figure(1);
plot(h,W);
xlabel('迭代次数'); ylabel('权值'); gtext('w1');
gtext('w2');
gtext('w3');
gtext('w4');
figure(2);
plot(1:N2,e); xlabel('迭代次数'); ylabel('MSE');
仿真结果：。