題目

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

【解】時(shí)間更新頻率為10HZ，量測(cè)更新頻率為2HZ，即時(shí)間更新5次，狀態(tài)更新1次。

代碼如下：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 功能描述：導(dǎo)彈運(yùn)動(dòng)Kalman濾波程序 % 課次：卡爾曼濾波與組合導(dǎo)航 第二次課程 % 時(shí)間：2021/5/4 % 作者：Li Lingwei %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear;clc;close all; %% 01 初始化參數(shù) T = 300; % 仿真時(shí)長(zhǎng)T_mea = 1/2; % 量測(cè)采樣時(shí)間 T_update = 1/10; % 時(shí)間更新間隔 Q = 0.05*T_update; % 過(guò)程噪聲 R = 50; % 量測(cè)噪聲 W = sqrt(Q)*randn(1,T); V = sqrt(R)*randn(1,T); P0 = diag([10^2,1^2]); % 系統(tǒng)矩陣 A = [0 -1;0 0]; % 狀態(tài)矩陣 I = eye(2); Phi = I + A*T_update; % 離散化 H = [1,0]; % 量測(cè)矩陣 Gamma = [0;1];% 初始化 nS = 2; nZ = 1; count = 1; % 用于計(jì)數(shù) 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 用模型模擬真實(shí)狀態(tài) for t = 2:TxState(:,t) = Phi*xState(:,t-1) + Gamma*W(:,t);zMea(:,t) = H*xState(:,t) + V(t); end %% 03-1 Kalman濾波（時(shí)間更新為10Hz,量測(cè)頻率為2Hz） for t = 2:T% 時(shí)間更新（時(shí)間更新為10Hz）xKF_pre(:,t) = Phi*xKF_10HZ(:,t-1);P_pre = Phi*P0*Phi' + Gamma*Q*Gamma';% 量測(cè)更新（量測(cè)頻率為2Hz）if (mod(count,T_mea/T_update)==0) % 時(shí)間更新5次，量測(cè)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; % 計(jì)數(shù)歸0elsexKF_10HZ(:,t) = xKF_pre(:,t); % 將上一拍的值傳給濾波器P0 = P_pre;count = count + 1; % 計(jì)數(shù)加1end end%% 03-2 Kalman濾波（時(shí)間更新為2Hz,量測(cè)頻率為2Hz） for t = 2:T% 時(shí)間更新（時(shí)間更新為2Hz）xKF_pre(:,t) = Phi*xKF_2HZ(:,t-1);P_pre = Phi*P0*Phi' + Gamma*Q*Gamma';% 量測(cè)更新（量測(cè)頻率為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-量測(cè)距離誤差與估計(jì)距離誤差的比較','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('時(shí)間 t/s');ylabel('誤差距離 m'); legend('量測(cè)距離誤差','估計(jì)距離誤差(10HZ)','估計(jì)距離誤差(2HZ)'); title('量測(cè)距離誤差與估計(jì)距離誤差的比較'); % 保存圖片 saveas(gcf,'01-量測(cè)距離誤差與估計(jì)距離誤差的比較.png');FigWin2=figure('position',[850 300 550 450],'Color',[0.8 0.8 0.8],...'Name','02-真實(shí)速度與估計(jì)速度的比較','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('時(shí)間 t/s');ylabel('誤差速度 m/s'); legend('估計(jì)速度(10HZ)','估計(jì)速度(2HZ)'); title('估計(jì)速度的比較'); % 保存圖片 saveas(gcf,'02-真實(shí)速度與估計(jì)速度的比較.png');FigWin3=figure('position',[300 200 550 450],'Color',[0.8 0.8 0.8],...'Name','03-導(dǎo)彈真實(shí)距離、雷達(dá)量測(cè)距離與估計(jì)距離','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('時(shí)間 t/s');ylabel('距離 m'); legend('導(dǎo)彈真實(shí)距離','雷達(dá)量測(cè)軌跡','導(dǎo)彈估計(jì)距離(10HZ)','導(dǎo)彈估計(jì)距離(2HZ)'); title('導(dǎo)彈真實(shí)距離、雷達(dá)量測(cè)距離與估計(jì)距離的比較'); 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-導(dǎo)彈真實(shí)距離、雷達(dá)量測(cè)距離與估計(jì)距離.png');FigWin4=figure('position',[850 200 550 450],'Color',[0.8 0.8 0.8],...'Name','04-導(dǎo)彈真實(shí)速度與估計(jì)速度的比較','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('時(shí)間 t/s');ylabel('速度 m/s'); legend('真實(shí)速度','估計(jì)速度(10HZ)','估計(jì)速度(2HZ)'); title('真實(shí)速度與估計(jì)速度的比較'); % 保存圖片 saveas(gcf,'04-真實(shí)速度與估計(jì)速度的比較.png');FigWin5=figure('position',[300 100 550 450],'Color',[0.8 0.8 0.8],...'Name','05-估計(jì)距離與估計(jì)速度','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估計(jì)值','2HZ估計(jì)值'); xlabel('時(shí)間 t/s');ylabel('估計(jì)距離 m'); title('距離估計(jì)值與速度估計(jì)值'); 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('時(shí)間 t/s');ylabel('估計(jì)速度 m/s'); % 保存圖片 saveas(gcf,'05-估計(jì)距離與估計(jì)速度.png');

仿真結(jié)果圖如下所示。

$???Theend???$

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。