Target Tracking - MATLAB Simulations for Radar Systems Design - page 465

% 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
% project ahead
xhatminus_next = phi*xhat ;
Pminus_next = phi*P*phi' + Q ;
xhatminus = xhatminus_next ;
Pminus = Pminus_next ;
end