Determine Pose Using Inertial Sensors and GPS

Sensor Fusion and Tracking Toolbox™ enables you to fuse data read from IMUs and GPS to estimate pose. Use the insfilter function to create an INS/GPS fusion filter suited to your system:

insfilterMARG –– Estimate pose using a magnetometer, gyroscope, accelerometer, and GPS data.
insfilterNonholonomic –– Estimate pose using a gyroscope, accelerometer, and GPS data. This algorithm uses nonholonomic constraints.

This tutorial provides an overview of inertial sensor fusion with GPS in Sensor Fusion and Tracking Toolbox.

To learn how to model inertial sensors and GPS, see Model IMU, GPS, and INS/GPS. To learn how to generate the ground-truth motion that drives sensor models, see waypointTrajectory and kinematicTrajectory.

You can also fuse inertial sensor data without GPS to estimate orientation. See Determine Orientation Using Inertial Sensors.

Fuse Inertial Sensor and GPS data

An inertial navigation system (INS) consists of sensors that detect translational motion (accelerometers), rotation (gyroscopes), and in some systems magnetic fields (magnetometers). By fusing the sensor data continuously, an INS can dead reckon a platform's pose without external sensors. However, INS pose estimation generally decreases in accuracy over time and needs to be corrected using an external reference, such as GPS readings. Common configurations for INS/GPS fusion include MARG+GPS for aerial vehicles and accelerometer+gyroscope+GPS with nonholonomic constraints for ground vehicles.

Fuse MARG and GPS

A magnetic, angular rate, and gravity (MARG) system consists of a magnetometer, gyroscope, and accelerometer. To fuse MARG and GPS data, create a insfilterMARG object:

FUSE = insfilterMARG

FUSE = 

  insfilterMARG with properties:

        IMUSampleRate: 100               Hz         
    ReferenceLocation: [0 0 0]           [deg deg m]
                State: [22x1 double]                
      StateCovariance: [22x22 double]               

   Multiplicative Process Noise Variances
            GyroscopeNoise: [1e-09 1e-09 1e-09]       (rad/s)²
        AccelerometerNoise: [0.0001 0.0001 0.0001]    (m/s²)² 
        GyroscopeBiasNoise: [1e-10 1e-10 1e-10]       (rad/s)²
    AccelerometerBiasNoise: [0.0001 0.0001 0.0001]    (m/s²)² 

   Additive Process Noise Variances
    GeomagneticVectorNoise: [1e-06 1e-06 1e-06]    uT²
     MagnetometerBiasNoise: [0.1 0.1 0.1]          uT²

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, and fusegps 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)

position = 1×3

10-15 ×
   -0.3331   -0.2775    0.3886

orientation = quaternion
      0.84705 -  0.25459i -  0.46613j - 0.020379k

For a complete example workflow using MARGGPSFuser, see IMU and GPS Fusion for Inertial Navigation.

Fuse Accelerometer, Gyroscope, and GPS with Nonholonomic Constraints

Fusing accelerometer, gyroscope, and GPS data with nonholonomic constraints is a common configuration for ground vehicle pose estimation. To fuse accelerometer, gyroscope, and GPS data, create a insfilterNonholonomic object:

FUSE = insfilterNonholonomic

FUSE = 
  insfilterNonholonomic with properties:

        IMUSampleRate: 100        Hz         
    ReferenceLocation: [0 0 0]    [deg deg m]
     DecimationFactor: 2                     

   Extended Kalman Filter Values
              State: [16x1 double]     
    StateCovariance: [16x16 double]    

   Process Noise Variances
                  GyroscopeNoise: [4.8e-06 4.8e-06 4.8e-06]    (rad/s)²
              AccelerometerNoise: [0.048 0.048 0.048]          (m/s²)² 
              GyroscopeBiasNoise: [4e-14 4e-14 4e-14]          (rad/s)²
        GyroscopeBiasDecayFactor: 0.999                                
          AccelerometerBiasNoise: [4e-14 4e-14 4e-14]          (m/s²)² 
    AccelerometerBiasDecayFactor: 0.9999                               

   Measurement Noise Variances
    ZeroVelocityConstraintNoise: 0.01    (m/s)²

insfilterNonholonomic uses an extended Kalman filter with the following functions:

predict –– Update states using accelerometer and gyroscope data
fusegps –– Correct states using GPS data

Generally, accelerometer and gyroscope data is acquired at a higher rate than GPS data. You can use nested loops to call predict and fusegps at different rates.

accelData   = [0 0 9.8];
gyroData    = [0 0 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);
    
end

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

[position, orientation] = pose(FUSE)

position = 1×3

     0     0     0

orientation = quaternion

      0.9726 +       0i +       0j + 0.23248k

For a complete example workflow using NHConstrainedIMUGPSFuser, see Estimate Position and Orientation of a Ground Vehicle.