# MATLAB Simulations for Radar Systems Design

### Bassem Mahafza (view profile)

• 1 file
• 4.36765

11 Sep 2003 (Updated )

MATLAB Simulations for Radar Systems Design

kalfilt(trajectory, x0, P0, phi, R, Q )
```function [filtered, residuals , covariances, kalmgains] = kalfilt(trajectory, x0, P0, phi, R, Q )
% kalfilt.m
% by David J. Hall
% for Bassem Mahafza
% 10 June 2003
% 11:46
%
% USAGE: [filtered, residuals , covariances, kalmgains] = kalfilt(trajectory, x0, P0, phi, R, Q )
%
% INPUTS
%
% name         dimension                    explanation                                  units
%------        ------                       ---------------                              -------
% trajectory   NUMMEASUREMENTS X NUMPOINTS  trajectory in radar reference coords         [m;m;m]
% x0           NUMSTATES X 1                initial estimate of state vector             m, m/s
% P0           NUMSTATES X NUMSTATES        initial estimate of covariance matrix        m, m/s
% phi          NUMSTATES X NUMSTATES        state transition matrix                      -
% R            NUMMEASUREMENTS X NUMMEASUREMENTS   measurement error covariance matrix   m
% Q            NUMSTATES X NUMSTATES        state error covariance matrix                m, m/s
%
% OUTPUTS
%
% name         dimension                    explanation                                  units
%------        ------                       ---------------                              -------
% filtered     NUMSTATES X NUMPOINTS        filtered trajectory x,y,z pos, vel    [m; m/s; m; m/s; m; m/s]
% residuals    NUMSTATES X NUMPOINTS        residuals of filtering                [m;m;m]
% covariances  NUMSTATES X NUMPOINTS        diagonal of covariance matrix         [m;m;m]
% kalmgains    (NUMSTATES X NUMMEASUREMENTS)
%                 X NUMPOINTS               Kalman gain matrix                    -
%

NUMSTATES = 6 ;
NUMMEASUREMENTS = 3 ;
NUMPOINTS = size(trajectory, 2) ;

% initialize output matrices
filtered = zeros(NUMSTATES, NUMPOINTS) ;
residuals = zeros(NUMSTATES, NUMPOINTS) ;
covariances = zeros(NUMSTATES, NUMPOINTS) ;
kalmgains = zeros(NUMSTATES*NUMMEASUREMENTS, NUMPOINTS) ;

% set matrix relating measurements to states
H = [1 0 0 0 0 0 ; 0 0 1 0 0 0 ; 0 0 0 0 1 0];

xhatminus = x0 ;
Pminus = P0 ;

for loop = 1: NUMPOINTS

% compute the Kalman gain
K = Pminus*H'*inv(H*Pminus*H' + R) ;
kalmgains(:,loop) = reshape(K, NUMSTATES*NUMMEASUREMENTS, 1) ;

% update the estimate with the measurement z
z = trajectory(:,loop) ;
xhat = xhatminus + K*(z - H*xhatminus) ;
filtered(:,loop) = xhat ;
residuals(:,loop) = xhat - xhatminus ;

% update the error covariance for the updated estimate
P = ( eye(NUMSTATES, NUMSTATES) - K*H)*Pminus ;
covariances(:,loop) = diag(P) ;  % only save diagonal of covariance matrix

xhatminus_next = phi*xhat ;
Pminus_next = phi*P*phi' + Q ;

xhatminus = xhatminus_next ;
Pminus = Pminus_next ;

end % next loop

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
```