# ahrsfilter

Orientation from accelerometer, gyroscope, and magnetometer readings

## Description

The `ahrsfilter`

System object™ fuses accelerometer, magnetometer, and gyroscope sensor data to estimate device
orientation.

To estimate device orientation:

Create the

`ahrsfilter`

object and set its properties.Call the object with arguments, as if it were a function.

To learn more about how System objects work, see What Are System Objects?

## Creation

### Description

returns an indirect Kalman
filter System object, `FUSE`

= ahrsfilter`FUSE`

, for sensor fusion of accelerometer, gyroscope, and
magnetometer data to estimate device orientation and angular velocity. The filter uses a
12-element state vector to track the estimation error for the orientation, the gyroscope
bias, the linear acceleration, and the magnetic disturbance.

returns an ahrsfilter System object that fuses accelerometer, gyroscope, and magnetometer data to estimate
device orientation relative to the reference frame `FUSE`

= ahrsfilter(`'ReferenceFrame'`

,`RF`

)`RF`

. Specify
`RF`

as `'NED'`

(North-East-Down) or
`'ENU'`

(East-North-Up). The default value is 'NED'.

sets each property `FUSE`

= ahrsfilter(___,`Name,Value`

)`Name`

to the specified `Value`

.
Unspecified properties have default values.

## Properties

Unless otherwise indicated, properties are *nontunable*, which means you cannot change their
values after calling the object. Objects lock when you call them, and the
`release`

function unlocks them.

If a property is *tunable*, you can change its value at
any time.

For more information on changing property values, see System Design in MATLAB Using System Objects.

`SampleRate`

— Input sample rate of sensor data (Hz)

`100`

(default) | positive scalar

Input sample rate of the sensor data in Hz, specified as a positive scalar.

**Tunable: **No

**Data Types: **`single`

| `double`

`DecimationFactor`

— Decimation factor

`1`

(default) | positive integer

Decimation factor by which to reduce the input sensor data rate as part of the fusion algorithm, specified as a positive integer.

The number of rows of the inputs –– `accelReadings`

,
`gyroReadings`

, and `magReadings`

–– must be a
multiple of the decimation factor.

**Data Types: **`single`

| `double`

`AccelerometerNoise`

— Variance of accelerometer signal noise ((m/s^{2})^{2})

`0.00019247`

(default) | positive real scalar

Variance of accelerometer signal noise in
(m/s^{2})^{2}, specified as a positive
real scalar.

**Tunable: **Yes

**Data Types: **`single`

| `double`

`MagnetometerNoise`

— Variance of magnetometer signal noise (μT^{2})

`0.1`

(default) | positive real scalar

Variance of magnetometer signal noise in μT^{2}, specified
as a positive real scalar.

**Tunable: **Yes

**Data Types: **`single`

| `double`

`GyroscopeNoise`

— Variance of gyroscope signal noise ((rad/s)^{2})

`9.1385e-5`

(default) | positive real scalar

Variance of gyroscope signal noise in (rad/s)^{2}, specified
as a positive real scalar.

**Tunable: **Yes

**Data Types: **`single`

| `double`

`GyroscopeDriftNoise`

— Variance of gyroscope offset drift ((rad/s)^{2})

`3.0462e-13`

(default) | positive real scalar

Variance of gyroscope offset drift in (rad/s)^{2}, specified
as a positive real scalar.

**Tunable: **Yes

**Data Types: **`single`

| `double`

`LinearAccelerationNoise`

— Variance of linear acceleration noise (m/s^{2})^{2}

`0.0096236`

(default) | positive real scalar

Variance of linear acceleration noise in
(m/s^{2})^{2}, specified as a positive
real scalar. Linear acceleration is modeled as a lowpass-filtered white noise
process.

**Tunable: **Yes

**Data Types: **`single`

| `double`

`LinearAccelerationDecayFactor`

— Decay factor for linear acceleration drift

`0.5`

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

Decay factor for linear acceleration drift, specified as a scalar in the range
[0,1). If linear acceleration is changing quickly, set
`LinearAcclerationDecayFactor`

to a lower value. If linear
acceleration changes slowly, set `LinearAcclerationDecayFactor`

to a
higher value. Linear acceleration drift is modeled as a lowpass-filtered white noise
process.

**Tunable: **Yes

**Data Types: **`single`

| `double`

`MagneticDisturbanceNoise`

— Variance of magnetic disturbance noise (μT^{2})

`0.5`

(default) | real finite positive scalar

Variance of magnetic disturbance noise in μT^{2}, specified
as a real finite positive scalar.

**Tunable: **Yes

**Data Types: **`single`

| `double`

`MagneticDisturbanceDecayFactor`

— Decay factor for magnetic disturbance

`0.5`

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

Decay factor for magnetic disturbance, specified as a positive scalar in the range [0,1]. Magnetic disturbance is modeled as a first order Markov process.

