基于卡尔曼滤波算法的雷达追踪算法.m

%
% Example 5.8
%
close all;
clear all;
clf;
disp（‘Covariance analysis of radar tracking problem‘）;
disp（‘given as Example 5.8 in‘）;
disp（‘M. S. Grewal and A. P. Andrews‘）;
disp（‘Kalman Filtering: Theory and Practice Using MATLAB‘）;
disp（‘4th Edition Wiley 2014.‘）;
disp（‘ ‘）;
disp（‘Plots histories of six mean squared state‘）;
disp（‘uncertainties and six magnitudes of Kalman gains‘）;
disp（‘for intersample intervals of 5 10 and 15 seconds.‘）;
disp（‘ ‘）;
disp（‘Six state variables:‘）;
disp（‘  1. Range to object being tracked.‘）;
disp（‘  2. Range rate of object being tracked.‘）;
disp（‘  3. object range maneuvering noise （pseudo state）.‘）;
disp（‘  4. Bearing to object being tracked.‘）;
disp（‘  5. Bearing rate of object being tracked.‘）;
disp（‘  6. object bearing maneuvering noise （pseudo state）.‘）;
disp（‘Pseudo states are used for modeling correlated noise.‘）;
%
sigma1sq = （103/3）^2;
sigma2sq = 1.3E-8;
sigmarsq = （1000）^2;
sigmatsq = （.017）^2;
rho      = 0.5;
%
% State Transition matrix （the part not depending on T）
%
Phi      = eye（6）;
Phi（23） = 1;
Phi（56） = 1;
Phi（33） = rho;
Phi（66） = rho;
Q        = zeros（6）;
Q（33）   = sigma1sq;
Q（66）   = sigma2sq;
R        = zeros（2）;
R（11）   = sigmarsq;
R（22）   = sigmatsq;
H        = zeros（26）;
H（11）   = 1;
H（24）   = 1;
%
% arrays for saving data to be plotted
%
t        = zeros（332）; % time （for 3 plots）
rcov     = zeros（332）; % Range covariance
rrcov    = zeros（332）; % Range Rate covariance
bcov     = zeros（332）; % Bearing covariance
brcov    = zeros（332）; % Bearing Rate covariance
rrncov   = zeros（332）; % Range Rate Noise covariance
brncov   = zeros（332）; % Bearing Rate Noise covariance
rkg      = zeros（332）; % Range Kalman gain
rrkg     = zeros（332）; % Range Rate Kalman gain
bkg      = zeros（332）; % Bearing Kalman gain
brkg     = zeros（332）; % Bearing Rate Kalman gain
rrnkg    = zeros（332）; % Range Rate Noise Kalman gain
brnkg    = zeros（332）; % Bearing Rate Noise Kalman gain
N=0;
   for T = 5:5:15
   N=N+1;
   disp（[‘Simulating tracking at ‘num2str（T）‘ second intervals.‘]）;
   Phi（12） = T;
   Phi（45） = T;
   P        = zeros（6）;
   P（11）   = sigmarsq;
   P（12）   = sigmarsq/T;
   P（21）   = P（12）;
   P（22）   = 2*sigmarsq/T^2 + sigma1sq;
   P（33）   = sigma1sq;
   P（44）   = sigmatsq;
   P（45）   = sigmatsq/T;
   P（54）   = P（45）;
   P（55）   = 2*sigmatsq/T^2 + sigma2sq;
   P（66）   = sigma2sq;
      for cycle=0:15
%
% Save a priori values
%
      prior      = 2*cycle+1;
      t（Nprior） = T*cycle;
      K          = P*H‘/（H*P*H‘+R）;
      rcov（Nprior）   = P（11）; % Range covariance
      rrcov（Nprior）  = P（22）; % Range Rate covariance
      bcov（Nprior）   = P（44）; % Bearing covariance
      brcov（Nprior）  = P（55）; % Bearing Rate covariance
      rrncov（Nprior） = P（33）; % Range Rate Noise covariance
      brncov（Nprior） = P（66）; % Bearing Rate Noise covariance
      rkg