%% Pose Estimation Using IMU and 2-D Lidar Scan Matching
% This example shows how to use the orientation from IMU on a series of lidar scans to estimate the relative pose of the robot. The goal of this example is to :
% Show, how to use the orientation estimated by imufilter in 2-D lidar scan matching.
% Analyze the performance of the 2-D lidar scan matching with IMU orientation as the initial condition.
% The orientation of the robot is estimated by the imufilter using the raw accelerometer and the gyroscope readings from IMU. Besides successive scan data, relative orientation estimated is also passed as the initial condition for the scan matching method. The example uses matchScans method for the purpose of scan matching. To analyze the improvement with IMU initialization, moving window Root Mean Square Error (RMSE) in relative pose (X, Y, Theta) from scan matching with and without IMU initialization are plotted comparatively.
%% Load Lidar Scan and IMU Data from File
% Load the mat file that contains
% 2-D lidar scan data which is an n-by-1 matrix where n is the number of 2-D lidar scans. The lidar scans used in this example are generated from point cloud data, captured using 3-D lidar device.
% Accelerometer and gyroscope readings from an IMU. The raw readings contain the aceeleration and angular velocity values in X, Y, Z axes respectively in the form of an n-by-3 matrix.
% A n-by-2 matrix that contains indices of lidar scan and IMU readings to synchronize. Each row contains the lidar scan index and the corresponding IMU reading index respectively.
% A n-by-3 matrix containing the robot poses captured from a GPS device. The mean covariance in X, Y for the GPS data are 0.04 square meters and 0.05 square meters respectively.
% The data was collected in an outdoor environment. IMU and lidar data are pre-processed to be synchronous so that the relative orientation corresponding to a particular scan can be directly computed using the orientation estimated by the imufilter.

load offlineData.mat % accelReadings, gpsPoses, gyroReadings, mapIndicesLidarImu, scans
numScans = length(scans);

%
%% Estimate the Orientation from Accelerometer and Gyroscope Readings of IMU
% Create imufilter object and set the sample rate of input sensor data. Accelerometer and Gyroscope readings are passed as the input arguments to the imufilter object. If the raw IMU data has magnetometer readings also, ahrsfilter can be used in the place of imufilter. The imufilter returns the orientation in quaternion. Convert the orientation from quaternion to Euler angles using quat2eul method.

Fs = 400; % Hz
filterObj = imufilter('SampleRate', Fs);
[orientation, angVel] = filterObj(accelReadings, gyroReadings);
orientation = quat2eul(orientation, 'ZYX');

%
%% Extract Yaw Orientation Synchronized With Lidar Scans
% Using the variable mapIndicesLidarImu which has the indices of lidar scan data and the indices of IMU data that are synchronized, the orientation estimated by the IMU filter is extracted and stored in the variable syncOrientation. The data extracted is considered to be the absolute orientation pertaining to that robot pose at that instance.

syncOrientation = orientation(mapIndicesLidarImu(:, 2), 1);

%
%% Run Scan Matching and Compute Trajectory Poses from Relative Poses
% Using matchScans method, estimate the relative pose between two successive scans. The relative orientation between successive robot poses is computed using the absolute orientation values stored in the syncOrientation variable. Iterate the process for all the scans in a loop and store the relative poses obtained. Compute the absolute poses of the robot trajectory using the relative poses obtained from the scan matching using exampleHelperComputeTrajPose method. The CellSize should be tuned according to the computation speed requirements and the nature of the lidar scans.

relPosesWithImu = zeros(numScans-1, 3);
trajPosesWithImu = zeros(numScans, 3);
for i = 2 : numScans
    relPose = matchScans(scans(i, 1), scans(i-1, 1), 'InitialPose', [0 0 syncOrientation(i, 1) - syncOrientation(i-1, 1)], 'CellSize', 1.01);
    relPosesWithImu(i-1, :) = relPose;
    absPoseMatrix = exampleHelperComputeTrajPose(relPose, trajPosesWithImu, i);
    trajPosesWithImu(i, :) = [absPoseMatrix(1,1), absPoseMatrix(2,1), trajPosesWithImu(i-1, 3) + relPose(1,3)];
end

% Repeat the scan matching process over all the scans in a loop without using the relative orientation. Store the relative poses obtained and compute the absolute trajectory poses.

