# 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 `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.

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

expand all

## Version History

Introduced in R2019a