Main Content

poseHistory

Retrieve corrected and predicted pose history

collapse all in page

    Syntax

    [correctedPose,predictedPose] = poseHistory(slamObj)

    Description

    example

    [correctedPose,predictedPose] = poseHistory(slamObj) retrieves the corrected and predicted pose history up to the current pose.

    Note

    To use this function during code generation, you must specify the MaxNumPoseStored property of the ekfSLAM object. Otherwise, this function returns an error.

    Examples

    collapse all

    Open Live Script

    Specify the initial vehicle state.

    initialState = [1; -2; 0.1];

    Specify the initial landmark positions.

    landmarkPosition = [15.8495; -12.9496;
                    25.2455; -15.4705;
                    37.5880;   3.1023;
                    16.5690;   2.7466];

    Specify the initial vehicle state covariance.

    initialStateCovar = diag([0.1*ones(1,3) 1.1*ones(1,8)]);

    Create an ekfSLAM object with initial state and initial state covariance.

    ekfSlamObj = ekfSLAM('State',[initialState; landmarkPosition], ...
                     'StateCovariance',initialStateCovar);

    Specify the control input and time step size for the state transition function.

    velocity = [1 0];
timeStep = 0.25;

    Call the predict function. 

    predict(ekfSlamObj,velocity,timeStep);

    Specify the measurement and measurement covariance for the data association function.

    measurement = [18.4500 -0.7354; 
               27.7362 -0.6071;
               36.9421  0.0386; 
               16.2765  0.1959];
measureCovar = [0.1^2 (1.0*pi/180)^2];

    Call the correct function.

    validationGate = 5.991;
associations = correct(ekfSlamObj,measurement, ...
                       measureCovar,validationGate);

    Get the pose history.

    [corrPose,predPose] = poseHistory(ekfSlamObj)
    corrPose = 1×3

    1.1609   -1.9736    0.0981


    predPose = 1×3

    1.2488   -1.9750    0.1000

    Input Arguments

    collapse all

    EKF SLAM object, specified as an ekfSLAM object.

    Output Arguments

    collapse all

    Corrected poses, returned as an M-by-3 matrix with rows of the form [X Y Yaw]. X and Y specify the position in meters. Yaw specifies the orientation in radians.

    Predicted poses, returned as an M-by-3 matrix with rows of the form [X Y Yaw]. X and Y specify the position in meters. Yaw specifies the orientation in radians.

    Extended Capabilities

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

    Version History

    Introduced in R2021b

    See Also

    Objects

    Functions