題目

巡航導彈沿直線飛向目標，目標處設有一監視雷達，雷達對導彈的距離進行觀測。
假設：（1）導彈初始距離$100km$，速度約為$300m/s$，基本勻速飛行，但受空氣擾動影響，擾動加速度為零均值白噪聲，方差強度$q=0.05m^2/s^3$ ；（2）雷達觀測頻率$2Hz$，觀測誤差為零均值白噪聲，均方差為$50m$；（3）時間更新頻率10Hz。對比該結果與時間/量測更新頻率均為2Hz的差別。
試使用卡爾曼濾波（Kalman filter）完成導彈的軌跡跟蹤。

【解】時間更新頻率為10HZ，量測更新頻率為2HZ，即時間更新5次，狀態更新1次。

代碼如下：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 功能描述：導彈運動Kalman濾波程序 % 課次：卡爾曼濾波與組合導航 第二次課程 % 時間：2021/5/4 % 作者：Li Lingwei %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear;clc;close all; %% 01 初始化參數 T = 300; % 仿真時長T_mea = 1/2; % 量測采樣時間 T_update = 1/10; % 時間更新間隔 Q = 0.05*T_update; % 過程噪聲 R = 50; % 量測噪聲 W = sqrt(Q)*randn(1,T); V = sqrt(R)*randn(1,T); P0 = diag([10^2,1^2]); % 系統矩陣 A = [0 -1;0 0]; % 狀態矩陣 I = eye(2); Phi = I + A*T_update; % 離散化 H = [1,0]; % 量測矩陣 Gamma = [0;1];% 初始化 nS = 2; nZ = 1; count = 1; % 用于計數 xState = zeros(nS,T); zMea = zeros(nZ,T); xKF_10HZ = zeros(nS,T); xKF_2HZ = zeros(nS,T); xKF_pre = zeros(nS,T); xState(:,1) = [100000;300]; zMea(:,1) = H*xState(:,1); xKF_10HZ(:,1) = xState(:,1); xKF_2HZ(:,1) = xState(:,1); %% 02 用模型模擬真實狀態 for t = 2:TxState(:,t) = Phi*xState(:,t-1) + Gamma*W(:,t);zMea(:,t) = H*xState(:,t) + V(t); end %% 03-1 Kalman濾波（時間更新為10Hz,量測頻率為2Hz） for t = 2:T% 時間更新（時間更新為10Hz）xKF_pre(:,t) = Phi*xKF_10HZ(:,t-1);P_pre = Phi*P0*Phi' + Gamma*Q*Gamma';% 量測更新（量測頻率為2Hz）if (mod(count,T_mea/T_update)==0) % 時間更新5次，量測1次K = P_pre*H'*pinv(H*P_pre*H'+R);xKF_10HZ(:,t) = xKF_pre(:,t) + K*(zMea(:,t)-H*xKF_pre(:,t));P0 = (I-K*H)*P_pre;count = 1; % 計數歸0elsexKF_10HZ(:,t) = xKF_pre(:,t); % 將上一拍的值傳給濾波器P0 = P_pre;count = count + 1; % 計數加1end end%% 03-2 Kalman濾波（時間更新為2Hz,量測頻率為2Hz） for t = 2:T% 時間更新（時間更新為2Hz）xKF_pre(:,t) = Phi*xKF_2HZ(:,t-1);P_pre = Phi*P0*Phi' + Gamma*Q*Gamma';% 量測更新（量測頻率為2Hz）K = P_pre*H'*pinv(H*P_pre*H'+R);xKF_2HZ(:,t) = xKF_pre(:,t) + K*(zMea(:,t)-H*xKF_pre(:,t));P0 = (I-K*H)*P_pre; end%% 04 畫圖 tPlot = 1:T; FigWin1=figure('position',[300 300 550 450],'Color',[0.8 0.8 0.8],...'Name','01-量測距離誤差與估計距離誤差的比較','NumberTitle','off');hold on;box on; plot(tPlot,xState(1,:)-zMea(1,:),'-b','LineWidth',1.5);hold on; plot(tPlot,xState(1,:)-xKF_10HZ(1,:),'-r','LineWidth',1.5);hold on; plot(tPlot,xState(1,:)-xKF_2HZ(1,:),'-g','LineWidth',1.5);hold on; xlabel('時間 t/s');ylabel('誤差距離 m'); legend('量測距離誤差','估計距離誤差(10HZ)','估計距離誤差(2HZ)'); title('量測距離誤差與估計距離誤差的比較'); % 保存圖片 saveas(gcf,'01-量測距離誤差與估計距離誤差的比較.png');FigWin2=figure('position',[850 300 550 450],'Color',[0.8 0.8 0.8],...'Name','02-真實速度與估計速度的比較','NumberTitle','off');hold on;box on; plot(tPlot,xState(2,:)-xKF_10HZ(2,:),'-r','LineWidth',1.5);hold on; plot(tPlot,xState(2,:)-xKF_2HZ(2,:),'-g','LineWidth',1.5);hold on; xlabel('時間 t/s');ylabel('誤差速度 m/s'); legend('估計速度(10HZ)','估計速度(2HZ)'); title('估計速度的比較'); % 保存圖片 saveas(gcf,'02-真實速度與估計速度的比較.png');FigWin3=figure('position',[300 200 550 450],'Color',[0.8 0.8 0.8],...'Name','03-導彈真實距離、雷達量測距離與估計距離','NumberTitle','off');hold on;box on; plot(tPlot,xState(1,:),'-k','LineWidth',1.5);hold on; plot(tPlot,zMea(1,:),'-b','LineWidth',1.5);hold on; plot(tPlot,xKF_10HZ(1,:),'-r','LineWidth',1.5);hold on; plot(tPlot,xKF_2HZ(1,:),'-g','LineWidth',1.5);hold on; xlabel('時間 t/s');ylabel('距離 m'); legend('導彈真實距離','雷達量測軌跡','導彈估計距離(10HZ)','導彈估計距離(2HZ)'); title('導彈真實距離、雷達量測距離與估計距離的比較'); axes('position',[0.25,0.25,0.25,0.25]); hold on; plot(tPlot,xState(1,:),'-k','LineWidth',1.5);hold on; plot(tPlot,zMea(1,:),'-b','LineWidth',1.5);hold on; plot(tPlot,xKF_10HZ(1,:),'-r','LineWidth',1.5);hold on; plot(tPlot,xKF_2HZ(1,:),'-g','LineWidth',1.5);hold on; xlim([T/2,T/2+0.5]); % 保存圖片 saveas(gcf,'03-導彈真實距離、雷達量測距離與估計距離.png');FigWin4=figure('position',[850 200 550 450],'Color',[0.8 0.8 0.8],...'Name','04-導彈真實速度與估計速度的比較','NumberTitle','off');hold on;box on; plot(tPlot,xState(2,:),'-b','LineWidth',1.5);hold on; plot(tPlot,xKF_10HZ(2,:),'-r','LineWidth',1.5);hold on; plot(tPlot,xKF_2HZ(2,:),'-g','LineWidth',1.5);hold on; xlabel('時間 t/s');ylabel('速度 m/s'); legend('真實速度','估計速度(10HZ)','估計速度(2HZ)'); title('真實速度與估計速度的比較'); % 保存圖片 saveas(gcf,'04-真實速度與估計速度的比較.png');FigWin5=figure('position',[300 100 550 450],'Color',[0.8 0.8 0.8],...'Name','05-估計距離與估計速度','NumberTitle','off');hold on;box on; subplot(211); plot(tPlot,xKF_10HZ(1,:),'-r','LineWidth',1.5);hold on; plot(tPlot,xKF_2HZ(1,:),'-b','LineWidth',1.5);hold on; legend('10HZ估計值','2HZ估計值'); xlabel('時間 t/s');ylabel('估計距離 m'); title('距離估計值與速度估計值'); subplot(212); plot(tPlot,xKF_10HZ(2,:),'-r','LineWidth',1.5);hold on; plot(tPlot,xKF_2HZ(2,:),'-b','LineWidth',1.5);hold on; xlabel('時間 t/s');ylabel('估計速度 m/s'); % 保存圖片 saveas(gcf,'05-估計距離與估計速度.png');

仿真結果圖如下所示。

$???Theend???$

總結

以上是生活随笔為你收集整理的用卡尔曼滤波器跟踪导弹（量测更新频率与时间更新频率不相等）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。