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

for loop = 1 : size(trajectory,2)
x = trajectory(1,loop);
y = trajectory(2,loop);
z = trajectory(3,loop);
azimuth_corrupted = atan2(y,x) + sigmaaz*randn(1) ;
elevation_corrupted = atan2(z, sqrt(x^2+y^2)) + sigmael*randn(1) ;
range_corrupted = sqrt(x^2+y^2+z^2) + sigmarange*randn(1) ;
x_corrupted =
range_corrupted*cos(elevation_corrupted)*cos(azimuth_corrupted) ;
y_corrupted =
range_corrupted*cos(elevation_corrupted)*sin(azimuth_corrupted) ;
z_corrupted = range_corrupted*sin(elevation_corrupted) ;
noisytraj(:,loop) = [x_corrupted ; y_corrupted; z_corrupted ] ;
end % next loop
Listing 9.8. MATLAB Function Ðkalfilt.mÑ
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
%