filteredoutput.m

ESTIMATION OF AIRCRAFT TRAJECTORY FROM ITS MOTION USING KALMAN TRACKING FILTER

function funcout = filteredoutput(pinput)
% Initialization
meas = pinput(1:2);
xhatPrev = pinput(3:6);
PPrev = pinput(7:22);       %  Covariance matrix (zeros assumes perfect estimate)
deltat = pinput(23);
xhat = xhatPrev(:);         %  Estimate
P    = reshape(PPrev,4,4);  
% Compute Phi, Q, and R 
Phi = [1 deltat 0 0; 0 1 0 0 ; 0 0 1 deltat; 0 0 0 1];
Q =  diag([0 .005 0 .005]);
R =  diag([300^2 0.001^2]);
% Propagate the covariance matrix:
P = Phi*P*Phi' + Q;
% Propagate the track estimate::
xhat = Phi*xhat;
%  Compute observation estimates:
Rangehat = sqrt(xhat(1)^2+xhat(3)^2);
Bearinghat = atan2(xhat(3),xhat(1));
%  Compute observation vector y and linearized measurement matrix M 
yhat = 	[Rangehat
          Bearinghat];
M = [ cos(Bearinghat)          0 sin(Bearinghat)          0
   -sin(Bearinghat)/Rangehat 0 cos(Bearinghat)/Rangehat 0 ];
%   Compute residual (Estimation Error)
residual = meas - yhat;
% Compute Kalman Gain:
W = P*M'/(M*P*M'+ R);
% Update estimate
xhat = xhat + W*residual;
% Update Covariance Matrix
P = (eye(4)-W*M)*P*(eye(4)-W*M)' + W*R*W';
% Output columwise for Simulink.
funcout = [residual;xhat;P(:);deltat];