# insfilterMARG

Estimate pose from MARG and GPS data

## Description

The `insfilterMARG` object implements sensor fusion of MARG and GPS data to estimate pose in the NED (or ENU) reference frame. MARG (magnetic, angular rate, gravity) data is typically derived from magnetometer, gyroscope, and accelerometer sensors. The filter uses a 22-element state vector to track the orientation quaternion, velocity, position, MARG sensor biases, and geomagnetic vector. The `insfilterMARG` object uses an extended Kalman filter to estimate these quantities.

## Creation

### Syntax

``filter = insfilterMARG``
``filter = insfilterMARG('ReferenceFrame',RF)``
``filter = insfilterMARG(___,Name,Value)``

### Description

example

````filter = insfilterMARG` creates an `insfilterMARG` object with default property values.```
````filter = insfilterMARG('ReferenceFrame',RF)` allows you to specify the reference frame, `RF`, of the `filter`. Specify `RF` as `'NED'` (North-East-Down) or `'ENU'` (East-North-Up). The default value is `'NED'`.```
````filter = insfilterMARG(___,Name,Value)` also allows you set properties of the created `filter` using one or more name-value pairs. Enclose each property name in single quotes. ```

## Properties

expand all

Sample rate of the inertial measurement unit (IMU) in Hz, specified as a positive scalar.

Data Types: `single` | `double`

Reference location, specified as a 3-element row vector in geodetic coordinates (latitude, longitude, and altitude). Altitude is the height above the reference ellipsoid model, WGS84. The reference location units are [degrees degrees meters].

Data Types: `single` | `double`

Multiplicative process noise variance from the gyroscope in (rad/s)2, specified as a scalar or 3-element row vector of positive real finite numbers.

• If `GyroscopeNoise` is specified as a row vector, the elements correspond to the noise in the x, y, and z axes of the gyroscope, respectively.

• If `GyroscopeNoise` is specified as a scalar, the single element is applied to the x, y, and z axes of the gyroscope.

Data Types: `single` | `double`

Multiplicative process noise variance from the gyroscope bias in (rad/s)2, specified as a scalar or 3-element row vector of positive real numbers.

• If `GyroscopeBiasNoise` is specified as a row vector, the elements correspond to the noise in the x, y, and z axes of the gyroscope bias, respectively.

• If `GyroscopeBiasNoise` is specified as a scalar, the single element is applied to each axis.

Data Types: `single` | `double`

Multiplicative process noise variance from the accelerometer in (m/s2)2, specified as a scalar or 3-element row vector of positive real finite numbers.

• If `AccelerometerNoise` is specified as a row vector, the elements correspond to the noise in the x, y, and z axes of the accelerometer, respectively.

• If `AccelerometerNoise` is specified as a scalar, the single element is applied to each axis.

Data Types: `single` | `double`

Multiplicative process noise variance from the accelerometer bias in (m/s2)2, specified as a scalar or 3-element row vector of positive real numbers.

• If `AccelerometerBiasNoise` is specified as a row vector, the elements correspond to the noise in the x, y, and z axes of the accelerometer bias, respectively.

• If `AccelerometerBiasNoise` is specified as a scalar, the single element is applied to each axis.

Data Types: `single` | `double`

Additive process noise for geomagnetic vector in µT2, specified as a scalar or 3-element row vector of positive real numbers.

• If `GeomagneticVectorNoise` is specified as a row vector, the elements correspond to the noise in the x, y, and z axes of the geomagnetic vector, respectively.

• If `GeomagneticVectorNoise` is specified as a scalar, the single element is applied to each axis.

Data Types: `single` | `double`

Additive process noise for magnetometer bias in µT2, specified as a scalar or 3-element row vector.

• If `MagnetometerBiasNoise` is specified as a row vector, the elements correspond to the noise in the x, y, and z axes of the magnetometer bias, respectively.

