trackerGNN for angle-only detections, no ground truth

I modified this example involving angle-only tracking using a moving single sensor. There are now no truth objects (previouly tam and tem), and the detections have no object ID's. But now the inferred tracks look very different from the truth data, and I'm not sure why. Can someone please help? The example if self contained. It loads 'PassiveSensorManeuveringTrajectory.mat' which came with the sensor Fusion toolbox.

%% 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);
target2 = 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]);

target2.Trajectory = kinematicTrajectory('Position',[5e4 -15e3 -12e3],...
        'Velocity',[-500/3 100/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
track2_truth = []; % truth

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

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

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

    detect1 = objectDetection(time, azimuth1, ...
       'MeasurementParameters',mp,'MeasurementNoise',angle_covar);
    detect2 = objectDetection(time, azimuth2, ...
       'MeasurementParameters',mp,'MeasurementNoise',angle_covar);
    detections = {detect1, detect2}; % what if index is unknown? )};
    
    % 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, origin);
            positionLog = updatePositionLog(positionLog, tracks, pos, cov);
        end

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

end

% compare truth tracks with inferred tracks. Why are they so different?
hold on
plot(track1_truth(:,1), track1_truth(:,2),'r-')
plot(track2_truth(:,1), track2_truth(:,2),'b-')
plot( positionLog(1).Position(:,1),  positionLog(1).Position(:,2),'m--')
plot( positionLog(2).Position(:,1),  positionLog(2).Position(:,2),'c--')
%%----------------------- 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

Answer 1

[Prashant Arora]

Hi,

The issue is in the way getTrackPositionsMSC is called in your code.

In the MSC frame, tracks are defined with respect to the ownship. To compute the track position, we need the position of the "ownship". In your code, replacing the call with this should produce correct results.

[pos, cov] = getTrackPositionsMSC(tracks, ownship_xyz(:));

Thanks,

Prashant

[black cat]

Thank you Prashant! The tracker now finds the correct locations about halfway through the scenario.