## 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:

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.