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

filter = insfilterErrorState creates an insfilterErrorState object with default property values.

example

filter = insfilterErrorState('ReferenceFrame',RF) allows you to specify the reference frame, RF, of the filter.

filter = insfilterErrorState(___,Name=Value) sets one or more properties using name-value arguments in addition to any of the previous input arguments.

Input Arguments

expand all

`RF` — Local navigation coordinate system
`'NED'` (default) | `'ENU'`

Local navigation coordinate system, specified as either 'NED' (North-East-Down) or 'ENU' (East-North-Up).

Data Types: char | string

Properties

expand all

`IMUSampleRate` — Sample rate of IMU (Hz)
`100` (default) | positive scalar

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

Data Types: single | double

`ReferenceLocation` — Reference location (deg, deg, meters)
`[0 0 0]` (default) | 3-element positive row vector

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

`GyroscopeNoise` — Multiplicative process noise variance from gyroscope ((rad/s)²)
`[1e-6 1e-6 1e-6]` (default) | scalar | 3-element row vector

Multiplicative process noise variance from the gyroscope in (rad/s)², 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

`GyroscopeBiasNoise` — Additive process noise variance from gyroscope bias ((rad/s)²)
`[1e-9 1e-9 1e-9]` (default) | scalar | 3-element row vector

Additive process noise variance from the gyroscope bias in (rad/s)², 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

`AccelerometerNoise` — Multiplicative process noise variance from accelerometer ((m/s²)²)
`[1e-4 1e-4 1e-4]` (default) | scalar | 3-element row vector

Multiplicative process noise variance from the accelerometer in (m/s²)², 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

`AccelerometerBiasNoise` — Additive process noise variance from accelerometer bias ((m/s²)²)
`[1e-4 1e-4 1e-4]` (default) | scalar | 3-element row vector

Additive process noise variance from accelerometer bias in (m/s²)², 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` — State vector of Kalman filter
`[1;zeros(15,1);1]` (default) | 17-element column vector

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

State	Units	Index
Orientation (quaternion parts)	N/A	1:4
Position (NED or ENU)	m	5:7
Velocity (NED or ENU)	m/s	8:10
Gyroscope Bias (XYZ)	rad/s	11:13
Accelerometer Bias (XYZ)	m/s²	14:16
Visual Odometry Scale (XYZ)	N/A	17

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

Data Types: single | double

`StateCovariance` — State error covariance for Kalman filter
`ones(16)` (default) | 16-by-16 matrix

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 Covariance	Row/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`
`tune`	Tune `insfilterErrorState` parameters to reduce estimation error
`copy`	Create copy of `insfilterErrorState`

Examples

collapse all

Estimate Pose of Ground Vehicle

Open Live Script

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')

Figure contains an axes object. The axes object with xlabel N (m), ylabel E (m) contains 2 objects of type line. These objects represent 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 = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \\ p o s i t i o n_{N} \\ p o s i t i o n_{E} \\ p o s i t i o n_{D} \\ v_{N} \\ v_{E} \\ v_{D} \\ g y r o b i a s_{X} \\ g y r o b i a s_{Y} \\ g y r o b i a s_{Z} \\ a c c e l b i a s_{X} \\ a c c e l b i a s_{Y} \\ \begin{array}{l} a c c e l b i a s_{Z} \\ \begin{matrix} s c a l e F a c t o r \end{matrix} \end{array} \end{matrix}]$

where

q₀, q₁, q₂, q₃ –– Parts of orientation quaternion. The orientation quaternion represents a frame rotation from the platform's current orientation to the local NED coordinate system.
position_N, position_E, position_D –– Position of the platform in the local NED coordinate system.
gyrobias_X, gyrobias_Y, gyrobias_Z –– Bias in the gyroscope reading.
accelbias_X, accelbias_Y, accelbias_Z –– 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 ({\hat{x}}_{k - 1 | k - 1})$

the predicted state estimate is:

