# insfilterNonholonomic

Estimate pose with nonholonomic constraints

## Description

The `insfilterNonholonomic` object implements sensor fusion of inertial measurement unit (IMU) and GPS data to estimate pose in the NED (or ENU) reference frame. IMU data is derived from gyroscope and accelerometer data. The filter uses a 16-element state vector to track the orientation quaternion, velocity, position, and IMU sensor biases. The `insfilterNonholonomic` object uses an extended Kalman filter to estimate these quantities.

## Creation

### Syntax

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

### Description

example

````filter = insfilterNonholonomic` creates an `insfilterErrorState` object with default property values.```
````filter = insfilterNonholonomic('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 = insfilterNonholonomic(___,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 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`

Decimation factor for kinematic constraint correction, specified as a positive integer scalar.

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 finite numbers. Gyroscope bias is modeled as a lowpass filtered white noise process.

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

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

Data Types: `single` | `double`

Decay factor for gyroscope bias, specified as a scalar in the range [0,1]. A decay factor of `0` models gyroscope bias as a white noise process. A decay factor of `1` models the gyroscope bias as a random walk process.

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. Accelerometer bias is modeled as a lowpass filtered white noise process.

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

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

Decay factor for accelerometer bias, specified as a scalar in the range [0,1]. A decay factor of `0` models accelerometer bias as a white noise process. A decay factor of `1` models the accelerometer bias as a random walk process.

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)m8:10
Velocity (NED or ENU)m/s11:13
Accelerometer Bias (XYZ)m/s214:16

Data Types: `single` | `double`

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

Data Types: `single` | `double`

Velocity constraints noise in (m/s)2, specified as a nonnegative scalar.

Data Types: `single` | `double`

## Object Functions

 `correct` Correct states using direct state measurements for `insfilterNonholonomic` `residual` Residuals and residual covariances from direct state measurements for `insfilterNonholonomic` `fusegps` Correct states using GPS data for `insfilterNonholonomic` `residualgps` Residuals and residual covariance from GPS measurements for `insfilterNonholonomic` `pose` Current orientation and position estimate for `insfilterNonholonomic` `predict` Update states using accelerometer and gyroscope data for `insfilterNonholonomic` `reset` Reset internal states for `insfilterNonholonomic` `stateinfo` Display state vector information for `insfilterNonholonomic` `copy` Create copy of `insfitlerNonholonomic`

## Examples

collapse all

This example shows how to estimate the pose of a ground vehicle from logged IMU and GPS sensor measurements and ground truth orientation and position.

Load the logged data of a ground vehicle following a circular trajectory.

```load('loggedGroundVehicleCircle.mat','imuFs','localOrigin','initialState','initialStateCovariance','accelData',... 'gyroData','gpsFs','gpsLLA','Rpos','gpsVel','Rvel','trueOrient','truePos');```

Initialize the `insfilterNonholonomic` object.

```filt = insfilterNonholonomic; filt.IMUSampleRate = imuFs; filt.ReferenceLocation = localOrigin; filt.State = initialState; filt.StateCovariance = initialStateCovariance; imuSamplesPerGPS = imuFs/gpsFs;```

Log data for final metric computation. Use the `predict` object function to estimate filter state based on accelerometer and gyroscope data. Then correct the filter state according to GPS data.

```numIMUSamples = size(accelData,1); estOrient = quaternion.ones(numIMUSamples,1); estPos = zeros(numIMUSamples,3); gpsIdx = 1; for idx = 1:numIMUSamples predict(filt,accelData(idx,:),gyroData(idx,:)); %Predict filter state if (mod(idx,imuSamplesPerGPS) == 0) %Correct filter state fusegps(filt,gpsLLA(gpsIdx,:),Rpos,gpsVel(gpsIdx,:),Rvel); gpsIdx = gpsIdx + 1; end [estPos(idx,:),estOrient(idx,:)] = pose(filt); %Log estimated pose end```

Calculate and display RMS errors.

```posd = estPos - truePos; quatd = rad2deg(dist(estOrient,trueOrient)); msep = sqrt(mean(posd.^2)); fprintf('Position RMS Error\n\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',msep(1),msep(2),msep(3)); ```
```Position RMS Error X: 0.15, Y: 0.11, Z: 0.01 (meters) ```
``` fprintf('Quaternion Distance RMS Error\n\t%.2f (degrees)\n\n',sqrt(mean(quatd.^2)));```
```Quaternion Distance RMS Error 0.26 (degrees) ```

## Algorithms

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

