kalman_filter.m

MATLAB Simulations for Radar Systems Design

function [residual, estimate] = kalman_filter(npts, T, X0, inp, R, nvar)
N = npts;
rn=1;
% read the intial estimate for the state vector
X = X0;
% it is assumed that the measurement vector H=[1,0,0]
% this is the state noise variance
VAR = nvar;
% setup the initial value for the prediction covariance.
S = [1. 1. 1.; 1. 1. 1.; 1. 1. 1.];
% setup the transition matrix PHI
PHI = [1. T (T^2)/2.; 0. 1. T; 0. 0. 1.];
% setup the state noise covariance matrix
Q(1,1) = (VAR * (T^5)) / 20.;
Q(1,2) = (VAR * (T^4)) / 8.;
Q(1,3) = (VAR * (T^3)) / 6.;
Q(2,1) = Q(1,2);
Q(2,2) = (VAR * (T^3)) / 3.;
Q(2,3) = (VAR * (T^2)) / 2.;
Q(3,1) = Q(1,3);
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