• If `MagnetometerBiasNoise` is specified as a scalar, the single element is applied to each axis.

Data Types: `single` | `double`

State vector of the extended Kalman filter. The state values represent:

StateUnitsIndex
Orientation (quaternion parts)N/A1:4
Position (NED or ENU)m5:7
Velocity (NED or ENU)m/s8:10
Delta Velocity Bias (XYZ)m/s14:16
Geomagnetic Field Vector (NED or ENU)µT17:19
Magnetometer Bias (XYZ)µT20:22

Data Types: `single` | `double`

State error covariance for the extended Kalman filter, specified as a 22-by-22-element matrix, or real numbers.

Data Types: `single` | `double`

## Object Functions

 `correct` Correct states using direct state measurements for `insfilterMARG` `residual` Residuals and residual covariances from direct state measurements for `insfilterMARG` `fusegps` Correct states using GPS data for `insfilterMARG` `residualgps` Residuals and residual covariance from GPS measurements for `insfilterMARG` `fusemag` Correct states using magnetometer data for `insfilterMARG` `residualmag` Residuals and residual covariance from magnetometer measurements for `insfilterMARG` `pose` Current orientation and position estimate for `insfilterMARG` `predict` Update states using accelerometer and gyroscope data for `insfilterMARG` `reset` Reset internal states for `insfilterMARG` `stateinfo` Display state vector information for `insfilterMARG` `tune` Tune `insfilterMARG` parameters to reduce estimation error `copy` Create copy of `insfitlerMARG`

## Examples

collapse all

This example shows how to estimate the pose of an unmanned aerial vehicle (UAV) from logged sensor data and ground truth pose.

Load the logged sensor data and ground truth pose of an UAV.

`load uavshort.mat`

Initialize the `insfilterMARG` filter object.

```f = insfilterMARG; f.IMUSampleRate = imuFs; f.ReferenceLocation = refloc; f.AccelerometerBiasNoise = 2e-4; f.AccelerometerNoise = 2; f.GyroscopeBiasNoise = 1e-16; f.GyroscopeNoise = 1e-5; f.MagnetometerBiasNoise = 1e-10; f.GeomagneticVectorNoise = 1e-12; f.StateCovariance = 1e-9*ones(22); f.State = initstate; gpsidx = 1; N = size(accel,1); p = zeros(N,3); q = zeros(N,1,'quaternion');```

Fuse accelerometer, gyroscope, magnetometer, and GPS data.

```for ii = 1:size(accel,1) % Fuse IMU f.predict(accel(ii,:), gyro(ii,:)); if ~mod(ii,fix(imuFs/2)) % Fuse magnetometer at 1/2 the IMU rate f.fusemag(mag(ii,:),Rmag); end if ~mod(ii,imuFs) % Fuse GPS once per second f.fusegps(lla(gpsidx,:),Rpos,gpsvel(gpsidx,:),Rvel); gpsidx = gpsidx + 1; end [p(ii,:),q(ii)] = pose(f); %Log estimated pose end```

Calculate and display RMS errors.

```posErr = truePos - p; qErr = rad2deg(dist(trueOrient,q)); pRMS = sqrt(mean(posErr.^2)); qRMS = sqrt(mean(qErr.^2)); fprintf('Position RMS Error\n\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));```
```Position RMS Error X: 0.57, Y: 0.53, Z: 0.68 (meters) ```
``` fprintf('Quaternion Distance RMS Error\n\t%.2f (degrees)\n\n',qRMS);```
```Quaternion Distance RMS Error 0.28 (degrees) ```

## Algorithms

Note: The following algorithm only applies to an NED reference frame.

`insfilterMARG` uses a 22-axis extended Kalman filter structure to estimate pose in the NED reference frame. The state is defined as:

