%% Create 3D Constant Acceleration EKF from Spherical Measurement
% Initialize a 3D constant-acceleration extended Kalman filter from an initial detection report made from an initial measurement in spherical coordinates. If you want to use spherical coordinates, then you must supply a measurement parameter structure as part of the detection report with the Frame field set to 'spherical'. Set the azimuth angle of the target to 45^{\circ}, the elevation to 22^{\circ}, the range to 1000 meters, and the range rate to -4.0 m/s.

frame = 'spherical';
sensorpos = [25,-40,-10].';
sensorvel = [0;5;0];
laxes = eye(3);

%
% Create the measurement parameters structure. Set 'HasVelocity' and 'HasElevation' to true. Then, the measurement vector consists of azimuth, elevation, range, and range rate.

measparms = struct('Frame',frame,'OriginPosition',sensorpos, ...
    'OriginVelocity',sensorvel,'Orientation',laxes,'HasVelocity',true, ...
    'HasElevation',true);
meas = [45;22;1000;-4];
measnoise = diag([3.0,2.5,2,1.0].^2);
detection = objectDetection(0,meas,'MeasurementNoise', ...
    measnoise,'MeasurementParameters',measparms)
filter = initcaekf(detection);

%
% Display the state vector.

disp(filter.State)

% Copyright 2017 The MathWorks, Inc.
%