%% Determine Pose Using Inertial Sensors and GPS
% A magnetic, angular rate, and gravity (MARG) system consists of a magnetometer, gyroscope, and accelerometer. To fuse MARG and GPS data, create an insfilterMARG object.

FUSE = insfilterMARG

% insfilterMARG uses an extended Kalman filter with the following methods:
% predict –– Update states using accelerometer and gyroscope data
% fusemag –– Correct states using magnetometer data
% fusegps –– Correct states using GPS data
% Generally, accelerometer and gyroscope data is acquired at a higher rate than magnetometer and GPS data. You can use nested loops to call predict, fusemag, andfusegps at different rates.

accelData   = [0 0 9.8];
gyroData    = [0 0 0];
magData     = [19.535 -5.109 47.930];
magCov      = 0;
position    = [0 0 0];
positionCov = 0;
velocity    = rand(1,3);
velocityCov = 0;

predictDataSampleRate = 100;
fuseDataSampleRate = 2;
predictSamplesPerFuse = predictDataSampleRate/fuseDataSampleRate;

duration = 5;

for i = 1:duration*fuseDataSampleRate
    
    for j = 1:predictSamplesPerFuse
        
        predict(FUSE,accelData,gyroData);
        
    end
    
    fusegps(FUSE,position,positionCov,velocity,velocityCov);
    fusemag(FUSE,magData,magCov);
    
end

% At any time, you can call pose to return the current position and orientation estimates:

[position, orientation] = pose(FUSE)

%
%
%
%
% Copyright 2019 The MathWorks, Inc.