**Tunable: **Yes

**Data Types: **`single`

| `double`

`InitialProcessNoise`

— Covariance matrix for process noise

12-by-12 matrix

Covariance matrix for process noise, specified as a 12-by-12 matrix. The default is:

Columns 1 through 6 0.000006092348396 0 0 0 0 0 0 0.000006092348396 0 0 0 0 0 0 0.000006092348396 0 0 0 0 0 0 0.000076154354947 0 0 0 0 0 0 0.000076154354947 0 0 0 0 0 0 0.000076154354947 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 Columns 7 through 12 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.009623610000000 0 0 0 0 0 0 0.009623610000000 0 0 0 0 0 0 0.009623610000000 0 0 0 0 0 0 0.600000000000000 0 0 0 0 0 0 0.600000000000000 0 0 0 0 0 0 0.600000000000000

The initial process covariance matrix accounts for the error in the process model.

**Data Types: **`single`

| `double`

`ExpectedMagneticFieldStrength`

— Expected estimate of magnetic field strength (μT)

`50`

(default) | real positive scalar

Expected estimate of magnetic field strength in μT, specified as a real positive scalar. The expected magnetic field strength is an estimate of the magnetic field strength of the Earth at the current location.

**Tunable: **Yes

**Data Types: **`single`

| `double`

`OrientationFormat`

— Output orientation format

`'quaternion'`

(default) | `'Rotation matrix'`

Output orientation format, specified as `'quaternion'`

or
`'Rotation matrix'`

. The size of the output depends on the input
size, *N*, and the output orientation format:

`'quaternion'`

–– Output is an*N*-by-1`quaternion`

.`'Rotation matrix'`

–– Output is a 3-by-3-by-*N*rotation matrix.

**Data Types: **`char`

| `string`

## Usage

### Description

`[`

fuses accelerometer, gyroscope, and magnetometer data to compute orientation and angular
velocity measurements. The algorithm assumes that the device is stationary before the
first call.`orientation`

,`angularVelocity`

] = FUSE(`accelReadings`

,`gyroReadings`

,`magReadings`

)

### Input Arguments

`accelReadings`

— Accelerometer readings in sensor body coordinate system
(m/s^{2})

*N*-by-3 matrix

Accelerometer readings in the sensor body coordinate system in
m/s^{2}, specified as an *N*-by-3 matrix.
*N* is the number of samples, and the three columns of
`accelReadings`

represent the [*x*
*y*
*z*] measurements. Accelerometer readings are assumed to correspond
to the sample rate specified by the SampleRate
property.

**Data Types: **`single`

| `double`

`gyroReadings`

— Gyroscope readings in sensor body coordinate system (rad/s)

*N*-by-3 matrix

Gyroscope readings in the sensor body coordinate system in rad/s, specified as an
*N*-by-3 matrix. *N* is the number of samples, and
the three columns of `gyroReadings`

represent the
[*x*
*y*
*z*] measurements. Gyroscope readings are assumed to correspond to
the sample rate specified by the SampleRate
property.

**Data Types: **`single`

| `double`

`magReadings`

— Magnetometer readings in sensor body coordinate system (µT)

*N*-by-3 matrix

Magnetometer readings in the sensor body coordinate system in µT, specified as an
*N*-by-3 matrix. *N* is the number of samples, and
the three columns of `magReadings`

represent the
[*x*
*y*
*z*] measurements. Magnetometer readings are assumed to correspond to
the sample rate specified by the SampleRate
property.

**Data Types: **`single`

| `double`

### Output Arguments

`orientation`

— Orientation that rotates quantities from local navigation coordinate system to
sensor body coordinate system

*M*-by-1 array of quaternions (default) | 3-by-3-by-*M* array

Orientation that can rotate quantities from the local navigation coordinate system
to a body coordinate system, returned as quaternions or an array. The size and type of
`orientation`

depends on whether the OrientationFormat
property is set to `'quaternion'`

or ```
'Rotation
matrix'
```

:

`'quaternion'`

–– the output is an*M*-by-1 vector of quaternions, with the same underlying data type as the inputs`'Rotation matrix'`

–– the output is a 3-by-3-by-*M*array of rotation matrices the same data type as the inputs

The number of input samples, *N*, and the DecimationFactor
property determine *M*.

You can use `orientation`

in a `rotateframe`

function to rotate quantities from a local navigation system to a sensor body
coordinate system.

**Data Types: **`quaternion`

| `single`

| `double`

`angularVelocity`

— Angular velocity in sensor body coordinate system (rad/s)

*M*-by-3 array (default)

Angular velocity with gyroscope bias removed in the sensor body coordinate system
in rad/s, returned as an *M*-by-3 array. The number of input samples,
*N*, and the `DecimationFactor`

property
determine *M*.

**Data Types: **`single`

| `double`

