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

load('ahrs10filterTuneData.mat');

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

sensorData = table(Accelerometer,Gyroscope,...
    Magnetometer,Altimeter);
groundTruth = table(Orientation, Altitude);

% Create an ahrs10filter filter object.

filter = ahrs10filter('State', initialState, ...
    'StateCovariance', initialStateCovariance);      

% Create a tuner configuration object for the filter. Set the maximum iterations to ten and set the objective limit to 0.001.

cfg = tunerconfig('ahrs10filter','MaxIterations',10,...
          'ObjectiveLimit',1e-3);

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

measNoise = tunernoise('ahrs10filter')

% Tune the filter and obtain the tuned parameters.

tunedNoise = tune(filter, measNoise, sensorData, ...
    groundTruth, cfg);

% Fuse the sensor data using the tuned filter.

N = size(sensorData,1);
qEstTuned = quaternion.zeros(N,1);
altEstTuned = zeros(N,1);
for ii=1:N
    predict(filter,Accelerometer(ii,:),Gyroscope(ii,:));
    if all(~isnan(Magnetometer(ii,1)))
        fusemag(filter, Magnetometer(ii,:),tunedNoise.MagnetometerNoise);
    end
    if ~isnan(Altimeter(ii))
        fusealtimeter(filter, Altimeter(ii),tunedNoise.AltimeterNoise);
    end
    [altEstTuned(ii), qEstTuned(ii)] = pose(filter);
end

% Compute the RMS errors.

orientationErrorTuned = rad2deg(dist(qEstTuned, Orientation));
rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
positionErrorTuned = altEstTuned - Altitude;
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 ahrs10filter" + newline + ...
    "Altitude 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.