%% Estimate Pose of UAV
% Load logged sensor data and ground truth pose.

load('uavshort.mat','refloc','initstate','imuFs', ...
    'accel','gyro','mag','lla','gpsvel', ...
    'trueOrient','truePos')

%
% Create an INS filter to fuse asynchronous MARG and GPS data to estimate pose.

filt = insfilterAsync;
filt.ReferenceLocation = refloc;
filt.State = [initstate(1:4);0;0;0;initstate(5:10);0;0;0;initstate(11:end)];

%
% Define sensor measurement noises. The noises were determined from datasheets and experimentation.

Rmag  = 80;
Rvel  = 0.0464;
Racc  = 800;
Rgyro = 1e-4;
Rpos  = 34;

%
% Preallocate variables for position and orientation. Allocate a variable for indexing into the GPS data.

N = size(accel,1);
p = zeros(N,3);
q = zeros(N,1,'quaternion');

gpsIdx = 1;

%
% Fuse accelerometer, gyroscope, magnetometer, and GPS data. The outer loop predicts the filter forward one time step and fuses accelerometer and gyroscope data at the IMU sample rate.

for ii = 1:N
    
    % Predict the filter forward one time step
    predict(filt,1./imuFs);
    
    % Fuse accelerometer and gyroscope readings
    fuseaccel(filt,accel(ii,:),Racc);
    fusegyro(filt,gyro(ii,:),Rgyro);
    
    % Fuse magnetometer at 1/2 the IMU rate
    if ~mod(ii, fix(imuFs/2))
        fusemag(filt,mag(ii,:),Rmag);
    end
    
    % Fuse GPS once per second
    if ~mod(ii,imuFs)
        fusegps(filt,lla(gpsIdx,:),Rpos,gpsvel(gpsIdx,:),Rvel);
        gpsIdx = gpsIdx + 1;
    end
    
    % Log the current pose estimate
    [p(ii,:),q(ii)] = pose(filt);
    
end

%
% Calculate the RMS errors between the known true position and orientation and the output from the asynchronous IMU filter.

posErr = truePos - p;
qErr = rad2deg(dist(trueOrient,q));

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

fprintf('Position RMS Error\n');
fprintf('\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));

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

%
% Visualize the true position and the estimated position.

plot3(truePos(:,1),truePos(:,2),truePos(:,3),'LineWidth',2)
hold on
plot3(p(:,1),p(:,2),p(:,3),'r:','LineWidth',2)
grid on
xlabel('N (m)')
ylabel('E (m)')
zlabel('D (m)')

%
% Copyright 2018 The MathWorks, Inc.
%