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

Q(3,2) = Q(2,3);
Q(3,3) = VAR * T;
while rn < N ;
%use the transition matrix to predict the next state
XN = PHI * X;
% Perform error covariance extrapolation
S = PHI * S * PHI' + Q;
% compute the Kalman gains
ak(1) = S(1,1) / (S(1,1) + R);
ak(2) = S(1,2) / (S(1,1) + R);
ak(3) = S(1,3) / (S(1,1) + R);
%perform state estimate update:
error = inp(rn) + normrnd(0,R) - XN(1);
residual(rn) = error;
tmp1 = ak(1) * error;
tmp2 = ak(2) * error;
tmp3 = ak(3) * error;
X(1) = XN(1) + tmp1;
X(2) = XN(2) + tmp2;
X(3) = XN(3) + tmp3;
estimate(rn) = X(1);
% update the error covariance
S(1,1) = S(1,1) * (1. -ak(1));
S(1,2) = S(1,2) * (1. -ak(1));
S(1,3) = S(1,3) * (1. -ak(1));
S(2,1) = S(1,2);
S(2,2) = -ak(2) * S(1,2) + S(2,2);
S(2,3) = -ak(2) * S(1,3) + S(2,3);
S(3,1) = S(1,3);
S(3,3) = -ak(3) * S(1,3) + S(3,3);
rn = rn + 1.;
end
Listing 9.5. MATLAB Program Ðfig9_28.mÑ
clear all
npts = 2000;
del = 1/2000;
t = 0:del:1;
inp = (1+.2 .* t + .1 .*t.^2) + cos(2. * pi * 2.5 .* t);
X0 = [1,.1,.01]';
% it is assumed that the measurement vector H=[1,0,0]
% this is the update interval in seconds
T = 1.;