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
AccelerometerNoise: [0.0001 0.0001 0.0001]    (m/s²)²
AccelerometerBiasNoise: [0.0001 0.0001 0.0001]    (m/s²)²

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
AccelerometerNoise: [0.048 0.048 0.048]          (m/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.