Why is the covariance so large for angle-only tracking?

I have implemented the angle only tracking scenario similar to the one found here.

It gives a good mean estimate, but the final covariance is many orders of magnitude larger, corresponding to a thin ellipse along the direction of detection. I expected it to shrink over time since the sensor is moving with respect to the target. Am I doing something wrong? The example creates a plot of the results, and shows a very large ellipse. It loads the data from the example in the link.

%% Passive Ranging Using a Single Maneuvering Sensor, modified

% Setup scenario
scene = trackingScenario;
scene.StopTime = 75; % Scene duration (seconds)
scene.UpdateRate = 2; % Update rate (Hz)

% Create a platforms
ownship = platform(scene);
target = platform(scene);

% Define motion for ownship and target.
% The ownship is manuevering. The waypoints are defined in a MAT file.
load('PassiveSensorManeuveringTrajectory.mat','wp','time'); % loads time (1xN) and wp (Nx3)

% Use |waypointTrajectory| to mount the trajectory on the ownship
ownship.Trajectory = waypointTrajectory(wp,time,'Orientation',...
    repmat(quaternion([0 0 0],'rotvecd'),[901 1]));

% set z coordinate to same as ownship
target.Trajectory = kinematicTrajectory('Position',[7e4 2e3 -12e3],...
    'Velocity',[-500/3 -50/3 0]);

infraredSensor =  irSensor('SensorIndex',1,'ScanMode', 'No scanning');
ownship.Sensors{1} = infraredSensor;

%% Track Using an EKF in Modified Spherical Coordinates (MSC-EKF)

% Set the same random seed to compare with the same detections
rng(2015);

% store discovered tracks
positionLog = repmat(struct('TrackID',0,...
    'Position',zeros(0,3),...
    'PositionCovariance',zeros(3,3,0)),0,1);
origin=[0,0,-12000]'; % coordinate system origin

% Create a tracker using |trackerGNN| and MSC-EKF filter initialization.
tracker = trackerGNN('FilterInitializationFcn',@initMSCEKF,...
    'AssignmentThreshold',50);

% initialize measurement parameters for detections and ObjectAttributes
angle_covar=[4e-4]; % from example
mp = struct(Frame="Spherical", ...
    HasAzimuth = true,...
    HasElevation = false,...
    HasRange = false,...
    HasVelocity = false,...
    OriginPosition=origin, ...
    Orientation=eye(3),...
    IsParentToChild=true); % 

% Initialization for MSC-EKF.
prevPose = pose(ownship,'true');
lastCorrectionTime = 0;
allTracks = [];
track1_truth = []; % truth

% Advance scenario, simulate detections and track
while advance(scene)
    
    time = scene.SimulationTime;
    
    target1_xyz  = scene.Platforms{2}.Position;
    ownship_xyz = pose(ownship,'true').Position;
    track1_truth = vertcat(track1_truth,target1_xyz);

    % angle-only detection
    theta1 = atan((target1_xyz(2)-ownship_xyz(2))/(target1_xyz(1)-ownship_xyz(1)));
    azimuth1 = [180*theta1/pi];

    % Generate detections from ownship, as cell array of objectDetection objects

    detect1 = objectDetection(time, azimuth1, ...
       'MeasurementParameters',mp,'MeasurementNoise',angle_covar);
    detections = {detect1};
    
    % Update the input from the ownship i.e. it's maneuver since last
    % correction time.
    currentPose = pose(ownship,'true');
    dT = time - lastCorrectionTime;
    observerManeuver = calculateManeuver(currentPose,prevPose,dT);

    for i = 1:numel(allTracks)
        % Set the ObserverInput property using |setTrackFilterProperties|
        % function of the tracker
        setTrackFilterProperties(tracker,allTracks(i).TrackID,'ObserverInput',observerManeuver);
    end
    
    % Pass detections to tracker
    if ~isempty(detections)
        lastCorrectionTime = time;
        % Store the previous pose to calculate maneuver
        prevPose = currentPose;
        [tracks,~,allTracks] = tracker(detections,time); % tracks=confirmed

        tracks_info = size(tracks);
        if tracks_info(1)~=0
            [pos, cov] = getTrackPositionsMSC(tracks, ownship_xyz(:));
            positionLog = updatePositionLog(positionLog, tracks, pos, cov);
        end

    elseif isLocked(tracker)
        tracks = predictTracksToTime(tracker,'confirmed',time);
    end

end

