insfilterAsync

Estimate pose from asynchronous MARG and GPS data

Description

The insfilterAsync 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 data, respectively. The filter uses a 28-element state vector to track the orientation quaternion, velocity, position, MARG sensor biases, and geomagnetic vector. The insfilterAsync object uses a continuous-discrete extended Kalman filter to estimate these quantities.

Creation

Syntax

filter = insfilterAsync

filter = insfilterAsync('ReferenceFrame',RF)

filter = insfilterAsync(___,Name,Value)

Description

example

filter = insfilterAsync creates an insiflterAsync object to fuse asynchronous MARG and GPS data with default property values.

filter = insfilterAsync('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 = insfilterAsync(___,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

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

Reference location, specified as a three-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

`QuaternionNoise` — Additive quaternion process noise variance
`[1e-6 1e-6 1e-6 1e-6]` (default) | scalar | four-element row vector

Additive quaternion process noise variance, specified as a scalar or four-element vector of quaternion parts.

Data Types: single | double

`AngularVelocityNoise` — Additive angular velocity process noise in local navigation coordinate system ((rad/s)²)
`[0.005 0.005 0.005]` (default) | scalar | three-element row vector

Additive angular velocity process noise in the local navigation coordinate system in (rad/s)², specified as a scalar or three-element row vector of positive real finite numbers.

If AngularVelocityNoise is a row vector, the elements correspond to the noise in the x, y, and z axes of the local navigation coordinate system, respectively.
If AngularVelocityNoise is a scalar, the single element is applied to each axis.

Data Types: single | double

`PositionNoise` — Additive position process noise variance in local navigation coordinate system (m²)
`[1e-6 1e-6 1e-6]` (default) | scalar | three-element row vector

Additive position process noise in the local navigation coordinate system in m², specified as a scalar or three-element row vector of positive real finite numbers.

If PositionNoise is a row vector, the elements correspond to the noise in the x, y, and z axes of the local navigation coordinate system, respectively.
If PositionNoise is a scalar, the single element is applied to each axis.

Data Types: single | double

`VelocityNoise` — Additive velocity process noise variance in local navigation coordinate system ((m/s)²)
`[1e-6 1e-6 1e-6]` (default) | scalar | three-element row vector

Additive velocity process noise in the local navigation coordinate system in (m/s)², specified as a scalar or three-element row vector of positive real finite numbers.

If VelocityNoise is a row vector, the elements correspond to the noise in the x, y, and z axes of the local navigation coordinate system, respectively.
If VelocityNoise is a scalar, the single element is applied to each axis.

Data Types: single | double

`AccelerationNoise` — Additive acceleration process noise variance in local navigation coordinate system ((m/s²)²)
`[50 50 50]` (default) | scalar | three-element row vector

Additive acceleration process noise in (m/s²)², specified as a scalar or three-element row vector of positive real finite numbers.

If AccelerationNoise is a row vector, the elements correspond to the noise in the x, y, and z axes of the local navigation coordinate system, respectively.
If AccelerationNoise is 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-10 1e-10 1e-10]` (default) | scalar | three-element row vector

Additive process noise variance from the gyroscope bias in (rad/s)², specified as a scalar or three-element row vector of positive real finite numbers.

If GyroscopeBiasNoise is a row vector, the elements correspond to the noise in the x, y, and z axes of the gyroscope, respectively.
If GyroscopeBiasNoise is 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) | positive scalar | three-element row vector

Additive process noise variance from accelerometer bias in (m/s²)², specified as a scalar or three-element row vector of positive real numbers.

If AccelerometerBiasNoise is a row vector, the elements correspond to the noise in the x, y, and z axes of the accelerometer, respectively.
If AccelerometerBiasNoise is a scalar, the single element is applied to each axis.

`GeomagneticVectorNoise` — Additive process noise variance of geomagnetic vector in local navigation coordinate system (μT²)
`[1e-6 1e-6 1e-6]` (default) | positive scalar | three-element row vector

Additive process noise variance of geomagnetic vector in μT², specified as a scalar or three-element row vector of positive real numbers.

If GeomagneticVectorNoise is a row vector, the elements correspond to the noise in the x, y, and z axes of the local navigation coordinate system, respectively.
If GeomagneticVectorNoise is a scalar, the single element is applied to each axis.

`MagnetometerBiasNoise` — Additive process noise variance from magnetometer bias (μT²)
`[0.1 0.1 0.1]` (default) | positive scalar | three-element row vector

Additive process noise variance from magnetometer bias in μT², specified as a scalar or three-element row vector of positive real numbers.

If MagnetometerBiasNoise is a row vector, the elements correspond to the noise in the x, y, and z axes of the magnetometer, respectively.
If MagnetometerBiasNoise is a scalar, the single element is applied to each axis.

`State` — State vector of extended Kalman filter
28-element column vector

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

State	Units	Index
Orientation (quaternion parts)	N/A	1:4
Angular Velocity (XYZ)	rad/s	5:7
Position (NED or ENU)	m	8:10
Velocity (NED or ENU)	m/s	11:13
Acceleration (NED or ENU)	m/s²	14:16
Accelerometer Bias (XYZ)	m/s²	17:19
Gyroscope Bias (XYZ)	rad/s	20:22
Geomagnetic Field Vector (NED or ENU)	μT	23:25
Magnetometer Bias (XYZ)	μT	26:28

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 extended Kalman filter
`eye(28)` (default) | 28-by-28 matrix

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

Data Types: single | double

Object Functions

`predict`	Update states based on motion model for `insfilterAsync`
`fuseaccel`	Correct states using accelerometer data for `insfilterAsync`
`fusegyro`	Correct states using gyroscope data for `insfilterAsync`
`fusemag`	Correct states using magnetometer data for `insfilterAsync`
`fusegps`	Correct states using GPS data for `insfilterAsync`
`correct`	Correct states using direct state measurements for `insfilterAsync`
`residual`	Residuals and residual covariances from direct state measurements for `insfilterAsync`
`residualaccel`	Residuals and residual covariance from accelerometer measurements for `insfilterAsync`
`residualgps`	Residuals and residual covariance from GPS measurements for `insfilterAsync`
`residualmag`	Residuals and residual covariance from magnetometer measurements for `insfilterAsync`
`residualgyro`	Residuals and residual covariance from gyroscope measurements for `insfilterAsync`
`pose`	Current position, orientation, and velocity estimate for `insfilterAsync`
`reset`	Reset internal states for `insfilterAsync`
`stateinfo`	Display state vector information for `insfilterAsync`
`copy`	Create copy of `insfilterAsync`
`tune`	Tune `insfilterAsync` parameters to reduce estimation error
`tunernoise`	Noise structure of fusion filter

Examples

collapse all

Estimate Pose of UAV

Open Live Script

Load logged sensor data and ground truth pose.

load('uavshort.mat','refloc','initstate','imuFs', ...
    'accel','gyro','mag','lla','gpsvel', ...
    'trueOrient','truePos')

Create an INS filter to fuse asynchronous MARG and GPS data to estimate pose.

filt = insfilterAsync;
filt.ReferenceLocation = refloc;
filt.State = [initstate(1:4);0;0;0;initstate(5:10);0;0;0;initstate(11:end)];

Define sensor measurement noises. The noises were determined from datasheets and experimentation.

Rmag  = 80;
Rvel  = 0.0464;
Racc  = 800;
Rgyro = 1e-4;
Rpos  = 34;

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

N = size(accel,1);
p = zeros(N,3);
q = zeros(N,1,'quaternion');

gpsIdx = 1;

Fuse accelerometer, gyroscope, magnetometer, and GPS data. The outer loop predicts the filter forward one time step and fuses accelerometer and gyroscope data at the IMU sample rate.

for ii = 1:N
    
    % Predict the filter forward one time step
    predict(filt,1./imuFs);
    
    % Fuse accelerometer and gyroscope readings
    fuseaccel(filt,accel(ii,:),Racc);
    fusegyro(filt,gyro(ii,:),Rgyro);
    
    % Fuse magnetometer at 1/2 the IMU rate
    if ~mod(ii, fix(imuFs/2))
        fusemag(filt,mag(ii,:),Rmag);
    end
    
    % Fuse GPS once per second
    if ~mod(ii,imuFs)
        fusegps(filt,lla(gpsIdx,:),Rpos,gpsvel(gpsIdx,:),Rvel);
        gpsIdx = gpsIdx + 1;
    end
    
    % Log the current pose estimate
    [p(ii,:),q(ii)] = pose(filt);
    
end

Calculate the RMS errors between the known true position and orientation and the output from the asynchronous IMU filter.

posErr = truePos - p;
qErr = rad2deg(dist(trueOrient,q));

pRMS = sqrt(mean(posErr.^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.55, Y: 0.71, Z: 0.74 (meters)

fprintf('Quaternion Distance RMS Error\n');

Quaternion Distance RMS Error

fprintf('\t%.2f (degrees)\n\n', qRMS);

	4.72 (degrees)

Visualize the true position and the estimated position.

plot3(truePos(:,1),truePos(:,2),truePos(:,3),'LineWidth',2)
hold on
plot3(p(:,1),p(:,2),p(:,3),'r:','LineWidth',2)
grid on
xlabel('N (m)')
ylabel('E (m)')
zlabel('D (m)')

Algorithms

expand all

Dynamic Model Used in `insfilterAsync`

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

insfilterAsync implements a 28-axis continuous-discrete extended Kalman filter using sequential fusion. The filter relies on the assumption that individual sensor measurements are uncorrelated. The filter uses an omnidirectional motion model and assumes constant angular velocity and constant acceleration. The state is defined as:

$x = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \\ a n g V e l_{X} \\ a n g V e l_{Y} \\ a n g V e l_{Z} \\ 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} \\ ν_{N} \\ ν_{E} \\ ν_{D} \\ a c c e l_{N} \\ a c c e l_{E} \\ a c c e l_{D} \\ 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} \\ 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} \\ g e o m a g n e t i c F i e l d V e c t o r_{N} \\ g e o m a g n e t i c F i e l d V e c t o r_{E} \\ g e o m a g n e t i c F i e l d V e c t o r_{D} \\ m a g b i a s_{X} \\ m a g b i a s_{Y} \\ m a g b i a s_{Z} \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.
angVel_X, angVel_Y, angVel_Z –– Angular velocity relative to the platform's body frame.
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.
accel_N, accel_E, accel_D –– Acceleration of the platform in the local NED coordinate system.
accelbias_X, accelbias_Y, accelbias_Z –– Bias in the accelerometer reading.
gyrobias_X, gyrobias_Y, gyrobias_Z –– Bias in the gyroscope 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 process equation, $\dot{x} = f (x) + w$ , w is the process noise, $\dot{x}$ is the derivative of x, and:

$f (x) = [\begin{matrix} \frac{- (q_{1}) (a n g V e l_{X}) - (q_{2}) (a n g V e l_{Y}) - (q_{3}) (a n g V e l_{Z})}{2} \\ \frac{(q_{0}) (a n g V e l_{X}) - (q_{3}) (a n g V e l_{Y}) + (q_{1}) (a n g V e l_{Z})}{2} \\ \frac{(q_{3}) (a n g V e l_{X}) + (q_{0}) (a n g V e l_{Y}) - (q_{1}) (a n g V e l_{Z})}{2} \\ \frac{(q_{1}) (a n g V e l_{X}) - (q_{2}) (a n g V e l_{Y}) + (q_{0}) (a n g V e l_{Z})}{2} \\ 0 \\ 0 \\ 0 \\ ν_{N} \\ ν_{E} \\ ν_{D} \\ a c c e l_{N} \\ a c c e l_{E} \\ a c c e l_{D} \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{matrix}]$

Documentation

insfilterAsync

Description

Creation

Syntax

Description

Properties

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

`QuaternionNoise` — Additive quaternion process noise variance
`[1e-6 1e-6 1e-6 1e-6]` (default) | scalar | four-element row vector

`AngularVelocityNoise` — Additive angular velocity process noise in local navigation coordinate system ((rad/s)²)
`[0.005 0.005 0.005]` (default) | scalar | three-element row vector

`PositionNoise` — Additive position process noise variance in local navigation coordinate system (m²)
`[1e-6 1e-6 1e-6]` (default) | scalar | three-element row vector

`VelocityNoise` — Additive velocity process noise variance in local navigation coordinate system ((m/s)²)
`[1e-6 1e-6 1e-6]` (default) | scalar | three-element row vector

`AccelerationNoise` — Additive acceleration process noise variance in local navigation coordinate system ((m/s²)²)
`[50 50 50]` (default) | scalar | three-element row vector

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

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

`GeomagneticVectorNoise` — Additive process noise variance of geomagnetic vector in local navigation coordinate system (μT²)
`[1e-6 1e-6 1e-6]` (default) | positive scalar | three-element row vector

`MagnetometerBiasNoise` — Additive process noise variance from magnetometer bias (μT²)
`[0.1 0.1 0.1]` (default) | positive scalar | three-element row vector

`State` — State vector of extended Kalman filter
28-element column vector

`StateCovariance` — State error covariance for extended Kalman filter
`eye(28)` (default) | 28-by-28 matrix

Object Functions

Examples

Estimate Pose of UAV

Algorithms

Dynamic Model Used in `insfilterAsync`

Extended Capabilities

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

See Also

Sensor Fusion and Tracking Toolbox Documentation

Support

Documentation

insfilterAsync

Description

Creation

Syntax

Description

Properties

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

QuaternionNoise — Additive quaternion process noise variance [1e-6 1e-6 1e-6 1e-6] (default) | scalar | four-element row vector

AngularVelocityNoise — Additive angular velocity process noise in local navigation coordinate system ((rad/s)2) [0.005 0.005 0.005] (default) | scalar | three-element row vector

PositionNoise — Additive position process noise variance in local navigation coordinate system (m2) [1e-6 1e-6 1e-6] (default) | scalar | three-element row vector

VelocityNoise — Additive velocity process noise variance in local navigation coordinate system ((m/s)2) [1e-6 1e-6 1e-6] (default) | scalar | three-element row vector

AccelerationNoise — Additive acceleration process noise variance in local navigation coordinate system ((m/s2)2) [50 50 50] (default) | scalar | three-element row vector

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

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

GeomagneticVectorNoise — Additive process noise variance of geomagnetic vector in local navigation coordinate system (μT2) [1e-6 1e-6 1e-6] (default) | positive scalar | three-element row vector

MagnetometerBiasNoise — Additive process noise variance from magnetometer bias (μT2) [0.1 0.1 0.1] (default) | positive scalar | three-element row vector

State — State vector of extended Kalman filter 28-element column vector

StateCovariance — State error covariance for extended Kalman filter eye(28) (default) | 28-by-28 matrix

Object Functions

Examples

Estimate Pose of UAV

Algorithms

Dynamic Model Used in insfilterAsync

Extended Capabilities

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

See Also

Sensor Fusion and Tracking Toolbox Documentation

Support

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

`QuaternionNoise` — Additive quaternion process noise variance
`[1e-6 1e-6 1e-6 1e-6]` (default) | scalar | four-element row vector

`AngularVelocityNoise` — Additive angular velocity process noise in local navigation coordinate system ((rad/s)²)
`[0.005 0.005 0.005]` (default) | scalar | three-element row vector

`PositionNoise` — Additive position process noise variance in local navigation coordinate system (m²)
`[1e-6 1e-6 1e-6]` (default) | scalar | three-element row vector

`VelocityNoise` — Additive velocity process noise variance in local navigation coordinate system ((m/s)²)
`[1e-6 1e-6 1e-6]` (default) | scalar | three-element row vector

`AccelerationNoise` — Additive acceleration process noise variance in local navigation coordinate system ((m/s²)²)
`[50 50 50]` (default) | scalar | three-element row vector

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

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

`GeomagneticVectorNoise` — Additive process noise variance of geomagnetic vector in local navigation coordinate system (μT²)
`[1e-6 1e-6 1e-6]` (default) | positive scalar | three-element row vector

`MagnetometerBiasNoise` — Additive process noise variance from magnetometer bias (μT²)
`[0.1 0.1 0.1]` (default) | positive scalar | three-element row vector

`State` — State vector of extended Kalman filter
28-element column vector

`StateCovariance` — State error covariance for extended Kalman filter
`eye(28)` (default) | 28-by-28 matrix

Dynamic Model Used in `insfilterAsync`

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