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:

  • MARGGPSFuser –– Estimate pose using a magnetometer, gyroscope, accelerometer, and GPS data.

  • NHConstrainedIMUGPSFuser –– 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 MARGGPSFuser object using the insfilter function:

FUSE = insfilter('NonholonomicHeading',false,'Magnetometer',true)
FUSE = 

  MARGGPSFuser 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²

MARGGPSFuser 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*predictDataSampleRate
    
    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.0e-15 *

   -0.0000   -0.0555    0.1110


orientation = 

  quaternion


     0.65342 + 0.56507i + 0.31113j + 0.39615k

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 NHConstrainedIMUGPSFuser object using the insfilter function:

FUSE = insfilter('NonholonomicHeading',true,'Magnetometer',false)
FUSE = 

  NHConstrainedIMUGPSFuser 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)²

NHConstrainedIMUGPSFuser 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*predictDataSampleRate
    
    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.0e-15 *

   -0.0000   -0.0555    0.1110


orientation = 

  quaternion


     0.65342 + 0.56507i + 0.31113j + 0.39615k

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

See Also

| | | | |

Related Topics