目标跟踪

 1 T=0.05;
 2 R=0.15;
 3 N=30;
 4 m=2:31;%预测采样点
 5 n=1:40;t=n*T;
 6 I=eye(3);
 7 H=[1,0,0]; %
 8 X0=[0,0,0.2]';
 9 P0=[10,0,0;0,12,0;0,0,5];
10 Q=[1,T,T^2/2;0,1,T;0,0,1];
11 R=0.2;
12 z=t.^2+10*t;
13  
14 s=zeros(1,30);
15 v=zeros(1,30);
16 a=zeros(1,30);
17 Fore_s=zeros(1,30);
18 Fore_v=zeros(1,30);
19 Fore_a=zeros(1,30);
20 x=zeros(1,30);
21 for i=1:N
22     Fore_P=Q*P0*Q';%一步转移矩阵
23     K=Fore_P*H'*inv(H*Fore_P*H'+R);%滤波增益矩阵
24     Pi=(I-K*H)*Fore_P;%滤波均方差矩阵
25     Xi=Q*X0+K*(z(i)-H*Q*X0);%状态滤波值 
26     Fore_Xi=Q*Xi;%一步预测值
27     x(i)=Xi(1,1);
28     X0=Xi;
29     P0=Pi;
30     s(i)=Pi(1,1);
31     v(i)=Pi(2,2);
32     a(i)=Pi(3,3);
33     Fore_s(i)=Fore_P(1,1);
34     Fore_v(i)=Fore_P(2,2);
35     Fore_a(i)=Fore_P(3,3);
36 end
37 figure(1)
38 plot(t,z,'m','LineWidth',2);
39 hold on;
40 plot(t(2:31),z(2:31),'*');
41 grid on;
42 title('真实运动估计和观测点');
43 legend('真实轨迹','观测点');
44 xlabel('时间');
45 ylabel('位置');
46  
47 figure(2)
48 plot(t,z,'m','LineWidth',2);
49 hold on;
50 plot(t(2:31),x,'g','LineWidth',2);
51 grid on;
52 title('真实轨迹和估计轨迹');
53 legend('真实轨迹','估计轨迹');
54 xlabel('时间');
55 ylabel('位置');
56  
57 figure(3)
58 plot(m,s,'m','LineWidth',2);
59 hold on;
60 plot(m,Fore_s,'g','LineWidth',2);
61 grid on;
62 title('预测位置误差和更新位置误差');
63 legend('预测位置误差','更新位置误差');
64 xlabel('采样点m');
65 ylabel('误差');
66  
67 figure(4)
68 plot(m,v,'m','LineWidth',2);
69 hold on;
70 plot(m,Fore_v,'g','LineWidth',2);
71 grid on;
72 title('预测速度误差和更新速度误差');
73 legend('预测速度误差','更新速度误差');
74 xlabel('采样点m');
75 ylabel('误差');
76  
77 figure(5)
78 plot(m,a,'m','LineWidth',2);
79 hold on;
80 plot(m,Fore_a,'g','LineWidth',2);
81 grid on;
82 title('预测加速度误差和更新加速度误差');
83 legend('预测加速度误差','更新加速度误差');
84 xlabel('采样点m');
85 ylabel('误差');