## Object Functions

To use an object function, specify the
System object as the first input argument. For
example, to release system resources of a System object named `obj`

, use
this syntax:

release(obj)

## Examples

### Estimate Orientation Using `ahrsfilter`

Load the `rpy_9axis`

file, which contains recorded accelerometer, gyroscope, and magnetometer sensor data from a device oscillating in pitch (around *y*-axis), then yaw (around *z*-axis), and then roll (around *x*-axis). The file also contains the sample rate of the recording.

load 'rpy_9axis' sensorData Fs accelerometerReadings = sensorData.Acceleration; gyroscopeReadings = sensorData.AngularVelocity; magnetometerReadings = sensorData.MagneticField;

Create an `ahrsfilter`

System object™ with `SampleRate`

set to the sample rate of the sensor data. Specify a decimation factor of two to reduce the computational cost of the algorithm.

decim = 2; fuse = ahrsfilter('SampleRate',Fs,'DecimationFactor',decim);

Pass the accelerometer readings, gyroscope readings, and magnetometer readings to the `ahrsfilter`

object, `fuse`

, to output an estimate of the sensor body orientation over time. By default, the orientation is output as a vector of quaternions.

q = fuse(accelerometerReadings,gyroscopeReadings,magnetometerReadings);

Orientation is defined by angular displacement required to rotate a parent coordinate system to a child coordinate system. Plot the orientation in Euler angles in degrees over time.

`ahrsfilter`

correctly estimates the change in orientation over time, including the south-facing initial orientation.

time = (0:decim:size(accelerometerReadings,1)-1)/Fs; plot(time,eulerd(q,'ZYX','frame')) title('Orientation Estimate') legend('z-axis', 'y-axis', 'x-axis') ylabel('Rotation (degrees)')

### Simulate Magnetic Jamming on `ahrsFilter`

This example shows how performance of the `ahrsfilter`

System object™ is affected by magnetic jamming.

Load `StationaryIMUReadings`

, which contains accelerometer, magnetometer, and gyroscope readings from a stationary IMU.

load 'StationaryIMUReadings.mat' accelReadings magReadings gyroReadings SampleRate numSamples = size(accelReadings,1);

The `ahrsfilter`

uses magnetic field strength to stabilize its orientation against the assumed constant magnetic field of the Earth. However, there are many natural and man-made objects which output magnetic fields and can confuse the algorithm. To account for the presence of transient magnetic fields, you can set the `MagneticDisturbanceNoise`

property on the `ahrsfilter`

object.

Create an `ahrsfilter`

object with the decimation factor set to 2 and note the default expected magnetic field strength.

decim = 2; FUSE = ahrsfilter('SampleRate',SampleRate,'DecimationFactor',decim);

Fuse the IMU readings using the attitude and heading reference system (AHRS) filter, and then visualize the orientation of the sensor body over time. The orientation fluctuates at the beginning and stabilizes after approximately 60 seconds.

orientation = FUSE(accelReadings,gyroReadings,magReadings); orientationEulerAngles = eulerd(orientation,'ZYX','frame'); time = (0:decim:(numSamples-1))'/SampleRate; figure(1) plot(time,orientationEulerAngles(:,1), ... time,orientationEulerAngles(:,2), ... time,orientationEulerAngles(:,3)) xlabel('Time (s)') ylabel('Rotation (degrees)') legend('z-axis','y-axis','x-axis') title('Filtered IMU Data')

Mimic magnetic jamming by adding a transient, strong magnetic field to the magnetic field recorded in the `magReadings`

. Visualize the magnetic field jamming.

jamStrength = [10,5,2]; startStop = (50*SampleRate):(150*SampleRate); jam = zeros(size(magReadings)); jam(startStop,:) = jamStrength.*ones(numel(startStop),3); magReadings = magReadings + jam; figure(2) plot(time,magReadings(1:decim:end,:)) xlabel('Time (s)') ylabel('Magnetic Field Strength (\mu T)') title('Simulated Magnetic Field with Jamming') legend('z-axis','y-axis','x-axis')

Run the simulation again using the `magReadings`

with magnetic jamming. Plot the results and note the decreased performance in orientation estimation.

reset(FUSE) orientation = FUSE(accelReadings,gyroReadings,magReadings); orientationEulerAngles = eulerd(orientation,'ZYX','frame'); figure(3) plot(time,orientationEulerAngles(:,1), ... time,orientationEulerAngles(:,2), ... time,orientationEulerAngles(:,3)) xlabel('Time (s)') ylabel('Rotation (degrees)') legend('z-axis','y-axis','x-axis') title('Filtered IMU Data with Magnetic Disturbance and Default Properties')

The magnetic jamming was misinterpreted by the AHRS filter, and the sensor body orientation was incorrectly estimated. You can compensate for jamming by increasing the `MagneticDisturbanceNoise`

