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

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.

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

Input Arguments

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

IMUSampleRate — Sample rate of the IMU (Hz)

100 (default) | positive scalar

Sample rate of the 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

DecimationFactor — Decimation factor for kinematic constraint correction

2 (default) | positive integer scalar

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

Data Types: single | double

GyroscopeNoise — Multiplicative process noise variance from gyroscope (rad/s)²

[4.8e-6 4.8e-6 4.8e-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 the x, y, and z axes of the gyroscope.

Data Types: single | double

GyroscopeBiasNoise — Multiplicative process noise variance from gyroscope bias (rad/s)²

[4e-14 4e-14 4e-14] (default) | scalar | 3-element row vector

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

GyroscopeBiasDecayFactor — Decay factor for gyroscope bias

0.999 (default) | scalar in the range [0,1]

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

AccelerometerNoise — Multiplicative process noise variance from accelerometer (m/s²)²

[4.8e-2 4.8e-2 4.8e-2] (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 — Multiplicative process noise variance from accelerometer bias (m/s²)²

[4e-14 4e-14 4e-14] (default) | positive scalar | 3-element row vector

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

AccelerometerBiasDecayFactor — Decay factor for accelerometer bias

0.9999 (default) | scalar in the range [0,1]

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 — State vector of extended Kalman filter

[1;zeros(15,1)] | 16-element column vector

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

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

Data Types: single | double

StateCovariance — State error covariance for extended Kalman filter

eye(16) (default) | 16-by-16 matrix

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

Data Types: single | double

ZeroVelocityConstraintNoise — Velocity constraints noise (m/s)²

1e-2 (default) | nonnegative scalar

Velocity constraints noise in (m/s)², 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
tune	Tune insfilterNonholonomic parameters to reduce estimation error
copy	Create copy of insfitlerNonholonomic

Examples

Estimate Pose of Ground Vehicle

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

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

• gyrobias_X, gyrobias_Y, gyrobias_Z –– Bias in the gyroscope reading.

• position_N, position_E, position_D –– Position of the platform in the local NED coordinate system.

• ν_N, ν_E, ν_D –– Velocity of the platform in the local NED coordinate system.

• accelbias_X, accelbias_Y, accelbias_Z –– Bias in the accelerometer reading.

Given the conventional formulation of the state transition function,

$$x_{k|k-1}=f({\widehat{x}}_{k-1|k-1})$$

the predicted state estimate is:

$$x_{k|k-1}=\left[\begin{array}{c}{q}_0+\Delta t\ast q_1(gyrobia{s}_X/2-gyr{o}_X/2)+\Delta t\ast q_2\ast (gyrobia{s}_Y/2-gyr{o}_Y/2)+\Delta t\ast q_3\ast (gyrobia{s}_Z/2-gyr{o}_Z/2)\\ q_1-\Delta t\ast q_0(gyrobia{s}_X/2-gyr{o}_X/2)+\Delta t\ast q_3\ast (gyrobia{s}_Y/2-gyr{o}_Y/2)-\Delta t\ast q_2\ast (gyrobia{s}_Z/2-gyr{o}_Z/2)\\ q_2-\Delta t\ast q_3(gyrobia{s}_X/2-gyr{o}_X/2)-\Delta t\ast q_0\ast (gyrobia{s}_Y/2-gyr{o}_Y/2)+\Delta t\ast q_1\ast (gyrobia{s}_Z/2-gyr{o}_Z/2)\\ q_3+\Delta t\ast q_2(gyrobia{s}_X/2-gyr{o}_X/2)-\Delta t\ast q_1\ast (gyrobia{s}_Y/2-gyr{o}_Y/2)-\Delta t\ast q_0\ast (gyrobia{s}_Z/2-gyr{o}_Z/2)\\ -gryobia{s}_X\ast (\Delta t\ast {\lambda }_{gyro}-1)\\ -gryobia{s}_Y\ast (\Delta t\ast {\lambda }_{gyro}-1)\\ -gryobia{s}_Z\ast (\Delta t\ast {\lambda }_{gyro}-1)\\ 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 (\Delta t\ast {\lambda }_{accel}-1)\\ -accelbia{s}_Y\ast (\Delta t\ast {\lambda }_{accel}-1)\\ -accelbia{s}_Z\ast (\Delta t\ast {\lambda }_{accel}-1)\end{array}\right]$$

where

• Δt –– IMU sample time.

• g_N, g_E, g_D –– Constant gravity vector in the NED frame.

• accel_X, accel_Y, accel_Z –– 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.

Extended Capabilities

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

Version History

Introduced in R2018b