ekfSLAM

Perform simultaneous localization and mapping using extended Kalman filter

Since R2021b

Description

The ekfSLAM object performs simultaneous localization and mapping (SLAM) using an extended Kalman filter (EKF). It takes in observed landmarks from the environment and compares them with known landmarks to find associations and new landmarks. Use the associations to correct the state and state covariance. The new landmarks are augmented in the state vector.

Creation

Syntax

slamObj = ekfSLAM

slamObj = ekfSLAM(Name,Value)

slamObj = ekfSLAM('MaxNumLandmark',N,Name,Value)

slamObj = ekfSLAM('MaxNumLandmark',N,‘MaxNumPoseStored’,M,Name,Value)

Description

slamObj = ekfSLAM creates an EKF SLAM object with default properties.

slamObj = ekfSLAM(Name,Value) sets properties using one or more name-value pair arguments in addition to any combination of input arguments from previous syntaxes. Any unspecified properties have default values.

slamObj = ekfSLAM('MaxNumLandmark',N,Name,Value) specifies an upper bound on the number of landmarks N allowed in the state vector when generating code. This limit on the number of landmarks applies only when generating code.

slamObj = ekfSLAM('MaxNumLandmark',N,‘MaxNumPoseStored’,M,Name,Value) specifies the maximum size of the pose history M along with the maximum number of landmarks N in the state vector while generating code. These limits apply only when generating code.

Properties

You cannot change the value of the properties State, StateCovariance, StateTransitionFcn, and MaxNumLandmark after the object is created. Set the value of these properties as a default or while creating the object.

State — State vector

[0; 0; 0] (default) | M-element column vector

State vector, specified as an M-element column vector.

Data Types: single | double

StateCovariance — State estimation error covariance

eye(3) (default) | M-by-M matrix

State estimation error covariance, specified as an M-by-M matrix. M is the number of states in the state vector.

Data Types: single | double

StateTransitionFcn — State transition function

nav.algs.velocityMotionModel (default) | function handle

State transition function, specified as a function handle. This function calculates the state vector at time step k from the state vector at time step k-1. The function can take additional input parameters, such as control inputs or time step size.

The function also calculates the Jacobians with respect to the current pose and controller input. If not specified, the Jacobians are computed using numerical differencing at each call to the predict function. This computation can increase processing time and numerical inaccuracy.

The function considers nonadditive process noise, and should have this signature:

[pose(k),jacPose,jacControl] = StateTransitionFcn(pose(k-1),controlInput,parameters)

• pose(k) is the estimated pose at time k.

• jacPose is the Jacobian of StateTransitionFcn with respect to pose(k-1).

• jacControl is the Jacobian of StateTransitionFcn with respect to controlInput.

• controlInput is the input for propagating the state.

• parameters are any additional arguments required by the state transition function.

Data Types: function_handle

MeasurementFcn — Measurement function

nav.algs.rangeBearingMeasurement (default) | function handle

Measurement function, specified as a function handle. This function calculates an N-element measurement vector for an M-element state vector.

The function also calculates the Jacobians with respect to the current pose and landmark position. If not specified, the Jacobians are computed using numerical differencing at each call to the correct function. This computation can increase processing time and numerical inaccuracy.

The function considers additive measurement noise, and should have this signature:

[measurements(k),jacPose,jacLandmarks] = MeasurementFcn(pose(k),landmarks)

• pose(k) is the estimated pose at time k.

• measurements(k) is the estimated measurement at time k.

• landmarks are the positions of the landmarks.

• jacPose is the Jacobian of MeasurementFcn with respect to pose(k).

• jacLandmarks is the Jacobian of MeasurementFcn with respect to landmarks.

Data Types: function_handle

InverseMeasurementFcn — Inverse measurement function

nav.algs.rangeBearingInverseMeasurement (default) | function handle

Inverse measurement function, specified as a function handle. This function calculates the landmark position as an M-element state vector for an N-element measurement vector.

The function also calculates the Jacobians with respect to the current pose and measurement. If not specified, the Jacobians are computed using numerical differencing at each call to the correct function. This computation can increase processing time and numerical inaccuracy.

The function should have this signature:

[landmarks(k),jacPose,jacMeasurements] = InverseMeasurementFcn(pose(k),measurements)