property. Increasing the `MagneticDisturbanceNoise`

property increases the assumed noise range for magnetic disturbance, and the entire magnetometer signal is weighted less in the underlying fusion algorithm of `ahrsfilter`

.

Set the `MagneticDisturbanceNoise`

to `200`

and run the simulation again.

The orientation estimation output from `ahrsfilter`

is more accurate and less affected by the magnetic transient. However, because the magnetometer signal is weighted less in the underlying fusion algorithm, the algorithm may take more time to restabilize.

reset(FUSE) FUSE.MagneticDisturbanceNoise = 20; orientation = FUSE(accelReadings,gyroReadings,magReadings); orientationEulerAngles = eulerd(orientation,'ZYX','frame'); figure(4) plot(time,orientationEulerAngles(:,1), ... time,orientationEulerAngles(:,2), ... time,orientationEulerAngles(:,3)) xlabel('Time (s)') ylabel('Rotation (degrees)') legend('z-axis','y-axis','x-axis') title('Filtered IMU Data with Magnetic Disturbance and Modified Properties')

### Track Shaking 9-Axis IMU

This example uses the `ahrsfilter`

System object™ to fuse 9-axis IMU data from a sensor body that is shaken. Plot the quaternion distance between the object and its final resting position to visualize performance and how quickly the filter converges to the correct resting position. Then tune parameters of the `ahrsfilter`

so that the filter converges more quickly to the ground-truth resting position.

Load `IMUReadingsShaken`

into your current workspace. This data was recorded from an IMU that was shaken then laid in a resting position. Visualize the acceleration, magnetic field, and angular velocity as recorded by the sensors.

load 'IMUReadingsShaken' accelReadings gyroReadings magReadings SampleRate numSamples = size(accelReadings,1); time = (0:(numSamples-1))'/SampleRate; figure(1) subplot(3,1,1) plot(time,accelReadings) title('Accelerometer Reading') ylabel('Acceleration (m/s^2)') subplot(3,1,2) plot(time,magReadings) title('Magnetometer Reading') ylabel('Magnetic Field (\muT)') subplot(3,1,3) plot(time,gyroReadings) title('Gyroscope Reading') ylabel('Angular Velocity (rad/s)') xlabel('Time (s)')

Create an `ahrsfilter`

and then fuse the IMU data to determine orientation. The orientation is returned as a vector of quaternions; convert the quaternions to Euler angles in degrees. Visualize the orientation of the sensor body over time by plotting the Euler angles required, at each time step, to rotate the global coordinate system to the sensor body coordinate system.

fuse = ahrsfilter('SampleRate',SampleRate); orientation = fuse(accelReadings,gyroReadings,magReadings); orientationEulerAngles = eulerd(orientation,'ZYX','frame'); figure(2) plot(time,orientationEulerAngles(:,1), ... time,orientationEulerAngles(:,2), ... time,orientationEulerAngles(:,3)) xlabel('Time (s)') ylabel('Rotation (degrees)') title('Orientation over Time') legend('Rotation around z-axis', ... 'Rotation around y-axis', ... 'Rotation around x-axis')

In the IMU recording, the shaking stops after approximately six seconds. Determine the resting orientation so that you can characterize how fast the `ahrsfilter`

converges.

To determine the resting orientation, calculate the averages of the magnetic field and acceleration for the final four seconds and then use the `ecompass`

function to fuse the data.

Visualize the quaternion distance from the resting position over time.

restingOrientation = ecompass(mean(accelReadings(6*SampleRate:end,:)), ... mean(magReadings(6*SampleRate:end,:))); figure(3) plot(time,rad2deg(dist(restingOrientation,orientation))) hold on xlabel('Time (s)') ylabel('Quaternion Distance (degrees)')

Modify the default `ahrsfilter`

properties so that the filter converges to gravity more quickly. Increase the `GyroscopeDriftNoise`

to `1e-2`

and decrease the `LinearAccelerationNoise`

to `1e-4`

. This instructs the `ahrsfilter`

algorithm to weigh gyroscope data less and accelerometer data more. Because the `accelerometer`

data provides the stabilizing and consistent gravity vector, the resulting orientation converges more quickly.

Reset the filter, fuse the data, and plot the results.

fuse.LinearAccelerationNoise = 1e-4; fuse.GyroscopeDriftNoise = 1e-2; reset(fuse) orientation = fuse(accelReadings,gyroReadings,magReadings); figure(3) plot(time,rad2deg(dist(restingOrientation,orientation))) legend('Default AHRS Filter','Tuned AHRS Filter')

## Algorithms

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

The `ahrsfilter`

uses the nine-axis Kalman filter structure described in
[1]. The algorithm attempts to track the
errors in orientation, gyroscope offset, linear acceleration, and magnetic disturbance to
output the final orientation and angular velocity. Instead of tracking the orientation
directly, the indirect Kalman filter models the error process, *x*, with a
recursive update:

