%% Retrieve Pose History from ekfSLAM Object
% 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)

% Copyright 2021 The MathWorks, Inc.