`$x=\left[\begin{array}{c}{q}_{0}\\ {q}_{1}\\ {q}_{2}\\ {q}_{3}\\ positio{n}_{\text{N}}\\ positio{n}_{\text{E}}\\ positio{n}_{\text{D}}\\ {\nu }_{\text{N}}\\ {\nu }_{\text{E}}\\ {\nu }_{\text{D}}\\ \Delta \theta bia{s}_{\text{X}}\\ \Delta \theta bia{s}_{\text{Y}}\\ \Delta \theta bia{s}_{\text{Z}}\\ \Delta \nu bia{s}_{\text{X}}\\ \Delta \nu bia{s}_{\text{Y}}\\ \Delta \nu bia{s}_{\text{Z}}\\ geomagneticFieldVecto{r}_{\text{N}}\\ geomagneticFieldVecto{r}_{\text{E}}\\ geomagneticFieldVecto{r}_{\text{D}}\\ magbia{s}_{\text{X}}\\ magbia{s}_{\text{Y}}\\ magbia{s}_{\text{Z}}\end{array}\right]$`

where

• q0, q1, q2, q3 –– Parts of orientation quaternion. The orientation quaternion represents a frame rotation from the platform's current orientation to the local NED coordinate system.

• positionN, positionE, positionD –– Position of the platform in the local NED coordinate system.

• νN, νE, νD –– Velocity of the platform in the local NED coordinate system.

• ΔθbiasX, ΔθbiasY, ΔθbiasZ –– Bias in the integrated gyroscope reading.

• ΔνbiasX, ΔνbiasY, ΔνbiasZ –– Bias in the integrated accelerometer reading.

• geomagneticFieldVectorN, geomagneticFieldVectorE, geomagneticFieldVectorD –– Estimate of the geomagnetic field vector at the reference location.

• magbiasX, magbiasY, magbiasZ –– Bias in the magnetometer readings.

Given the conventional formation of the predicted state estimate,

`${x}_{k|k-1}=f\left({\stackrel{^}{x}}_{k-1|k-1},{u}_{k}\right)$`

uk is controlled by accelerometer and gyroscope data that has been converted to delta velocity and delta angle through trapezoidal integration. The predicted state estimation is:

`${x}_{k|k-1}=\left[\begin{array}{c}{q}_{0}-{q}_{1}\left(\frac{\Delta {\theta }_{\text{X}}-\Delta \theta bia{s}_{\text{X}}}{2}\right)-{q}_{2}\left(\frac{\Delta {\theta }_{\text{Y}}-\Delta \theta bia{s}_{\text{Y}}}{2}\right)-{q}_{3}\left(\frac{\Delta {\theta }_{\text{Z}}-\Delta \theta bia{s}_{\text{Z}}}{2}\right)\\ {q}_{1}+{q}_{0}\left(\frac{\Delta {\theta }_{\text{X}}-\Delta \theta bia{s}_{\text{X}}}{2}\right)-{q}_{3}\left(\frac{\Delta {\theta }_{\text{Y}}-\Delta \theta bia{s}_{\text{Y}}}{2}\right)+{q}_{2}\left(\frac{\Delta {\theta }_{\text{Z}}-\Delta \theta bia{s}_{\text{Z}}}{2}\right)\\ {q}_{2}+{q}_{3}\left(\frac{\Delta {\theta }_{\text{X}}-\Delta \theta bia{s}_{\text{X}}}{2}\right)+{q}_{0}\left(\frac{\Delta {\theta }_{\text{Y}}-\Delta \theta bia{s}_{\text{Y}}}{2}\right)-{q}_{1}\left(\frac{\Delta {\theta }_{\text{Z}}-\Delta \theta bia{s}_{\text{Z}}}{2}\right)\\ {q}_{3}-{q}_{2}\left(\frac{\Delta {\theta }_{\text{X}}-\Delta \theta bia{s}_{\text{X}}}{2}\right)+{q}_{1}\left(\frac{\Delta {\theta }_{\text{Y}}-\Delta \theta bia{s}_{\text{Y}}}{2}\right)+{q}_{0}\left(\frac{\Delta {\theta }_{\text{Z}}-\Delta \theta bia{s}_{\text{Z}}}{2}\right)\\ positio{n}_{\text{N}}+\left(\Delta t\right)\left({\nu }_{\text{N}}\right)\\ positio{n}_{\text{E}}+\left(\Delta t\right)\left({\nu }_{\text{E}}\right)\\ positio{n}_{\text{D}}+\left(\Delta t\right)\left({\nu }_{\text{D}}\right)\\ {\nu }_{\text{N}}+\left(\Delta t\right)\left({g}_{\text{N}}\right)+\left(\Delta {\nu }_{\text{X}}-\Delta \nu bia{s}_{\text{X}}\right)\left({q}_{0}^{2}+{q}_{{}_{1}}^{2}-{q}_{2}^{2}-{q}_{3}^{2}\right)-2\left(\Delta {\nu }_{\text{Y}}-\Delta \nu bia{s}_{\text{Y}}\right)\left({q}_{0}{q}_{3}-{q}_{1}{q}_{2}\right)+2\left(\Delta {\nu }_{\text{Z}}-\Delta \nu bia{s}_{\text{Z}}\right)\left({q}_{0}{q}_{2}+{q}_{1}{q}_{3}\right)\\ {\nu }_{\text{E}}+\left(\Delta t\right)\left({g}_{\text{E}}\right)+\left(\Delta {\nu }_{\text{Y}}-\Delta \nu bia{s}_{\text{Y}}\right)\left({q}_{0}^{2}-{q}_{{}_{1}}^{2}+{q}_{2}^{2}-{q}_{3}^{2}\right)+2\left(\Delta {\nu }_{\text{X}}-\Delta \nu bia{s}_{\text{X}}\right)\left({q}_{0}{q}_{3}+{q}_{1}{q}_{2}\right)-2\left(\Delta {\nu }_{\text{Z}}-\Delta \nu bia{s}_{\text{Z}}\right)\left({q}_{0}{q}_{1}-{q}_{2}{q}_{3}\right)\\ {\nu }_{\text{D}}+\left(\Delta t\right)\left({g}_{\text{D}}\right)+\left(\Delta {\nu }_{\text{Z}}-\Delta \nu bia{s}_{\text{Z}}\right)\left({q}_{0}^{2}-{q}_{{}_{1}}^{2}-{q}_{2}^{2}+{q}_{3}^{2}\right)-2\left(\Delta {\nu }_{\text{X}}-\Delta \nu bia{s}_{\text{X}}\right)\left({q}_{0}{q}_{2}-{q}_{1}{q}_{3}\right)+2\left(\Delta {\nu }_{\text{Y}}-\Delta \nu bia{s}_{\text{Y}}\right)\left({q}_{0}{q}_{1}+{q}_{2}{q}_{3}\right)\\ \Delta \theta bia{s}_{\text{X}}\\ \Delta \theta bia{s}_{\text{Y}}\\ \Delta \theta bia{s}_{\text{Z}}\\ \Delta \nu bia{s}_{\text{X}}\\ \Delta \nu bia{s}_{\text{Y}}\\ \Delta \nu bia{s}_{\text{Z}}\\ geomagneticFieldVecto{r}_{\text{N}}\\ geomagneticFieldVecto{r}_{\text{E}}\\ geomagneticFieldVecto{r}_{\text{D}}\\ magbia{s}_{\text{X}}\\ magbia{s}_{\text{Y}}\\ magbia{s}_{\text{Z}}\end{array}\right]$`

where

• ΔθX, ΔθY, ΔθZ –– Integrated gyroscope reading.

• ΔνX, ΔνY, ΔνZ –– Integrated accelerometer readings.

• Δt –– IMU sample time.

• gN, gE, gD –– Constant gravity vector in the NED frame.