# AHRS

Orientation from accelerometer, gyroscope, and magnetometer readings

• Library:

Sensor Fusion and Tracking Toolbox / Multisensor Positioning / Navigation Filters

## Description

The `AHRS` Simulink® block fuses accelerometer, magnetometer, and gyroscope sensor data to estimate device orientation.

## Ports

### Input

expand all

Accelerometer readings in the sensor body coordinate system in m/s2, specified as an N-by-3 matrix of real scalars. N is the number of samples, and the three columns of `Accel` represent the [x y z] measurements, respectively.

Data Types: `single` | `double`

Gyroscope readings in the sensor body coordinate system in rad/s, specified as an N-by-3 matrix of real scalars. N is the number of samples, and the three columns of `Gyro` represent the [x y z] measurements, respectively.

Data Types: `single` | `double`

Magnetometer readings in the sensor body coordinate system in µT, specified as an N-by-3 matrix of real scalars. N is the number of samples, and the three columns of `magReadings` represent the [x y z] measurements, respectively.

Data Types: `single` | `double`

### Output

expand all

Orientation of the sensor body frame relative to the navigation frame, return as an M-by-4 array of scalars or a 3-by-3-by-M array of rotation matrices. Each row the of the N-by-4 array is assumed to be the four elements of a `quaternion`. The number of input samples, N, and the Decimation Factor parameter determine the output size M.

Data Types: `single` | `double`

Angular velocity with gyroscope bias removed in the sensor body coordinate system in rad/s, returned as an M-by-3 array of real scalars. The number of input samples, N, and the Decimation Factor parameter determine the output size M.

Data Types: `single` | `double`

## Parameters

expand all

Main

Navigation reference frame, specified as `NED` (North-East-Down) or `ENU` (East-North-Up).

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

The number of rows of the inputs –– Accel, Gyro , and Mag –– must be a multiple of the decimation factor.

Data Types: `single` | `double`

Initial process noise, specified as a 12-by-12 matrix of real scalars. The default value, `ahrsfilter.defaultProcessNoise`, is a 12-by-12 diagonal matrix as:

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

Data Types: `single` | `double`

Output orientation format, specified as `'quaternion'` or `'Rotation matrix'`:

• `'quaternion'` –– Output is an M-by-4 array of real scalars. Each row of the array represents the four components of a `quaternion`.

• `'Rotation matrix'` –– Output is a 3-by-3-by-M rotation matrix.

The output size M depends on the input dimension N and the Decimation Factor parameter.

Data Types: `char` | `string`

• `Interpreted execution` — Simulate the model using the MATLAB® interpreter. This option shortens startup time. In `Interpreted execution` mode, you can debug the source code of the block.

• `Code generation` — Simulate the model using generated C code. The first time you run a simulation, Simulink generates C code for the block. The C code is reused for subsequent simulations as long as the model does not change. This option requires additional startup time.

Measurement Noise

Variance of accelerometer signal noise in (m/s2)2, specified as a positive real scalar.

Data Types: `single` | `double`

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

Data Types: `single` | `double`

Variance of magnetometer signal noise in μT2, specified as a positive real scalar.

Data Types: `single` | `double`

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

Data Types: `single` | `double`

Environmental Noise

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

Data Types: `single` | `double`

Variance of magnetic disturbance noise in μT2, specified as a real finite positive scalar.

Data Types: `single` | `double`

Decay factor for linear acceleration drift, specified as a scalar in the range [0,1). If linear acceleration changes quickly, set this parameter to a lower value. If linear acceleration changes slowly, set this parameter to a higher value. Linear acceleration drift is modeled as a lowpass-filtered white noise process.

Data Types: `single` | `double`

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.

Data Types: `single` | `double`

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

Data Types: `single` | `double`

## Algorithms

expand all

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

The AHRS block 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 xk is a 12-by-1 vector consisting of:

• θk –– 3-by-1 orientation error vector, in degrees, at time k

• bk –– 3-by-1 gyroscope zero angular rate bias vector, in deg/s, at time k

• ak –– 3-by-1 acceleration error vector measured in the sensor frame, in g, at time k

• dk –– 3-by-1 magnetic disturbance error vector measured in the sensor frame, in µT, at time k

and where wk is a 12-by-1 additive noise vector, and Fk is the state transition model.

Because xk is defined as the error process, the a priori estimate is always zero, and therefore the state transition model, Fk, 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:

• xk –– predicted (a priori) state estimate; the error process

• Pk –– predicted (a priori) estimate covariance

• yk –– innovation

• Sk –– innovation covariance

• Kk –– Kalman gain

• xk+ –– updated (a posteriori) state estimate

• Pk+ –– 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.

## References

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

## Version History

Introduced in R2020a