# insfilterErrorState

Estimate pose from IMU, GPS, and monocular visual odometry (MVO) data

## Description

The `insfilterErrorState` object implements sensor fusion of IMU, GPS, and monocular visual odometry (MVO) data to estimate pose in the NED (or ENU) reference frame. The filter uses a 17-element state vector to track the orientation `quaternion`, velocity, position, IMU sensor biases, and the MVO scaling factor. The `insfilterErrorState` object uses an error-state Kalman filter to estimate these quantities.

## Creation

### Syntax

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

### Description

example

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

Data Types: `single` | `double`

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

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

Additive process noise variance from 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, respectively.

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

State vector of the extended Kalman filter, specified as a 17-element column vector. The state values represent:

StateUnitsIndex
Orientation (quaternion parts)N/A1:4
Position (NED or ENU)m5:7
Velocity (NED or ENU)m/s8:10
Accelerometer Bias (XYZ)m/s214:16
Visual Odometry Scale (XYZ)N/A17

The default initial state corresponds to an object at rest located at `[0 0 0]` in geodetic LLA coordinates.

Data Types: `single` | `double`

State error covariance for the Kalman filter, specified as a 16-by-16-element matrix of real numbers. The state error covariance values represent:

State CovarianceRow/Column Index
δ Rotation Vector (XYZ)1:3
δ Position (NED or ENU)4:6
δ Velocity (NED or ENU)7:9
δ Gyroscope Bias (XYZ)10:12
δ Accelerometer Bias (XYZ)13:15
δ Visual Odometry Scale (XYZ)16

Note that because this is an error-state Kalman filter, it tracks the errors in the states. δ represents the error in the corresponding state.

Data Types: `single` | `double`

## Object Functions

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

## Examples

collapse all

Load logged data of a ground vehicle following a circular trajectory. The `.mat` file contains IMU and GPS sensor measurements and ground truth orientation and position.

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

Create an INS filter to fuse IMU and GPS data using an error-state Kalman filter.

```initialState = [compact(trueOrient(1)),truePos(1,:),-6.8e-3,2.5002,0,zeros(1,6),1].'; filt = insfilterErrorState; filt.IMUSampleRate = imuFs; filt.ReferenceLocation = localOrigin; filt.State = initialState; filt.StateCovariance = initialStateCovariance;```

Preallocate variables for position and orientation. Allocate a variable for indexing into the GPS data.

```numIMUSamples = size(accelData,1); estOrient = ones(numIMUSamples,1,'quaternion'); estPos = zeros(numIMUSamples,3); gpsIdx = 1;```

Fuse accelerometer, gyroscope, and GPS data. The outer loop predicts the filter forward at the fastest sample rate (the IMU sample rate).

```for idx = 1:numIMUSamples % Use predict to estimate the filter state based on the accelData and % gyroData arrays. predict(filt,accelData(idx,:),gyroData(idx,:)); % GPS data is collected at a lower sample rate than IMU data. Fuse GPS % data at the lower rate. if mod(idx, imuFs / gpsFs) == 0 % Correct the filter states based on the GPS data. fusegps(filt,gpsLLA(gpsIdx,:),Rpos,gpsVel(gpsIdx,:),Rvel); gpsIdx = gpsIdx + 1; end % Log the current pose estimate [estPos(idx,:), estOrient(idx,:)] = pose(filt); end```

Calculate the RMS errors between the known true position and orientation and the output from the error-state filter.

```pErr = truePos - estPos; qErr = rad2deg(dist(estOrient,trueOrient)); pRMS = sqrt(mean(pErr.^2)); qRMS = sqrt(mean(qErr.^2)); fprintf('Position RMS Error\n');```
```Position RMS Error ```
`fprintf('\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));`
``` X: 0.40, Y: 0.24, Z: 0.05 (meters) ```
`fprintf('Quaternion Distance RMS Error\n');`
```Quaternion Distance RMS Error ```
`fprintf('\t%.2f (degrees)\n\n',qRMS);`
``` 0.30 (degrees) ```

Visualize the true position and the estimated position.

```plot(truePos(:,1),truePos(:,2),estPos(:,1),estPos(:,2),'r:','LineWidth',2) grid on axis square xlabel('N (m)') ylabel('E (m)') legend('Ground Truth','Estimation')```

## Algorithms

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

`insfilterErrorState` uses a 17-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}\\ positio{n}_{N}\\ positio{n}_{E}\\ positio{n}_{D}\\ {v}_{N}\\ {v}_{E}\\ {v}_{D}\\ gyrobia{s}_{X}\\ gyrobia{s}_{Y}\\ gyrobia{s}_{Z}\\ accelbia{s}_{X}\\ accelbia{s}_{Y}\\ \begin{array}{l}accelbia{s}_{Z}\\ \begin{array}{c}scaleFactor\end{array}\end{array}\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.

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

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

• scaleFactor –– Scale factor of the pose estimate.

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)\\ 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\}\\ gyrobia{s}_{X}\\ gyrobia{s}_{Y}\\ gyrobia{s}_{Z}\\ accelbia{s}_{X}\\ accelbia{s}_{Y}\\ accelbia{s}_{Z}\\ scaleFactor\end{array}\right]$`

where

• Δt –– IMU sample time.

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