$${x}_{k}=\left[\begin{array}{c}{\theta}_{k}\\ {b}_{k}\\ {a}_{k}\\ {d}_{k}\end{array}\right]={F}_{k}\left[\begin{array}{c}{\theta}_{k-1}\\ {b}_{k-1}\\ {a}_{k-1}\\ {d}_{k-1}\end{array}\right]+{w}_{k}$$

where *x*_{k} is a 12-by-1 vector consisting of:

*θ*–– 3-by-1 orientation error vector, in degrees, at time_{k}*k**b*–– 3-by-1 gyroscope zero angular rate bias vector, in deg/s, at time_{k}*k**a*–– 3-by-1 acceleration error vector measured in the sensor frame, in g, at time_{k}*k**d*–– 3-by-1 magnetic disturbance error vector measured in the sensor frame, in µT, at time_{k}*k*

and where *w*_{k} is a 12-by-1 additive
noise vector, and *F*_{k} is the state transition
model.

Because *x*_{k} is defined as the error process, the
*a priori* estimate is always zero, and therefore the state transition
model, *F*_{k}, is zero. This insight results in the
following reduction of the standard Kalman equations:

Standard Kalman equations:

$$\begin{array}{l}{x}_{k}^{-}={F}_{k}{x}_{k-1}^{+}\\ {P}_{k}^{-}={F}_{k}{P}_{k-1}^{+}{F}_{k}^{T}+{Q}_{k}\\ \\ {y}_{k}={z}_{k}-{H}_{k}{x}_{k}^{-}\\ {S}_{k}\text{\hspace{0.17em}}={R}_{k}+{H}_{k}{P}_{k}^{-}{H}_{k}{}^{T}\\ {K}_{k}={P}_{k}^{-}{H}_{k}^{T}{\left({S}_{k}\right)}^{-1}\\ {x}_{k}^{+}={x}_{k}^{-}+{K}_{k}{y}_{k}\\ {P}_{k}^{+}={P}_{k}{}^{-}-{K}_{k}{H}_{k}{P}_{k}^{-}\end{array}$$

Kalman equations used in this algorithm:

$$\begin{array}{l}{x}_{k}^{-}=0\\ {P}_{k}^{-}={Q}_{k}\\ \\ {y}_{k}={z}_{k}\\ {S}_{k}\text{\hspace{0.17em}}={R}_{k}+{H}_{k}{P}_{k}^{-}{H}_{k}{}^{T}\\ {K}_{k}={P}_{k}^{-}{H}_{k}^{T}{\left({S}_{k}\right)}^{-1}\\ {x}_{k}^{+}={K}_{k}{y}_{k}\\ {P}_{k}^{+}={P}_{k}{}^{-}-{K}_{k}{H}_{k}{P}_{k}^{-}\end{array}$$

where:

*x*_{k}^{−}–– predicted (*a priori*) state estimate; the error process*P*_{k}^{−}–– predicted (*a priori*) estimate covariance*y*–– innovation_{k}*S*–– innovation covariance_{k}*K*–– Kalman gain_{k}*x*_{k}^{+}–– updated (*a posteriori*) state estimate*P*_{k}^{+}–– updated (*a posteriori*) estimate covariance

*k* represents the iteration, the superscript
^{+} represents an *a posteriori* estimate, and
the superscript ^{−} represents an *a priori*
estimate.

The graphic and following steps describe a single frame-based iteration through the algorithm.

Before the first iteration, the `accelReadings`

,
`gyroReadings`

, and `magReadings`

inputs are chunked
into `DecimationFactor`

-by-3 frames. For each chunk, the algorithm uses the
most current accelerometer and magnetometer readings corresponding to the chunk of gyroscope
readings.

### Detailed Overview

Walk through the algorithm for an explanation of each stage of the detailed overview.

### Model

The algorithm models acceleration and angular change as linear processes.

**Predict Orientation**

The orientation for the current frame is predicted by first estimating the angular change from the previous frame:

$$\Delta {\phi}_{N\times 3}=\frac{\left(gyroReading{s}_{N\times 3}-gyroOffse{t}_{1\times 3}\right)}{fs}$$

where *N* is the decimation factor specified by the DecimationFactor
property and *fs* is the sample rate specified by the SampleRate
property.

The angular change is converted into quaternions using the `rotvec`

`quaternion`

construction syntax:

$$\Delta {Q}_{N\times 1}=\mathrm{quaternion}(\Delta {\phi}_{N\times 3},\text{'}rotvec\text{'})$$

The previous orientation estimate is updated by rotating it by Δ*Q*:

$${q}_{1\times 1}^{-}=\left({q}_{1\times 1}^{+}\right)\left({\displaystyle \prod _{n=1}^{N}\Delta {Q}_{n}}\right)$$

