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

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.

filter = insfilterMARG(___,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 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-9 (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)²

1e-10 (default) | positive 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 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

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

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

1e-4 (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.

• 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

GeomagneticVectorNoise — Additive process noise for geomagnetic vector (µT²)

1e-6 (default) | positive scalar | 3-element row vector

Additive process noise for geomagnetic vector in µT², 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

MagnetometerBiasNoise — Additive process noise for magnetometer bias (µT²)

0.1 (default) | positive scalar | 3-element row vector

Additive process noise for magnetometer bias in µT², 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 — State vector of extended Kalman filter

22-element column vector

State vector of the extended Kalman filter. 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
Delta Angle Bias (XYZ)	rad	11:13
Delta Velocity Bias (XYZ)	m/s	14:16
Geomagnetic Field Vector (NED or ENU)	µT	17:19
Magnetometer Bias (XYZ)	µT	20:22

Data Types: single | double

StateCovariance — State error covariance for extended Kalman filter

eye(22)*1e-6 (default) | 22-by-22 matrix

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

Estimate Pose of UAV

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;
magFs = imuFs/2; % Hz
gpuFs = 1;       % Hz
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/magFs))            % Fuse magnetometer
       f.fusemag(mag(ii,:),Rmag);
   end
  
   if ~mod(ii,imuFs/gpuFs)                   % 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.69 (meters)

    
fprintf('Quaternion Distance RMS Error\n\t%.2f (degrees)\n\n',qRMS);

Quaternion Distance RMS Error
	0.20 (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

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

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

• Δθbias_X, Δθbias_Y, Δθbias_Z –– Bias in the integrated gyroscope reading.

• Δνbias_X, Δνbias_Y, Δνbias_Z –– Bias in the integrated accelerometer reading.

• geomagneticFieldVector_N, geomagneticFieldVector_E, geomagneticFieldVector_D –– Estimate of the geomagnetic field vector at the reference location.

• magbias_X, magbias_Y, magbias_Z –– Bias in the magnetometer readings.

Given the conventional formation of the predicted state estimate,

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

u_k 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}_0^{\text{'}}-q_1{q}_1^{\text{'}}-q_2{q}_2^{\text{'}}-q_3{q}_3^{\text{'}}\\ q_1{q}_0^{\text{'}}+q_0{q}_1^{\text{'}}-q_3{q}_2^{\text{'}}+q_2{q}_3^{\text{'}}\\ q_2{q}_0^{\text{'}}+q_3{q}_1^{\text{'}}+q_0{q}_2^{\text{'}}-q_1{q}_3^{\text{'}}\\ q_3{q}_0^{\text{'}}-q_2{q}_1^{\text{'}}+q_1{q}_2^{\text{'}}+q_0{q}_3^{\text{'}}\\ 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]$$

In the equation, (q₀^', q₁^', q₂^', q₃^') is the quaternion that accounts for the orientation change from one step to the next step. Assuming the orientation change is small, then the rotation vector can be approximated as (Δθ_X − Δθbias_X, Δθ_Y − Δθbias_Y, Δθ_Z − Δθbias_Z), where Δθ_X, Δθ_Y, Δθ_Z are the integrated gyroscope readings. (q₀^', q₁^', q₂^', q₃^') is then obtained by converting the approximated rotation vector to a quaternion. In each calculation, the quaternion is normalized such that the length of the quaternion is 1 and its real part q₀ is nonnegative.

Additionally,

• Δν_X, Δν_Y, Δν_Z –– Integrated accelerometer readings.

• Δ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 R2018b