`insfilterNonholonomic` uses a 16-axis error state 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}\\ gyrobia{s}_{X}\\ gyrobia{s}_{Y}\\ gyrobia{s}_{Z}\\ positio{n}_{N}\\ positio{n}_{E}\\ positio{n}_{D}\\ {v}_{N}\\ {v}_{E}\\ {v}_{D}\\ accelbia{s}_{X}\\ accelbia{s}_{Y}\\ accelbia{s}_{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.

• gyrobiasX, gyrobiasY, gyrobiasZ –– Bias in the gyroscope reading.

• 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.

• accelbiasX, accelbiasY, accelbiasZ –– Bias in the accelerometer reading.

Given the conventional formulation of the state transition function,

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

the predicted state estimate is:

`${x}_{k|k-1}=\left[\begin{array}{c}{q}_{0}+\Delta t\ast {q}_{1}\left(gyrobia{s}_{X}/2-gyr{o}_{X}/2\right)+\Delta t\ast {q}_{2}\ast \left(gyrobia{s}_{Y}/2-gyr{o}_{Y}/2\right)+\Delta t\ast {q}_{3}\ast \left(gyrobia{s}_{Z}/2-gyr{o}_{Z}/2\right)\\ {q}_{1}-\Delta t\ast {q}_{0}\left(gyrobia{s}_{X}/2-gyr{o}_{X}/2\right)+\Delta t\ast {q}_{3}\ast \left(gyrobia{s}_{Y}/2-gyr{o}_{Y}/2\right)-\Delta t\ast {q}_{2}\ast \left(gyrobia{s}_{Z}/2-gyr{o}_{Z}/2\right)\\ {q}_{2}-\Delta t\ast {q}_{3}\left(gyrobia{s}_{X}/2-gyr{o}_{X}/2\right)-\Delta t\ast {q}_{0}\ast \left(gyrobia{s}_{Y}/2-gyr{o}_{Y}/2\right)+\Delta t\ast {q}_{1}\ast \left(gyrobia{s}_{Z}/2-gyr{o}_{Z}/2\right)\\ {q}_{3}+\Delta t\ast {q}_{2}\left(gyrobia{s}_{X}/2-gyr{o}_{X}/2\right)-\Delta t\ast {q}_{1}\ast \left(gyrobia{s}_{Y}/2-gyr{o}_{Y}/2\right)-\Delta t\ast {q}_{0}\ast \left(gyrobia{s}_{Z}/2-gyr{o}_{Z}/2\right)\\ -gryobia{s}_{X}\ast \left(\Delta t\ast {\lambda }_{gyro}-1\right)\\ -gryobia{s}_{Y}\ast \left(\Delta t\ast {\lambda }_{gyro}-1\right)\\ -gryobia{s}_{Z}\ast \left(\Delta t\ast {\lambda }_{gyro}-1\right)\\ positio{n}_{N}+\Delta t\ast {v}_{N}\\ positio{n}_{E}+\Delta t\ast {v}_{E}\\ positio{n}_{D}+\Delta t\ast {v}_{D}\\ {v}_{N}+\Delta t\ast \left\{\begin{array}{l}{q}_{0}\ast \left({q}_{0}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)-{q}_{3}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)+{q}_{2}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)-{g}_{N}+\\ {q}_{2}\ast \left({q}_{1}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)-{q}_{2}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)+{q}_{0}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)+\\ {q}_{1}\ast \left({q}_{1}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)+{q}_{2}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)+{q}_{3}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)-\\ {q}_{3}\ast \left({q}_{3}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)+{q}_{0}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)-{q}_{1}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)\end{array}\right\}\\ {v}_{E}+\Delta t\ast \left\{\begin{array}{l}{q}_{0}\ast \left({q}_{3}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)+{q}_{0}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)-{q}_{1}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)-{g}_{E}-\\ {q}_{1}\ast \left({q}_{1}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)-{q}_{2}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)+{q}_{0}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)+\\ {q}_{2}\ast \left({q}_{1}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)+{q}_{2}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)+{q}_{3}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)+\\ {q}_{3}\ast \left({q}_{0}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)-{q}_{3}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)+{q}_{2}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)\end{array}\right\}\\ {v}_{D}+\Delta t\ast \left\{\begin{array}{l}{q}_{0}\ast \left({q}_{1}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)-{q}_{2}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)+{q}_{0}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)-{g}_{D}+\\ {q}_{1}\ast \left({q}_{3}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)+{q}_{0}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)-{q}_{1}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)-\\ {q}_{2}\ast \left({q}_{0}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)-{q}_{3}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)+{q}_{2}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)-\\ {q}_{3}\ast \left({q}_{1}\ast \left(accelbia{s}_{X}-acce{l}_{X}\right)+{q}_{2}\ast \left(accelbia{s}_{Y}-acce{l}_{Y}\right)+{q}_{3}\ast \left(accelbia{s}_{Z}-acce{l}_{Z}\right)\right)\end{array}\right\}\\ -accelbia{s}_{X}\ast \left(\Delta t\ast {\lambda }_{accel}-1\right)\\ -accelbia{s}_{Y}\ast \left(\Delta t\ast {\lambda }_{accel}-1\right)\\ -accelbia{s}_{Z}\ast \left(\Delta t\ast {\lambda }_{accel}-1\right)\end{array}\right]$`

where

• Δt –– IMU sample time.

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

• accelX, accelY, accelZ –– Acceleration vector in the body frame.

• λaccel –– Accelerometer bias decay factor.

• λgyro –– Gyroscope bias decay factor.

## References

[1] Munguía, R. "A GPS-Aided Inertial Navigation System in Direct Configuration." Journal of applied research and technology. Vol. 12, Number 4, 2014, pp. 803 – 814.