AHRS
Orientation based on accelerometer, gyroscope, and magnetometer readings.
blockType: AHRS
|
Path in the library: |
Description
Block AHRS Combines accelerometer, magnetometer, and gyroscope data to estimate device orientation.
Ports
Input
#
Accel
—
accelerometer readings in the coordinate system of the sensor housing, m/s2
the matrix of real scalars N by 3
Details
Accelerometer readings in the coordinate system of the sensor housing in m/s2, set as a matrix on real scalar quantities, where — the number of counts, and the three columns of the Accel matrix represent measurements respectively.
| Data types |
|
| Complex numbers support |
No |
#
Gyro
—
gyroscope readings in the coordinate system of the sensor housing, rad/s
the matrix of real scalars N by 3
Details
Gyroscope readings in the coordinate system of the sensor housing in rad/s, set as a matrix on real scalar quantities, where — the number of counts, and the three columns of the Gyro matrix represent measurements respectively.
| Data types |
|
| Complex numbers support |
No |
#
Mag
—
magnetometer readings in the coordinate system of the sensor housing, MCT
the matrix of real scalars N by 3
Details
Magnetometer readings in the coordinate system of the sensor housing in mkTl, set as a matrix on real scalar quantities, where — the number of counts, and the three columns of the Gyro matrix represent measurements respectively.
| Data types |
|
| Complex numbers support |
No |
Output
#
Orientation
—
orientation of the sensor housing coordinate system relative to the navigation coordinate system
the matrix of scalars M by 4 | an array of 3-by-3-by-M rotation matrices
Details
The orientation of the sensor housing coordinate system relative to the navigation coordinate system, returned as a matrix of scalars on or as an array of rotation matrices on on . Each row of the matrix on It contains four quaternion elements. Number of input samples and the parameter value Decimation factor determine the size of the output data .
| Data types |
|
| Complex numbers support |
No |
#
Angular Velocity
—
angular velocity in the coordinate system of the sensor housing, rad/s
the matrix of real scalars M by 3
Details
Angular velocity, taking into account the displacement of the gyroscope in the coordinate system of the sensor housing in rad/s, returned as a matrix on real scalar quantities. Number of input samples and the parameter value Decimation factor determine the size of the output data .
| Data types |
|
| Complex numbers support |
No |
Parameters
Main
#
Reference frame —
navigation coordinate system
NED | ENU
Details
The navigation coordinate system, set as NED (north-east-down) or ENU (east-north-up).
| Values |
|
| Default value |
|
| Program usage name |
|
| Tunable |
No |
| Evaluatable |
No |
#
Decimation factor —
decimation coefficient
Real number
Details
The decimation coefficient by which the data transfer rate from the input sensor is reduced, set as a positive integer.
The number of rows in the input data matrices Accel, Gyro and Mag must be a multiple of the decimation coefficient.
| Default value |
|
| Program usage name |
|
| Tunable |
No |
| Evaluatable |
Yes |
#
Initial process noise —
the initial noise of the process
Matrix of real numbers
Details
The initial noise level of the process, defined as a matrix of real scalar quantities with the size on .
| Default value |
|
| Program usage name |
|
| Tunable |
No |
| Evaluatable |
Yes |
#
Orientation format —
output orientation format
quaternion | Rotation matrix
Details
The output data orientation format, set as quaternion or Rotation matrix:
-
quaternion— the output matrix of real scalars on . Each row of the matrix contains four quaternion elements. -
Rotation matrix— output array of rotation matrices on on
Number of input samples and the parameter value Decimation factor determine the size of the output data .
| Values |
|
| Default value |
|
| Program usage name |
|
| Tunable |
No |
| Evaluatable |
No |
Main
#
Accelerometer noise ((m/s²)²) —
noise dispersion of the accelerometer signal, (m/s2)2
Real number
Details
The noise dispersion of the accelerometer signal in (m/s2)2, given as a positive real scalar.
| Default value |
|
| Program usage name |
|
| Tunable |
Yes |
| Evaluatable |
Yes |
#
Gyroscope noise ((rad/s)²) —
noise dispersion of the gyroscope signal, (rad/s)2
Real number
Details
The noise dispersion of the gyroscope signal in (rad/s) 2, given as a positive real scalar.
| Default value |
|
| Program usage name |
|
| Tunable |
Yes |
| Evaluatable |
Yes |
#
Magnetometer noise ((μT)²) —
noise dispersion of the magnetometer signal, MCT2
Real number
Details
The noise dispersion of the magnetometer signal in MCT2, given as a positive real scalar.
| Default value |
|
| Program usage name |
|
| Tunable |
Yes |
| Evaluatable |
Yes |
#
Gyroscope drift noise ((rad/s)²) —
gyroscope displacement drift variance, (rad/s)2
Real number
Details
The variance of the gyroscope displacement drift in (rad/s)2, given as a positive real scalar.
| Default value |
|
| Program usage name |
|
| Tunable |
Yes |
| Evaluatable |
Yes |
Main
#
Linear acceleration noise ((m/s²)²) —
noise dispersion of linear acceleration, (m/s2)2
Real number
Details
The noise variance of linear acceleration in (m/s2)2, given as a positive real scalar. Linear acceleration is modeled as a white noise process filtered out by a low-pass filter.
| Default value |
|
| Program usage name |
|
| Tunable |
Yes |
| Evaluatable |
Yes |
#
Magnetic disturbance noise ((μT)²) —
dispersion of magnetic interference noise, mcTl2
Real number
Details
The dispersion of magnetic interference noise in mcTl 2, given as a finite positive real scalar.
| Default value |
|
| Program usage name |
|
| Tunable |
Yes |
| Evaluatable |
Yes |
#
Linear acceleration decay factor —
attenuation coefficient for linear acceleration drift
Real number
Details
Attenuation coefficient for linear acceleration drift, given as a scalar in the range [0, 1). If the linear acceleration changes rapidly, set this parameter to a lower value. If the linear acceleration changes slowly, set this parameter to a higher value. Linear acceleration drift is modeled as a white noise process filtered out by a low-pass filter.
| Default value |
|
| Program usage name |
|
| Tunable |
Yes |
| Evaluatable |
Yes |
#
Magnetic disturbance decay factor —
magnetic disturbance attenuation coefficient
Real number
Details
The attenuation coefficient of the magnetic disturbance, set as a scalar in the range [0, 1]. The magnetic disturbance is modeled as a first-order Markov process.
| Default value |
|
| Program usage name |
|
| Tunable |
Yes |
| Evaluatable |
Yes |
#
Magnetic field strength (μT) —
magnetic field strength, MCT
Real number
Details
The magnetic field strength in MCT, given as a real positive scalar. Magnetic field strength is an estimate of the strength of the Earth’s magnetic field at the current location.
| Default value |
|
| Program usage name |
|
| Tunable |
Yes |
| Evaluatable |
Yes |
Algorithms
The algorithm given in this section is applicable only to the coordinate system. NED.
|
Block AHRS uses the structure of the nine-axis Kalman filter described in [1]. The algorithm tries to track orientation errors, gyroscope offsets, linear acceleration, and magnetic disturbances to obtain the final orientation and angular velocity. Instead of direct orientation tracking, the indirect Kalman filter simulates the error process. using recursive updates:
where
-
— vector on , containing:
-
— orientation vector on , in degrees, at a time ;
-
— the displacement vector of the zero angular velocity of the gyroscope on , in deg/s, at a time ;
-
— acceleration error vector on , measured in the sensor coordinate system, in m/s2, at a time ;
-
— the error vector of the magnetic disturbance on , measured in the sensor coordinate system, in MCT, at a time ;
-
-
— additive noise vector on ;
-
— a state transition model.
Because It is defined as an error process, the a priori estimate is always zero, and hence the state transition model it is equal to zero. This leads to the following simplification of the standard Kalman equations:
The Kalman equations used in this algorithm:
where
-
— predictable (a priori) assessment of the condition; error process;
-
— predicted (a priori) estimate of covariance;
-
— discrepancy;
-
— covariance of residuals;
-
— Kalman gain factor;
-
— updated (a posteriori) assessment of the condition;
-
— updated (a posteriori) estimation of covariance;
-
lower index — iteration number;
-
the upper index — a posteriori assessment;
-
the upper index — a priori assessment.
The graphic image and the following steps describe one iteration of the algorithm based on individual frames.
Before the first iteration, the input data , and they are divided into frames with a decimation coefficient 3. For each frame, the algorithm uses the most up-to-date accelerometer and magnetometer readings corresponding to the frame of gyroscope readings.
Detailed description
Details
For a detailed description of each stage of the review, check out the algorithm.
Model
Details
The algorithm models acceleration and angle change as linear processes.
predicting orientation
The orientation for the current frame is determined by first estimating the angular change from the previous frame:
where
-
— the decimation coefficient set by the parameter Decimation factor;
-
— sampling rate.
The angular change is converted to quaternions using the quaternion construction syntax.:
The previous orientation score is updated by rotating to :
During the first iteration, orientation assessment initialized with ecompass.
_ Estimation of gravity by orientation_
The gravity vector is interpreted as the third column of the quaternion in the form of a rotation matrix:
_ Estimation of gravity by acceleration_
The second estimate of the gravity vector is made by subtracting the estimate of the decaying linear acceleration from the previous iteration from the accelerometer readings.:
_ Estimation of the Earth’s magnetic vector_
The Earth’s magnetic vector is estimated by rotating the magnetic vector estimate from the previous iteration to an a priori orientation estimate in the form of a rotation matrix:
The error model
Details
The error model combines two differences:
-
the difference between the gravity estimate based on accelerometer readings and the gravity estimate based on gyroscope readings: ;
-
the difference between the estimate of the magnetic vector according to the gyroscope and the estimate of the magnetic vector according to the magnetometer: .
Magnetometer correction
Details
The magnetometer correction determines the error in the estimation of the magnetic vector and detects magnetic interference.
_ Error of magnetic disturbance_
The magnetic disturbance error is calculated by matrix multiplication of the Kalman gain associated with the magnetic vector by the error signal:
Kalman Gain factor — this is the Kalman gain factor calculated in the current iteration.
The detection of magnetic interference
Magnetic interference is determined by checking that the power of the detected magnetic disturbance is less than or equal to four times the power of the expected magnetic field strength.:
The Kalman equations
Details
The Kalman equations use an estimate of gravity , obtained from gyroscope readings, magnetic vector estimate obtained from the gyroscope readings, and measurement of the error process to update the Kalman gain and intermediate covariance matrices. The Kalman gain is applied to the error signal to obtain a posteriori error estimate .
_ Measurement model_
The measurement model displays the observed states with the size on , and , to the true state size on .
The measurement model is constructed as follows:
where
-
, and — -, - and -elements of the gravity vector, estimated on the basis of a priori orientation, respectively;
-
, and — -, - and -elements of the magnetic field vector, estimated based on a priori orientation, respectively;
-
is a constant equal to the ratio of the decimation coefficient to the sampling frequency.
covariation of residuals
The covariance of residuals is a matrix of size on , used to track the variability of measurements. The residual covariance matrix is calculated as follows:
where
-
— the matrix of the measurement model;
-
— the predicted (a priori) estimate of the measurement model covariance calculated in the previous iteration;
-
— the noise covariance of the measurement model, calculated as follows:
where
_ Updating the error estimate covariance_
The covariance of the error estimate is a matrix on , used to track the variability of the state.
The error estimation covariance matrix is updated as follows:
where
-
— Kalman gain factor;
-
— measurement matrix;
-
— the covariance of the error estimate calculated in the previous iteration.
_ Predicted covariance of the error estimate_
The covariance of the error estimate is a matrix on , used to track the variability of the state. A priori covariance of the error estimate It is established that the covariance of the process noise is equal , defined in the previous iteration. calculated as a function of the a posteriori covariance of the error estimate . When calculating It is assumed that the cross-correlation terms are negligible compared to the autocorrelation terms and are set to zero.:
where
-
— updated (a posteriori) error estimation covariance;
-
is the decimation coefficient divided by the sampling frequency;
-
— gyroscope drift noise;
-
— gyroscope noise;
-
— attenuation coefficient of linear acceleration;
-
— noise of linear acceleration;
-
— the attenuation coefficient of magnetic interference;
-
— the noise of magnetic interference.
The Kalman gain coefficient
The Kalman gain matrix is a matrix of the size on , used for weighing the discrepancy. In this algorithm, the discrepancy is interpreted as an error process. .
The Kalman gain matrix is constructed as follows:
where
-
— predicted error covariance;
-
— measurement model;
-
— covariance of residuals.
_ Updating a posteriori error_
The a posteriori error estimate is determined by combining the Kalman gain matrix with the error in the estimates of the gravity vector and the magnetic field vector:
If magnetic interference is detected in the current iteration, the magnetic vector error signal is ignored, and the a posteriori error estimate is calculated as follows:
Amendment
Details
_ Assessment of orientation_
The orientation score is updated by multiplying the previous score by the error:
_ Estimation of linear acceleration_
The linear acceleration estimate is updated by reducing the linear acceleration estimate from the previous iteration and subtracting the error:
where — attenuation coefficient of linear acceleration.
_ Estimation of gyroscope displacement_
The gyro offset estimate is updated by subtracting the gyro offset error from the offset value obtained in the previous iteration.:
Calculation of angular velocity
Details
The data is averaged to estimate the angular velocity. , and then the gyroscope offset calculated in the previous iteration is subtracted.:
where — the decimation coefficient.
The gyroscope offset estimate is initialized to zeros for the first iteration.
Updating the magnetic vector
Details
If no magnetic interference has been detected in the current iteration, estimate the magnetic vector It is updated using the a posteriori error of the magnetic disturbance and the a posteriori orientation.
The error of the magnetic disturbance is converted into a navigation coordinate system:
The error of the magnetic disturbance in the navigation coordinate system is subtracted from the previous estimate of the magnetic vector, and then interpreted as an inclination.:
The inclination is converted into a limited estimate of the magnetic field vector for the next iteration.:
Literature
-
Open Source Sensor Fusion. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs
-
D. Roetenberg, H.J. Luinge, C.T.M. Baten, 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.