%% Tune ahrsfilter to Improve Orientation Estimate
% Load recorded sensor data and ground truth data.

ld = load('ahrsfilterTuneData.mat');
qTrue = ld.groundTruth.Orientation; % true orientation

% Create an arhsfitler object.

fuse = ahrsfilter;

% Fuse the sensor data using the default, untuned filter.

qEstUntuned = fuse(ld.sensorData.Accelerometer, ...
    ld.sensorData.Gyroscope, ld.sensorData.Magnetometer);

% Create a tunerconfig object. Tune the ahrsfilter object to improve the orientation estimation based on the configuration.

config = tunerconfig('ahrsfilter');
tune(fuse,ld.sensorData,ld.groundTruth,config);

% Fuse the sensor data using the tuned filter.

qEstTuned = fuse(ld.sensorData.Accelerometer, ...
    ld.sensorData.Gyroscope, ld.sensorData.Magnetometer);

% Compare the tuned and untuned RMS error performances.

dUntuned = rad2deg(dist(qEstUntuned, qTrue));
dTuned = rad2deg(dist(qEstTuned, qTrue));
rmsUntuned = sqrt(mean(dUntuned.^2))
rmsTuned = sqrt(mean(dTuned.^2))

% Visualize the errors with respect to time.

N = numel(dUntuned);
t = (0:N-1)./ fuse.SampleRate;
plot(t, dUntuned, 'r', t, dTuned, 'b');
legend('Untuned', 'Tuned');
title('ahrsfilter - Tuned vs Untuned Error')
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');

% Copyright 2020 The MathWorks, Inc.