Main Content

AHRS

Orientation from accelerometer, gyroscope, and magnetometer readings

Since R2020a

  • AHRS block

Libraries:
Navigation Toolbox / Multisensor Positioning / Navigation Filters
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:

xk=[θkbkakdk]=Fk[θk1bk1ak1dk1]+wk

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:

xk=Fkxk1+Pk=FkPk1+FkT+Qkyk=zkHkxkSk=Rk+HkPkHkTKk=PkHkT(Sk)1xk+=xk+KkykPk+=PkKkHkPk

Kalman equations used in this algorithm:

xk=0Pk=Qkyk=zkSk=Rk+HkPkHkTKk=PkHkT(Sk)1xk+=KkykPk+=PkKkHkPk

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.

Algorithm Flowchart

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.

Extended Capabilities

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

Version History

Introduced in R2020a