Main Content

ekfSLAM

Perform simultaneous localization and mapping using extended Kalman filter

Since R2021b

expand all in page

    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.

    example

    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

    expand all

    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 vector, specified as an M-element column vector.

    Data Types: single | double

    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

    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

    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

    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

    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

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

    Data Types: single | double

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

    Data Types: single | double

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

    Data Types: single | double

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

    Data Types: single | double

    Object Functions

    copyCreate deep copy of EKF SLAM object
    correctCorrect state and state error covariance
    landmarkInfoRetrieve landmark information
    poseHistoryRetrieve corrected and predicted pose history
    predictPredict state and state error covariance
    removeLandmarkRemove landmark from state vector
    resetReset state and state estimation error covariance

    Examples

    collapse all

    Open Live Script

    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")

    Figure contains an axes object. The axes object contains 4 objects of type line. One or more of the lines displays its values using only markers These objects represent Robot trajectory, Landmarks, Start, End.

    Extended Capabilities

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

    Version History

    Introduced in R2021b

    See Also

    Objects

    Functions

    Topics