%% Estimate Orientation Using ahrsfilter
% Load the rpy_9axis file, which contains recorded accelerometer, gyroscope, and magnetometer sensor data from a device oscillating in pitch (around y-axis), then yaw (around z-axis), and then roll (around x-axis). The file also contains the sample rate of the recording.

load 'rpy_9axis' sensorData Fs
accelerometerReadings = sensorData.Acceleration;
gyroscopeReadings = sensorData.AngularVelocity;
magnetometerReadings = sensorData.MagneticField;

%
% Create an ahrsfilter System object™ with SampleRate set to the sample rate of the sensor data. Specify a decimation factor of two to reduce the computational cost of the algorithm.

decim = 2;
fuse = ahrsfilter('SampleRate',Fs,'DecimationFactor',decim);

%
% Pass the accelerometer readings, gyroscope readings, and magnetometer readings to the ahrsfilter object, fuse, to output an estimate of the sensor body orientation over time. By default, the orientation is output as a vector of quaternions.

q = fuse(accelerometerReadings,gyroscopeReadings,magnetometerReadings);

%
% Orientation is defined by angular displacement required to rotate a parent coordinate system to a child coordinate system. Plot the orientation in Euler angles in degrees over time.
% ahrsfilter correctly estimates the change in orientation over time, including the south-facing initial orientation.

time = (0:decim:size(accelerometerReadings,1)-1)/Fs;

plot(time,eulerd(q,'ZYX','frame'))
title('Orientation Estimate')
legend('z-axis', 'y-axis', 'x-axis')
ylabel('Rotation (degrees)')

%
% Copyright 2018 The MathWorks, Inc.
%