EKF-Based Landmark SLAM

Load Data Set

Load a modified version of the Victoria Park data set that contains the controller inputs, measurements, GPS latitude and longitude, and dead reckoning generated using the controller inputs and motion model.

load("victoriaParkDataset.mat","controllerInput", ...
     "measurements","gpsLatLong","deadReckoning");

Set Up Parameters

Specify the initial vehicle state and state covariance.

initialState = [gpsLatLong(1,2) gpsLatLong(1,1) deg2rad(37)]';
initialCovar = eps*eye(3);

Specify the process noise covariance in velocity and steering commands.

sigmaVelocity = 2;      % [m/s]
sigmaSteer= deg2rad(6); % [rad]
processNoise = [sigmaVelocity^2 0; 0 sigmaSteer^2];

Specify the measurement covariance in range and bearing.

sigmaRange = 1;            % [m]
sigmaBearing = deg2rad(3); % [rad]
measCovar = [sigmaRange^2 sigmaBearing^2];

Specify the maximum range at which to check landmarks for association.

maxSensorRange = 30; % [m]

Specify the time step size in which the vehicle moves.

timeStep = 0.025; % [sec]

Specify the thresholds for the data association function, nav.algs.associateMaxLikelihood. The landmark rejection threshold is the chi-square ($${\chi }^2$$) distribution table value for a 95% correct association.

landmarkRejectionThres = 5.991;  % maximum distance for association
landmarkAugmentationThres = 200; % minimum distance for creation of new landmark
validationGate = [landmarkRejectionThres landmarkAugmentationThres];

Set a flag that determines whether to plot the map during the run.

plotOnTheRun = false;

Get the number of samples from the data set.

numSamples = size(controllerInput,1);

Initialize Figure or Progress Bar

If the live plot option is enabled, create a new figure and set up handles for plotting various components. Otherwise, create a wait bar that updates with the proportion of the data set that has been executed. Set the wait bar so that clicking Cancel stops an in-progress computation. Closing the wait bar stops the execution.

if plotOnTheRun
    [robotHandle,covarianceHandle,sensorHandle, ...
     observationHandle,landmarkHandle, ...
     deadRecHandle,estTrajHandle] = ...
        exampleHelperInitializeVisualizationEKFSLAM(initialState,gpsLatLong);
else
    waitBarHandle = waitbar(0,'Example initializing...', ...
        'Name','ekfSLAM example', ...
        'CreateCancelBtn','setappdata(gcbf,''canceling'',1)');
    setappdata(waitBarHandle,'canceling',0);
end

Set Up ekfSLAM

Configure the ekfSLAM object using the initial state of the vehicle, initial state covariance, process noise covariance, and the motion model of the vehicle.

ekfSlamObj = ekfSLAM('State',initialState, ...
    'StateCovariance',initialCovar, ...
    'StateTransitionFcn',@exampleHelperVictoriaParkStateTransition);
ekfSlamObj.ProcessNoise = processNoise;
ekfSlamObj.MaxAssociationRange = maxSensorRange;

Main Loop

The main loop consists of these primary operations:

Prediction

In this example, the vehicle moves relative to its previous state based on the control input while the landmarks remain stationary. Thus, only the state of the vehicle is propagated. Use the motion model of the vehicle to propagate the state of the vehicle to next time step using the data set.

The predict method calls the function specified in the StateTransitionFcn property of ekfSlamObj to predict the state of the vehicle. The predict method passes the control input, other necessary inputs, and the current vehicle pose to the StateTransitionFcn.

for count = 1:numSamples
    predict(ekfSlamObj,controllerInput(count,:),timeStep);

Landmark Extraction

This example uses a list of observed landmarks from the data set, so you do not have to extract landmarks from the environment using a sensor. The landmarks are from a semi-circular region in front of the vehicle. The radius of the semi-circular region is defined by the maximum range of the sensor.

    observedLandmarks = measurements{count};

Correction

The ekfSLAM object corrects the state based on a given measurement, returning a list of matched landmarks and a list of new landmarks. The correct method uses the function specified in the DataAssociationFcn property of ekfSlamObj to associate the observed landmarks or measurements to known landmarks. The data association function returns a list of associations and a list of new landmarks. The correct method uses the associations to update the belief of the vehicle and the map using the correlation between the observed landmarks and the known landmarks. Additionally, the correct method augments the position and covariance of any new landmark in the State and StateCovariance vectors, respectively.

If the live plot option is enabled, update the figure with the scans at the current time.

  if ~isempty(observedLandmarks)
        correct(ekfSlamObj,observedLandmarks,measCovar,validationGate);
        % Update the corrected position of vehicle in the figure
        if plotOnTheRun
            exampleHelperUpdateScans(ekfSlamObj.State, ...
                              ekfSlamObj.MaxAssociationRange, ...
                              observedLandmarks, ...
                              sensorHandle,observationHandle);
        end
  end

Use drawnow to update the figure with the current position of the vehicle and all the known landmarks and their covariance.

If the live plot is not enabled and the wait bar is active, then update the fraction of the data set executed.

    if plotOnTheRun
        addpoints(deadRecHandle,deadReckoning(count,1),deadReckoning(count,2));
        addpoints(estTrajHandle,ekfSlamObj.State(1),ekfSlamObj.State(2));
        exampleHelperUpdateRobotAndLandmarks(ekfSlamObj.State,ekfSlamObj.StateCovariance, ...
            robotHandle,covarianceHandle,landmarkHandle);
        drawnow limitrate
    else
        % update the wait bar
        waitbar(count/numSamples,waitBarHandle, ...
            sprintf("%2.2f%% executed",count/numSamples*100));
        % Check for clicked Cancel button
        if getappdata(waitBarHandle,"canceling")
            break
        end
    end
end

Use the delete function to close the wait bar once all the computations are complete.

if ~plotOnTheRun
    delete(waitBarHandle);
end

Visualize Map

Create a geographic plot with the ground truth from the GPS data. Plot the vehicle trajectory, along with all detected landmarks and their associated covariance, on the same geographic plot.

% get the corrected and predicted poses
[corrPoses,predPoses] = poseHistory(ekfSlamObj);

% show the map
exampleHelperShowMap(ekfSlamObj.State,ekfSlamObj.StateCovariance, ...
                     gpsLatLong,corrPoses,predPoses);