Main Content

poseHistory

Retrieve corrected and predicted pose history

    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

    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™.

    See Also

    Objects

    Functions

    Introduced in R2021b