我们可设飞机轨迹为z=t.^2+10*t
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('误差');
转载请注明出处