%% Estimate Pose of UAV
% This example shows how to estimate the pose of an unmanned aerial vehicle (UAV) from logged sensor data and ground truth pose.
%
% Load the logged sensor data and ground truth pose of an UAV.

load uavshort.mat

%
% Initialize the insfilterMARG filter object.

f = insfilterMARG;
f.IMUSampleRate = imuFs;
f.ReferenceLocation = refloc;
f.AccelerometerBiasNoise = 2e-4;
f.AccelerometerNoise = 2;
f.GyroscopeBiasNoise = 1e-16;
f.GyroscopeNoise = 1e-5;
f.MagnetometerBiasNoise = 1e-10;
f.GeomagneticVectorNoise = 1e-12;
f.StateCovariance = 1e-9*ones(22);
f.State = initstate;
 
gpsidx = 1;
N = size(accel,1);
p = zeros(N,3);
q = zeros(N,1,'quaternion');

%
%  Fuse accelerometer, gyroscope, magnetometer, and GPS data.

for ii = 1:size(accel,1)               % Fuse IMU
   f.predict(accel(ii,:), gyro(ii,:));
        
   if ~mod(ii,fix(imuFs/2))            % Fuse magnetometer at 1/2 the IMU rate
       f.fusemag(mag(ii,:),Rmag);
   end
  
   if ~mod(ii,imuFs)                   % Fuse GPS once per second
       f.fusegps(lla(gpsidx,:),Rpos,gpsvel(gpsidx,:),Rvel);
       gpsidx = gpsidx + 1;
   end
 
   [p(ii,:),q(ii)] = pose(f);           %Log estimated pose
end

%
% Calculate and display RMS errors.

posErr = truePos - p;
qErr = rad2deg(dist(trueOrient,q));
pRMS = sqrt(mean(posErr.^2));
qRMS = sqrt(mean(qErr.^2));
fprintf('Position RMS Error\n\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));
    
fprintf('Quaternion Distance RMS Error\n\t%.2f (degrees)\n\n',qRMS);

%
% Copyright 2018 The MathWorks, Inc.
%