During the first iteration, the orientation estimate,
*q*^{−}, is initialized by `ecompass`

.

**Estimate Gravity from Orientation**

The gravity vector is interpreted as the third column of the quaternion,
*q*^{−}, in rotation matrix form:

$${g}_{1\times 3}={\left(rPrior(:,3)\right)}^{T}$$

See [1] for an explanation of
why the third column of *rPrior* can be interpreted as the gravity
vector.

**Estimate Gravity from Acceleration**

A second gravity vector estimation is made by subtracting the decayed linear acceleration estimate of the previous iteration from the accelerometer readings:

$$gAcce{l}_{1\times 3}=accelReading{s}_{1\times 3}-linAccelprio{r}_{1\times 3}$$

**Estimate Earth's Magnetic Vector**

Earth's magnetic vector is estimated by rotating the magnetic vector estimate from the
previous iteration by the *a priori* orientation estimate, in rotation
matrix form:

$$mGyr{o}_{1\times 3}={\left(\left(rPrior\right)\left({m}^{T}\right)\right)}^{T}$$

### Error Model

The error model combines two differences:

The difference between the gravity estimate from the accelerometer readings and the gravity estimate from the gyroscope readings: $${z}_{g}=g-gAccel$$

The difference between the magnetic vector estimate from the gyroscope readings and the magnetic vector estimate from the magnetometer:$${z}_{m}=mGyro-magReadings$$

### Magnetometer Correct

The magnetometer correct estimates the error in the magnetic vector estimate and detects magnetic jamming.

**Magnetometer Disturbance Error**

The magnetic disturbance error is calculated by matrix multiplication of the Kalman gain associated with the magnetic vector with the error signal:

$$mErro{r}_{3\times 1}={\left(\left(K{(\text{10:12},:)}_{3\times 6}\right){\left({z}_{1\times 6}\right)}^{T}\right)}^{T}$$

The Kalman gain, *K*, is the Kalman gain calculated in the current
iteration.

**Magnetic Jamming Detection**

Magnetic jamming is determined by verifying that the power of the detected magnetic disturbance is less than or equal to four times the power of the expected magnetic field strength:

$$tf=\{\begin{array}{c}\text{true}\\ \text{false}\end{array}\begin{array}{c}\text{\hspace{0.17em}}\text{\hspace{0.17em}}\text{if}\\ \text{\hspace{0.17em}}\text{\hspace{0.17em}}\text{else}\end{array}\begin{array}{c}\text{\hspace{0.17em}}\text{\hspace{0.17em}}{{\displaystyle \sum \left|mError\right|}}^{2}>\left(4\right){\left(\text{ExpectedMagneticFieldStrength}\right)}^{2}\\ \text{\hspace{0.17em}}\end{array}$$

ExpectedMagneticFieldStrength
is a property of `ahrsfilter`

.

### Kalman Equations

The Kalman equations use the gravity estimate derived from the gyroscope readings,
*g*, the magnetic vector estimate derived from the gyroscope readings,
*mGyro*, and the observation of the error process, *z*,
to update the Kalman gain and intermediary covariance matrices. The Kalman gain is applied
to the error signal, *z*, to output an *a posteriori*
error estimate, *x*^{+}.

**Observation Model**

The observation model maps the 1-by-3 observed states, *g* and
*mGyro*, into the 6-by-12 true state, *H*.

The observation model is constructed as:

$${H}_{3\times 9}=\left[\begin{array}{cccccccccccc}0& {g}_{z}& -{g}_{y}& 0& -\kappa {g}_{z}& \kappa {g}_{y}& 1& 0& 0& 0& 0& 0\\ -{g}_{z}& 0& {g}_{x}& \kappa {g}_{z}& 0& -\kappa {g}_{x}& 0& 1& 0& 0& 0& 0\\ {g}_{y}& -{g}_{x}& 0& -\kappa {g}_{y}& \kappa {g}_{x}& 0& 0& 0& 1& 0& 0& 0\\ 0& {m}_{z}& -{m}_{y}& 0& -\kappa {m}_{z}& -\kappa {m}_{y}& 0& 0& 0& -1& 0& 0\\ -{m}_{z}& 0& {m}_{x}& \kappa {m}_{z}& 0& -\kappa {m}_{x}& 0& 0& 0& 0& -1& 0\\ {m}_{y}& -{m}_{x}& 0& -\kappa {m}_{y}& \kappa {m}_{x}& 0& 0& 0& 0& 0& 0& -1\end{array}\right]$$

where *g*_{x},
*g*_{y}, and
*g*_{z} are the *x*-,
*y*-, and *z*-elements of the gravity vector estimated
from the *a priori* orientation, respectively.
*m*_{x},
*m*_{y}, and
*m*_{z} are the *x*-,
*y*-, and *z*-elements of the magnetic vector
estimated from the *a priori* orientation, respectively.
*κ* is a constant determined by the SampleRate
and DecimationFactor
properties: *κ* =
`DecimationFactor`

/`SampleRate`

.

See sections 7.3 and 7.4 of [1] for a derivation of the observation model.

**Innovation Covariance**

The innovation covariance is a 6-by-6 matrix used to track the variability in the measurements. The innovation covariance matrix is calculated as:

$${S}_{6x6}={R}_{6x6}+\left({H}_{6x12}\right)\left({P}_{{}_{12x12}}^{-}\right){\left({H}_{6x12}\right)}^{T}$$

where

*H*is the observation model matrix*P*^{−}is the predicted (*a priori*) estimate of the covariance of the observation model calculated in the previous iteration*R*is the covariance of the observation model noise, calculated as:$${R}_{6\times 6}=\left[\begin{array}{cccccc}acce{l}_{\text{noise}}& 0& 0& 0& 0& 0\\ 0& acce{l}_{\text{noise}}& 0& 0& 0& 0\\ 0& 0& acce{l}_{\text{noise}}& 0& 0& 0\\ 0& 0& 0& ma{g}_{\text{noise}}& 0& 0\\ 0& 0& 0& 0& ma{g}_{\text{noise}}& 0\\ 0& 0& 0& 0& 0& ma{g}_{\text{noise}}\end{array}\right]$$

where

$$acce{l}_{\text{noise}}=\text{AccelerometerNoise}\text{\hspace{0.17em}}\text{+}\text{\hspace{0.17em}}\text{LinearAccelerationNoise}\text{\hspace{0.17em}}\text{+}\text{\hspace{0.17em}}{\kappa}^{2}\left(\text{GyroscopeDriftNoise}\text{\hspace{0.17em}}\text{+}\text{\hspace{0.17em}}\text{GyroscopeNoise}\right)$$

and

$$ma{g}_{\text{noise}}=\text{MagnetometerNoise}\text{\hspace{0.17em}}\text{+}\text{\hspace{0.17em}}\text{MagneticDisturbanceNoise}\text{\hspace{0.17em}}\text{+}\text{\hspace{0.17em}}{\kappa}^{2}\left(\text{GyroscopeDriftNoise}\text{\hspace{0.17em}}\text{+}\text{\hspace{0.17em}}\text{GyroscopeNoise}\right)$$

The following properties define the observation model noise variance:

**Update Error Estimate Covariance**

The error estimate covariance is a 12-by-12 matrix used to track the variability in the state.

The error estimate covariance matrix is updated as:

$${P}_{{}_{12\times 12}}^{+}={P}_{{}_{12\times 12}}^{-}-\left({K}_{12\times 6}\right)\left({H}_{6\times 12}\right)\left({P}_{{}_{12\times 12}}^{-}\right)$$

where *K* is the Kalman gain, *H* is
the measurement matrix, and *P*^{−} is the error
estimate covariance calculated during the previous iteration.

**Predict Error Estimate Covariance**

The error estimate covariance is a 12-by-12 matrix used to track the variability in
the state. The *a priori* error estimate covariance,
*P ^{−}*, is set to the process noise
covariance,

*Q*, determined during the previous iteration.

*Q*is calculated as a function of the

*a posteriori*error estimate covariance,

*P*. When calculating

^{+}*Q*, it is assumed that the cross-correlation terms are negligible compared to the autocorrelation terms, and are set to zero:

$$Q=\left[\begin{array}{cccccccccccc}{P}^{+}(1)+{\kappa}^{2}\left({P}^{+}(40)+\beta +\eta \right)& 0& 0& -\kappa \left({P}^{+}(40)+\beta \right)& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& {P}^{+}(14)+{\kappa}^{2}\left({P}^{+}(53)+\beta +\eta \right)& 0& 0& -\kappa \left({P}^{+}(53)+\beta \right)& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& {P}^{+}(27)+{\kappa}^{2}\left({P}^{+}(66)+\beta +\eta \right)& 0& 0& -\kappa \left({P}^{+}(66)+\beta \right)& 0& 0& 0& 0& 0& 0\\ -\kappa \left({P}^{+}(40)+\beta \right)& 0& 0& {P}^{+}(40)+\beta & 0& 0& 0& 0& 0& 0& 0& 0\\ 0& -\kappa \left({P}^{+}(53)+\beta \right)& 0& 0& {P}^{+}(53)+\beta & 0& 0& 0& 0& 0& 0& 0\\ 0& 0& -\kappa \left({P}^{+}(66)+\beta \right)& 0& 0& {P}^{+}(66)+\beta & 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& {\nu}^{2}{P}^{+}(79)+\xi & 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& {\nu}^{2}{P}^{+}(92)+\xi & 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& {\nu}^{2}{P}^{+}(105)+\xi & 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& {\sigma}^{2}{P}^{+}(118)+\gamma & 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& {\sigma}^{2}{P}^{+}(131)+\gamma & 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& {\sigma}^{2}{P}^{+}(144)+\gamma \end{array}\right]$$

where

*P*^{+}–– is the updated (*a posteriori*) error estimate covariance*β*–– GyroscopeDriftNoise*η*–– GyroscopeNoise

See section 10.1 of [1] for a derivation of the terms of the process error matrix.

**Kalman Gain**

The Kalman gain matrix is a 12-by-6 matrix used to weight the innovation. In this
algorithm, the innovation is interpreted as the error process,
*z*.

The Kalman gain matrix is constructed as:

$${K}_{12\times 6}=\left({P}_{{}_{12\times 12}}^{-}\right){\left({H}_{6\times 12}\right)}^{T}{\left({\left({S}_{6\times 6}\right)}^{T}\right)}^{-1}$$

where

*P*^{−}–– predicted error covariance*H*–– observation model*S*–– innovation covariance

**Update a Posteriori Error**

The *a posterior* error estimate is determined by combining the
Kalman gain matrix with the error in the gravity vector and magnetic vector estimations:

$${x}_{12\times 1}=\left({K}_{12\times 6}\right){({z}_{1\times 6})}^{T}$$

If magnetic jamming is detected in the current iteration, the magnetic
vector error signal is ignored, and the *a posterior* error estimate is
calculated as:

$${x}_{9\times 1}=\left(K(\text{1:9},\text{1:3}\right){({z}_{g})}^{T}$$

### Correct

**Estimate Orientation**

The orientation estimate is updated by multiplying the previous estimation by the error:

$${q}^{+}=\left({q}^{-}\right)\left({\theta}^{+}\right)$$

**Estimate Linear Acceleration**

The linear acceleration estimation is updated by decaying the linear acceleration estimation from the previous iteration and subtracting the error:

$$linAccelPrior=(linAccelPrio{r}_{k-1})\nu -{b}^{+}$$

where

**Estimate Gyroscope Offset**

The gyroscope offset estimation is updated by subtracting the gyroscope offset error from the gyroscope offset from the previous iteration:

$$gyroOffset=gyroOffse{t}_{k-1}-{a}^{+}$$

### Compute Angular Velocity

To estimate angular velocity, the frame of `gyroReadings`

are
averaged and the gyroscope offset computed in the previous iteration is subtracted:

$$angularVelocit{y}_{1\times 3}=\frac{{\displaystyle \sum gyroReading{s}_{N\times 3}}}{N}-gyroOffse{t}_{1\times 3}$$

where *N* is the decimation factor specified by the
`DecimationFactor`

property.

The gyroscope offset estimation is initialized to zeros for the first iteration.

### Update Magnetic Vector

If magnetic jamming was not detected in the current iteration, the magnetic vector
estimate, *m*, is updated using the *a posteriori*
magnetic disturbance error and the *a posteriori* orientation.

The magnetic disturbance error is converted to the navigation frame:

$$mErrorNE{D}_{1\times 3}={\left({\left(rPos{t}_{3\times 3}\right)}^{T}{(mErro{r}_{1\times 3})}^{T}\right)}^{T}$$

The magnetic disturbance error in the navigation frame is subtracted from the previous magnetic vector estimate and then interpreted as inclination:

$${\rm M}=m-mErrorNED$$

$$inclination=\text{atan2}({\rm M}(3),{\rm M}(1))$$

The inclination is converted to a constrained magnetic vector estimate for the next iteration:

$$\begin{array}{l}m(1)=\left(\text{ExpectedMagneticFieldStrength}\right)\left(\mathrm{cos}(inclination)\right)\\ m(2)=0\\ m(3)=\left(\text{ExpectedMagneticFieldStrength}\right)\left(\mathrm{sin}(inclination)\right)\end{array}$$

ExpectedMagneticFieldStrength
is a property of `ahrsfilter`

.

## References

[1] Open Source Sensor Fusion. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

[2] Roetenberg, D., H.J. Luinge, C.T.M. Baten,
and P.H. Veltink. "Compensation of Magnetic Disturbances Improves Inertial and Magnetic
Sensing of Human Body Segment Orientation." *IEEE Transactions on Neural Systems and
Rehabilitation Engineering*. Vol. 13. Issue 3, 2005, pp. 395-405.

## Extended Capabilities

### C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

See System Objects in MATLAB Code Generation (MATLAB Coder).

## Version History

**Introduced in R2018b**

## Open Example

You have a modified version of this example. Do you want to open this example with your edits?

## MATLAB Command

You clicked a link that corresponds to this MATLAB command:

Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.

# Select a Web Site

Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .

You can also select a web site from the following list:

## How to Get Best Site Performance

Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.

### Americas

- América Latina (Español)
- Canada (English)
- United States (English)

### Europe

- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)

- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)