% compare truth tracks with inferred tracks. Why is the covariance so
% large?
hold on
plot(track1_truth(:,1), track1_truth(:,2),'r-')
plot( positionLog(1).Position(:,1),  positionLog(1).Position(:,2),'m--')
plot_ellipse(positionLog(1))
%%----------------------- helper functions (unchanged) ---------------------------------
function positionLog = updatePositionLog(positionLog, tracks, pos, cov)

    for k = 1:numel(tracks)
        idx = find([positionLog.TrackID] == tracks(k).TrackID);
        if ~isempty(idx) % Track exists in the log
            positionLog(idx).Position = [positionLog(idx).Position;pos(k,:)];
            positionLog(idx).PositionCovariance = cat(3,positionLog(idx).PositionCovariance,cov(:,:,k));
        else % Add new track to the log
            newTrack.TrackID = tracks(k).TrackID;
            newTrack.Position = pos(k,:);
            newTrack.PositionCovariance = cov(:,:,k);
            positionLog = [positionLog;newTrack]; % expand
        end
    end
end

% *|initMSCEKF|*
% Initialize a MSC-EKF from an angle-only detection
function filter = initMSCEKF(detection)
% Use the second input of the |initcvmscekf| function to provide an
% estimate of range and standard deviation in range.
rangeEstimate = 5e4;
rangeSigma = 4e4;
filter = initcvmscekf(detection,[rangeEstimate,rangeSigma]);
% The initcvmscekf assumes a velocity standard deviation of 10 m/s, which
% is linearly transformed into azimuth rate, elevation rate and vr/r.
% Scale the velocity covariance by 400 to specify that target can move 20
% times faster.
filter.StateCovariance(2:2:end,2:2:end) = 400*filter.StateCovariance(2:2:end,2:2:end);
filter.ProcessNoise = eye(3);
end

% *|calculateManeuver|*
function maneuver = calculateManeuver(currentPose,prevPose,dT)
% Calculate maneuver i.e. 1st order+ motion of the observer. This is
% typically obtained using sensors operating at much higher rate.
v = prevPose.Velocity;
prevPos = prevPose.Position;
prevVel = prevPose.Velocity;
currentPos = currentPose.Position;
currentVel = currentPose.Velocity;

% position change apart from constant velocity motion
deltaP = currentPos - prevPos - v*dT;
% Velocity change
deltaV = currentVel - prevVel;

maneuver = zeros(6,1);
maneuver(1:2:end) = deltaP;
maneuver(2:2:end) = deltaV;
end

%%
% *|getTrackPositionsMSC|*
function [pos,cov] = getTrackPositionsMSC(tracks,observerPosition)

if isstruct(tracks) || isa(tracks,'objectTrack')
    % Track struct
    state = [tracks.State];
    stateCov = cat(3,tracks.StateCovariance);
elseif isa(tracks,'trackingMSCEKF')
    % Tracking Filter
    state = tracks.State;
    stateCov = tracks.StateCovariance;
end

% Get relative position using measurement function.
relPos = cvmeasmsc(state,'rectangular');

% Add observer position
pos = relPos + observerPosition;
pos = pos';

if nargout > 1
    cov = zeros(3,3,numel(tracks));
    
    for i = 1:numel(tracks)
        % Jacobian of position measurement
        jac = cvmeasmscjac(state(:,i),'rectangular');
        cov(:,:,i) = jac*stateCov(:,:,i)*jac';
    end
end

end

function [] = plot_ellipse(positionLog)

mu = positionLog.Position(end,:);
C = positionLog.PositionCovariance(1:2,1:2,end)
% Define the covariance matrix and mean vector
%C = [4 2; 2 3]; % Example covariance matrix
%mu = [1; 2];    % Example mean vector

% Calculate the eigevalues and eigenvectors
[eigenvec, eigenval] = eig(C);

% Calculate the angle of rotation
theta = atan2(eigenvec(2,1), eigenvec(1,1));

% Create points for the ellipse
t = linspace(0, 2*pi, 100);
ellipse = [cos(t); sin(t)];

% Scale the ellipse by the square root of the eigenvalues
scaledEllipse =(eigenval) * ellipse;

% Rotate and translate the ellipse
ellipsePoints = eigenvec * scaledEllipse;
ellipsePoints(1, :) = ellipsePoints(1, :) + mu(1);
ellipsePoints(2, :) = ellipsePoints(2, :) + mu(2);

% Plot the ellipse
%figure;
plot(ellipsePoints(1, :), ellipsePoints(2, :), 'b-', 'LineWidth', 2);
plot(mu(1), mu(2), 'ro'); % Plot the mean
end

Answer 1

[Prashant Arora]

I think you intended (but missed?) to compute the square root of the eigen values before computing the ellipse?

You can try to use theaterPlot and trackPlotter to plot position and covariance.

tp = theaterPlot;
trkP = trackPlotter(tp);
trkP.plotTrack(positionLog(1).Position(end,:),positionLog(1).PositionCovariance(:,:,end));

[black cat]

Thank you, how silly of me! That makes the ellipse much smaller. But now the actual track is outside of the ellipse, so the estimate seemes biased in some way. How can that be fixed? Could it have to do with the orientation reference frame?