$x_{k | k - 1} = [\begin{matrix} q_{0} + Δ t * q_{1} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) + Δ t * q_{2} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) + Δ t * q_{3} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ q_{1} - Δ t * q_{0} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) + Δ t * q_{3} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) - Δ t * q_{2} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ q_{2} - Δ t * q_{3} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) - Δ t * q_{0} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) + Δ t * q_{1} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ q_{3} + Δ t * q_{2} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) - Δ t * q_{1} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) - Δ t * q_{0} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ p o s i t i o n_{N} + Δ t * v_{N} \\ p o s i t i o n_{E} + Δ t * v_{E} \\ p o s i t i o n_{D} + Δ t * v_{D} \\ v_{N} - Δ t * {\begin{cases} q_{0} * (q_{0} * (a c c e l b i a s_{X} - a c c e l_{X}) - q_{3} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{2} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + g_{N} + \\ q_{2} * (q_{1} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{2} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + \\ q_{1} * (q_{1} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{2} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{3} * (a c c e l b i a s_{Z} - a c c e l_{Z})) - \\ q_{3} * (q_{3} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{1} * (a c c e l b i a s_{Z} - a c c e l_{Z})) \end{cases}} \\ v_{E} - Δ t * {\begin{cases} q_{0} * (q_{3} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{1} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + g_{E} - \\ q_{1} * (q_{1} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{2} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + \\ q_{2} * (q_{1} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{2} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{3} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + \\ q_{3} * (q_{0} * (a c c e l b i a s_{X} - a c c e l_{X}) - q_{3} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{2} * (a c c e l b i a s_{Z} - a c c e l_{Z})) \end{cases}} \\ v_{D} - Δ t * {\begin{cases} q_{0} * (q_{1} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{2} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + g_{D} + \\ q_{1} * (q_{3} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{1} * (a c c e l b i a s_{Z} - a c c e l_{Z})) - \\ q_{2} * (q_{0} * (a c c e l b i a s_{X} - a c c e l_{X}) - q_{3} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{2} * (a c c e l b i a s_{Z} - a c c e l_{Z})) - \\ q_{3} * (q_{1} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{2} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{3} * (a c c e l b i a s_{Z} - a c c e l_{Z})) \end{cases}} \\ g y r o b i a s_{X} \\ g y r o b i a s_{Y} \\ g y r o b i a s_{Z} \\ a c c e l b i a s_{X} \\ a c c e l b i a s_{Y} \\ a c c e l b i a s_{Z} \\ s c a l e F a c t o r \end{matrix}]$

where

Δt –– IMU sample time.
g_N, g_E, g_D –– Constant gravity vector in the NED frame.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2019a

insfilterErrorState

Description

Creation

Syntax

Description

Input Arguments

RF — Local navigation coordinate system 'NED' (default) | 'ENU'

Properties

IMUSampleRate — Sample rate of IMU (Hz) 100 (default) | positive scalar

ReferenceLocation — Reference location (deg, deg, meters) [0 0 0] (default) | 3-element positive row vector

GyroscopeNoise — Multiplicative process noise variance from gyroscope ((rad/s)2) [1e-6 1e-6 1e-6] (default) | scalar | 3-element row vector

GyroscopeBiasNoise — Additive process noise variance from gyroscope bias ((rad/s)2) [1e-9 1e-9 1e-9] (default) | scalar | 3-element row vector

AccelerometerNoise — Multiplicative process noise variance from accelerometer ((m/s2)2) [1e-4 1e-4 1e-4] (default) | scalar | 3-element row vector

AccelerometerBiasNoise — Additive process noise variance from accelerometer bias ((m/s2)2) [1e-4 1e-4 1e-4] (default) | scalar | 3-element row vector

State — State vector of Kalman filter [1;zeros(15,1);1] (default) | 17-element column vector

StateCovariance — State error covariance for Kalman filter ones(16) (default) | 16-by-16 matrix

Object Functions

Examples

Estimate Pose of Ground Vehicle

Algorithms

Extended Capabilities

C/C++ Code Generation Generate C and C++ code using MATLAB® Coder™.

Version History

See Also

`RF` — Local navigation coordinate system
`'NED'` (default) | `'ENU'`

`IMUSampleRate` — Sample rate of IMU (Hz)
`100` (default) | positive scalar

`ReferenceLocation` — Reference location (deg, deg, meters)
`[0 0 0]` (default) | 3-element positive row vector

`GyroscopeNoise` — Multiplicative process noise variance from gyroscope ((rad/s)²)
`[1e-6 1e-6 1e-6]` (default) | scalar | 3-element row vector

`GyroscopeBiasNoise` — Additive process noise variance from gyroscope bias ((rad/s)²)
`[1e-9 1e-9 1e-9]` (default) | scalar | 3-element row vector

`AccelerometerNoise` — Multiplicative process noise variance from accelerometer ((m/s²)²)
`[1e-4 1e-4 1e-4]` (default) | scalar | 3-element row vector

`AccelerometerBiasNoise` — Additive process noise variance from accelerometer bias ((m/s²)²)
`[1e-4 1e-4 1e-4]` (default) | scalar | 3-element row vector

`State` — State vector of Kalman filter
`[1;zeros(15,1);1]` (default) | 17-element column vector

`StateCovariance` — State error covariance for Kalman filter
`ones(16)` (default) | 16-by-16 matrix

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.