%% Estimate Pose of UAV
% Load logged sensor data, ground truth pose, and initial state and initial state covariance. Calculate the number of IMU samples per altimeter sample and the number of IMU samples per magnetometer sample.

load('fuse10exampledata.mat', ...
     'imuFs','accelData','gyroData', ...
     'magnetometerFs','magData', ...
     'altimeterFs','altData', ...
     'expectedHeight','expectedOrient', ...
     'initstate','initcov');

imuSamplesPerAlt = fix(imuFs/altimeterFs);
imuSamplesPerMag = fix(imuFs/magnetometerFs);

%
% Create an AHRS filter that fuses MARG and altimeter readings to estimate height and orientation. Set the sampling rate and measurement noises of the sensors. The values were determined from datasheets and experimentation.

filt = ahrs10filter('IMUSampleRate',imuFs, ...
                    'AccelerometerNoise',0.1, ...
                    'State',initstate, ...
                    'StateCovariance',initcov);

Ralt = 0.24;
Rmag = 0.9;

%
% Preallocate variables to log height and orientation.

numIMUSamples = size(accelData,1);
estHeight = zeros(numIMUSamples,1);
estOrient = zeros(numIMUSamples,1,'quaternion');

%
% Fuse accelerometer, gyroscope, magnetometer and altimeter data. The outer loop predicts the filter forward at the fastest sample rate (the IMU sample rate).

for ii = 1:numIMUSamples
    
    % Use predict to estimate the filter state based on the accelometer and
    % gyroscope data.
    predict(filt,accelData(ii,:),gyroData(ii,:));
    
    % Magnetometer data is collected at a lower rate than IMU data. Fuse
    % magnetometer data at the lower rate.
    if ~mod(ii,imuSamplesPerMag)
        fusemag(filt,magData(ii,:),Rmag);
    end
    
    % Altimeter data is collected at a lower rate than IMU data. Fuse
    % altimeter data at the lower rate.
    if ~mod(ii, imuSamplesPerAlt)
        fusealtimeter(filt,altData(ii),Ralt);
    end
    
    % Log the current height and orientation estimate.
    [estHeight(ii),estOrient(ii)] = pose(filt);
end

%
% Calculate the RMS errors between the known true height and orientation and the output from the AHRS filter.

pErr = expectedHeight - estHeight;
qErr = rad2deg(dist(expectedOrient,estOrient));

pRMS = sqrt(mean(pErr.^2));
qRMS = sqrt(mean(qErr.^2));

fprintf('Altitude RMS Error\n');
fprintf('\t%.2f (meters)\n\n',pRMS);

%
% Visualize the true and estimated height over time.

t = (0:(numIMUSamples-1))/imuFs;
plot(t,expectedHeight);hold on
plot(t,estHeight);hold off
legend('Ground Truth','Estimated Height','location','best')
ylabel('Height (m)')
xlabel('Time (s)')
grid on


fprintf('Quaternion Distance RMS Error\n');
fprintf('\t%.2f (degrees)\n\n',qRMS);

%
% Copyright 2018 The MathWorks, Inc.
%