relPosesWithoutImu = zeros(numScans-1, 3);
trajPosesWithoutImu = zeros(numScans, 3);
for i = 2 : numScans
    relPose = matchScans(scans(i, 1), scans(i-1, 1), 'CellSize', 1.01);
    relPosesWithoutImu(i-1, :) = relPose;
    absPoseMatrix = exampleHelperComputeTrajPose(relPose, trajPosesWithoutImu, i);
    trajPosesWithoutImu(i, :) = [absPoseMatrix(1, 1), absPoseMatrix(2, 1), trajPosesWithoutImu(i-1, 3) + relPose(1, 3)];
end

%
%% Compute Relative GPS Trajectory Poses and Moving Window RMSE Values
% Compute the relative poses between two successive robot poses from the original GPS trajectory poses.

gpsRelativePoses = zeros(length(gpsPoses)-1, 3);
for i = 2 : length(gpsPoses)
    gpsRelativePoses(i-1, :) = gpsPoses(i, :) - gpsPoses(i-1, :);
end

%  Compute the moving window RMSE with the window size 2, in X, Y, and Theta, in the relative pose estimation from scan matching with and without IMU initialization. Consider smaller window size if it is required to observe the change in the immediate successive values.

windowSize = 2;
movWindowRmseWithImu = zeros(numScans-windowSize, 3);
movWindowRmseWithoutImu = zeros(numScans-windowSize, 3);
for i = windowSize : length(relPosesWithImu)
    movWindowRmseWithImu(i-(windowSize-1), :) = sqrt(mean((gpsRelativePoses(i-(windowSize-1) : i, :) - relPosesWithImu(i-(windowSize-1) : i, :)).^2));
    movWindowRmseWithoutImu(i-(windowSize-1), :) = sqrt(mean((gpsRelativePoses(i-(windowSize-1) : i, :) - relPosesWithoutImu(i-(windowSize-1) : i, :)).^2));
end

%
%% Visualize the Trajectories
% Using trajectory poses populated from the relative poses obtained from scan matching, plot the trajectory of the robot with and without IMU initialization besides the GPS trajectory.

figure
plot(trajPosesWithImu(:, 1), trajPosesWithImu(:, 2), 'b')
hold on
plot(trajPosesWithoutImu(:, 1), trajPosesWithoutImu(:, 2), 'r')
plot(gpsPoses(:, 1)-gpsPoses(1, 1), gpsPoses(:, 2)-gpsPoses(1, 2), 'g')
xlabel('X [meters]')
ylabel('Y [meters]')
legend('Trajectory With IMU Initialization', 'Trajectory Without IMU Initialization', 'GPS Trajectory')
title('Trajectories Compared against GPS Trajectory')

%% Visualize the Moving Window RMSE Plots
% The RMSE values in X, Y, Theta are plotted to visualize the effect of initialization by using orientation from IMU.

figure
plot(movWindowRmseWithImu(:, 1), 'b') 
hold on
plot(movWindowRmseWithoutImu(:, 1), 'r')
xlabel('Number of Windows of Size 2')
ylabel('Root Mean Square Error in X [meters]')
legend('Moving window RMSE in X with IMU initialization', 'Moving window RMSE in X without IMU initialization')
title('Moving window RMSE in X')
figure
plot(movWindowRmseWithImu(:, 2), 'b') 
hold on
plot(movWindowRmseWithoutImu(:, 2), 'r')
xlabel('Number of Windows of Size 2')
ylabel('Root Mean Square Error in Y [meters]')
legend('Moving window RMSE in Y with IMU initialization', 'Moving window RMSE in Y without IMU initialization')
title('Moving window RMSE in Y')
figure
plot(movWindowRmseWithImu(:, 3), 'b')
hold on 
plot(movWindowRmseWithoutImu(:, 3), 'r')
xlabel('Number of Windows of Size 2')
ylabel('Root Mean Square Error in Theta [radians]')
legend('Moving window RMSE in Theta with IMU initialization', 'Moving window RMSE in Theta without IMU initialization')
title('Moving window RMSE in Theta')

% In some cases, RMSE with IMU initialization may appear to be higher than that of the RMSE without IMU initialization in X, Y, Theta. This is because of the smaller window size, which is aimed to show the change in the immediate successive values. The trajectories plot helps in clearly visualizing the reduction in the drift with the IMU initialization as compared against the case where only lidar scans are used without IMU initialization for scan matching.
% Copyright 2019 The MathWorks, Inc.