# 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

### Description

example

filter = insfilterAsync creates an insfilterAsync 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

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

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

Data Types: single | double

Additive angular velocity process noise in the local navigation coordinate system in (rad/s)2, 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

Additive position process noise in the local navigation coordinate system in m2, 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

Additive velocity process noise in the local navigation coordinate system in (m/s)2, 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

Additive acceleration process noise in (m/s2)2, 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

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

Additive process noise variance from accelerometer bias in (m/s2)2, 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.

Additive process noise variance of geomagnetic vector in μT2, 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.

Additive process noise variance from magnetometer bias in μT2, 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 vector of the extended Kalman filter. The state values represent:

StateUnitsIndex
Orientation (quaternion parts)N/A1:4
Position (NED or ENU)m8:10
Velocity (NED or ENU)m/s11:13
Acceleration (NED or ENU)m/s214:16
Accelerometer Bias (XYZ)m/s217:19
Geomagnetic Field Vector (NED or ENU)μT23:25
Magnetometer Bias (XYZ)μT26:28

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

Load logged sensor data and ground truth pose.

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

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

expand all