• pose(k) is the estimated pose at time k.

• landmarks(k) is the landmark position at time k.

• measurements are the observed landmarks at time k.

• jacPose is the Jacobian of InverseMeasurementFcn with respect to pose(k).

• jacMeasurements is the Jacobian of InverseMeasurementFcn with respect to measurements.

Data Types: function_handle

DataAssociationFcn — Data association function

nav.algs.associateMaxLikelihood (default) | function handle

Data association function, specified as a function handle. This function associates the measurements with the landmarks already available in the state vector. The function may take additional input parameters.

The function should have this signature:

[associations,newLandmarks] = DataAssociationFcn(knownLandmarks,knownLandmarksCovariance,observedLandmarks,observedLandmarksCovariance,parameters)

• knownLandmarks are known landmarks in the map.

• knownLandmarksCovariance is the covariance of knownLandmarks.

• observedLandmarks are the observed landmarks in the environment.

• observedLandmarksCovariance is the covariance of observedLandmarks.

• parameters are any additional arguments required.

• associations is a list of associations from knownLandmarks to observedLandmarks.

• newLandmarks are the indices of observedLandmarks that qualify as new landmarks.

Data Types: function_handle

ProcessNoise — Process noise covariance

eye(2) (default) | W-by-W matrix

Process noise covariance, specified as a W-by-W matrix. W is the number of process noise terms.

Data Types: single | double

MaxAssociationRange — Maximum range for landmarks to be checked for association

inf (default) | positive integer

Maximum range for the landmarks to be checked for association, specified as a positive integer.

Data Types: single | double

MaxNumLandmark — Maximum number of landmarks in state vector

inf (default) | positive integer

Maximum number of landmarks in the state vector, specified as a positive integer.

Data Types: single | double

MaxNumPoseStored — Maximum size of pose history

inf (default) | positive integer

Maximum size of pose history, specified as a positive integer.

Data Types: single | double

Object Functions

copy	Create deep copy of EKF SLAM object
correct	Correct state and state error covariance
landmarkInfo	Retrieve landmark information
poseHistory	Retrieve corrected and predicted pose history
predict	Predict state and state error covariance
removeLandmark	Remove landmark from state vector
reset	Reset state and state estimation error covariance

Examples

Perform Landmark SLAM Using Extended Kalman Filter

Load a race track data set that contains the initial vehicle state, initial vehicle state covariance, process noise covariance, control input, time step size, measurement, measurement covariance, and validation gate values.

load("racetrackDataset.mat","initialState","initialStateCovariance", ...
     "processNoise","controllerInputs","timeStep", ...
     "measurements","measCovar","validationGate");

Create an ekfSLAM object with initial state, initial state covariance, and process noise.

ekfSlamObj = ekfSLAM("State",initialState, ...
                     "StateCovariance",initialStateCovariance, ...
                     "ProcessNoise",processNoise);

Initialize a variable to store the pose.

storedPose = nan(size(controllerInputs,1)+1,3);
storedPose(1,:) = ekfSlamObj.State(1:3);

Predict the state using the control input and time step size for the state transition function. Then, correct the state using the data of the observed landmarks, measurement covariance, and validation gate for the data association function.

for count = 1:size(controllerInputs,1)
    % Predict the state
    predict(ekfSlamObj,controllerInputs(count,:),timeStep);
 
    % Get the landmarks in the environment
    observedLandmarks = measurements{count};
 
    % Correct the state
    if ~isempty(observedLandmarks)
        correct(ekfSlamObj,observedLandmarks, ...
                measCovar,validationGate);
    end
  
    % Log the estimated pose
    storedPose(count+1,:) = ekfSlamObj.State(1:3);
end

Visualize the created map.

fig = figure;
figAx = axes(fig);
axis equal
grid minor
hold on
plot(figAx,storedPose(:,1),storedPose(:,2),"g.-")
landmarks = reshape(ekfSlamObj.State(4:end),2,[])';
plot(figAx,landmarks(:,1),landmarks(:,2),"m+")
plot(figAx,storedPose(1,1),storedPose(1,2),"k*")
plot(figAx,storedPose(end,1),storedPose(end,2),"rd")
legend("Robot trajectory","Landmarks","Start","End")

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2021b