%% Tune insfilterNonholonomic to Optimize Pose Estimate
% Load the recorded sensor data and ground truth data.

load('insfilterNonholonomicTuneData.mat');

% Create tables for the sensor data and the truth data.

sensorData = table(Accelerometer, Gyroscope, ...
        GPSPosition, GPSVelocity);
groundTruth = table(Orientation, Position);

% Create an insfilterNonholonimic filter object.

filter = insfilterNonholonomic('State', initialState, ...
        'StateCovariance', initialStateCovariance, ...
        'DecimationFactor', 1);

% Create a tuner configuration object for the filter. Set the maximum number of iterations to 30.

config = tunerconfig('insfilterNonholonomic','MaxIterations',30);

% Use the tunernoise function to obtain a set of  initial sensor noises used in the filter.

measNoise = tunernoise('insfilterNonholonomic')

% Tune the filter and obtain the tuned measurement noise.

tunedNoise = tune(filter,measNoise,sensorData,groundTruth,config);

% Fuse the sensor data using the tuned filter. Obtain estimated pose and orientation.

N = size(sensorData,1);
qEstTuned = quaternion.zeros(N,1);
posEstTuned = zeros(N,3);
for ii=1:N
    predict(filter,Accelerometer(ii,:),Gyroscope(ii,:));
    if all(~isnan(GPSPosition(ii,1)))
        fusegps(filter, GPSPosition(ii,:), ...
            tunedNoise.GPSPositionNoise,GPSVelocity(ii,:), ...
            tunedNoise.GPSVelocityNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

% Compute the RMS errors.

orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation));
rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
positionErrorTuned = sqrt(sum((posEstTuned-Position).^2,2));
rmsPositionErrorTuned = sqrt(mean(positionErrorTuned.^2))

% Visualize the results.

figure;
t = (0:N-1)./filter.IMUSampleRate;
subplot(2,1,1)
plot(t,positionErrorTuned,'b');
title("Tuned insfilterNonholonomic" + newline + ...
    "Euclidean Distance Position Error")
xlabel('Time (s)');
ylabel('Position Error (meters)')
subplot(2,1,2)
plot(t,orientationErrorTuned,'b');
title("Orientation Error")
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');

% Copyright 2020 The MathWorks, Inc.
%
%
%