AHRS
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.
Examples
IMU Sensor Fusion with Simulink
Generate and fuse IMU sensor data using Simulink®. You can accurately model the behavior of an accelerometer, a gyroscope, and a magnetometer and fuse their outputs to compute orientation.
Ports
Input
Accel — Accelerometer readings in sensor body coordinate system
(m/s2)
N-by-3 matrix of real scalar
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
Gyro — Gyroscope readings in sensor body coordinate system (rad/s)
N-by-3 matrix of real scalar
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
Mag — Magnetometer readings in sensor body coordinate system (µT)
N-by-3 matrix of real scalar
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
Orientation — Orientation of sensor body frame relative to navigation frame
M-by-4 array of scalar | 3-by-3-by-M-element rotation matrix
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 — Angular velocity in sensor body coordinate system (rad/s)
M-by-3 array of real scalar (default)
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
Reference frame — Navigation reference frame
NED
(default) | ENU
Navigation reference frame, specified as NED
(North-East-Down) or
ENU
(East-North-Up).
Decimation factor — Decimation factor
1
(default) | positive integer
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 — Initial process noise
ahrsfilter.defaultProcessNoise
(default) | 12-by-12 matrix of real scalar
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
Orientation format — Orientation output format
'quaternion'
(default) | 'Rotation matrix'
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 aquaternion
.'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
Simulate using — Type of simulation to run
Interpreted Execution
(default) | Code Generation
Interpreted execution
— Simulate the model using the MATLAB® interpreter. This option shortens startup time. InInterpreted 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.
Accelerometer noise ((m/s2)2) — Variance of accelerometer signal noise
((m/s2)2)
0.0001924722
(default) | positive real scalar
Variance of accelerometer signal noise in (m/s2)2, specified as a positive real scalar.
Data Types: single
| double
Gyroscope noise ((rad/s)2) — 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.
Data Types: single
| double
Magnetometer noise (μT2) — Variance of magnetometer signal noise (μT2)
0.1
(default) | positive real scalar
Variance of magnetometer signal noise in μT2, specified as a positive real scalar.
Data Types: single
| double
Gyroscope drift noise (rad/s) — 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.
Data Types: single
| double
Linear acceleration noise ((m/s2)2) — Variance of linear acceleration noise
(m/s2)2
0.0096236100000000012
(default) | positive real scalar
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
Magnetic disturbance noise (μT2) — Variance of magnetic disturbance noise ((μT)2)
0.5
(default) | real finite positive scalar
Variance of magnetic disturbance noise in μT2, specified as a real finite positive scalar.
Data Types: single
| double
Linear acceleration decay factor — 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 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
Magnetic disturbance decay factor — 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.
Data Types: single
| double
Magnetic field strength (μT) — Magnetic field strength (μT)
50
(default) | real positive scalar
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
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:
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:
Kalman equations used in this algorithm:
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.
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.
The orientation for the current frame is predicted by first estimating the angular change from the previous frame:
where N is the decimation factor specified by the Decimation factor and fs is the sample rate.
The angular change is converted into quaternions using the rotvec
quaternion
construction syntax:
The previous orientation estimate is updated by rotating it by ΔQ:
During the first iteration, the orientation estimate,
q−, is initialized by ecompass
.
The gravity vector is interpreted as the third column of the quaternion, q−, in rotation matrix form:
A second gravity vector estimation is made by subtracting the decayed linear acceleration estimate of the previous iteration from the accelerometer readings:
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:
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:
The difference between the magnetic vector estimate from the gyroscope readings and the magnetic vector estimate from the magnetometer:
Magnetometer Correct
The magnetometer correct estimates the error in the magnetic vector estimate and detects magnetic jamming.
The magnetic disturbance error is calculated by matrix multiplication of the Kalman gain associated with the magnetic vector with the error signal:
The Kalman gain, K, is the Kalman gain calculated in the current iteration.
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:
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+.
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:
where gx,
gy, and
gz are the x-,
y-, and z-elements of the gravity vector estimated
from the a priori orientation, respectively.
mx,
my, and
mz are the x-,
y-, and z-elements of the magnetic vector
estimated from the a priori orientation, respectively.
κ is a constant determined by the Sample rate and Decimation factor
properties: κ = Decimation
factor
/Sample rate
.
The innovation covariance is a 6-by-6 matrix used to track the variability in the measurements. The innovation covariance matrix is calculated as:
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:
where
and
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:
where K is the Kalman gain, H is the measurement matrix, and P− is the error estimate covariance calculated during the previous iteration.
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:
where
P+ –– is the updated (a posteriori) error estimate covariance
κ –– Decimation factor divided by sample rate.
β –– Gyroscope drift noise.
η –– Gyroscope noise.
ν –– Linear acceleration decay factor.
ξ –– Linear acceleration noise.
σ –– Magnetic disturbance decay factor.
γ –– Magnetic disturbance noise.
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:
where
P− –– predicted error covariance
H –– observation model
S –– innovation covariance
The a posterior error estimate is determined by combining the Kalman gain matrix with the error in the gravity vector and magnetic vector estimations:
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:
Correct
The orientation estimate is updated by multiplying the previous estimation by the error:
The linear acceleration estimation is updated by decaying the linear acceleration estimation from the previous iteration and subtracting the error:
where
ν –– Linear acceleration decay factor
The gyroscope offset estimation is updated by subtracting the gyroscope offset error from the gyroscope offset from the previous iteration:
Compute Angular Velocity
To estimate angular velocity, the frame of gyroReadings
are
averaged and the gyroscope offset computed in the previous iteration is subtracted:
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:
The magnetic disturbance error in the navigation frame is subtracted from the previous magnetic vector estimate and then interpreted as inclination:
The inclination is converted to a constrained magnetic vector estimate for the next iteration:
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 Simulink® Coder™.
Usage notes and limitations:
This block also supports strict single-precision code generation.
Version History
Introduced in R2020a
See Also
ahrsfilter
| ecompass
| imufilter
| imuSensor
| gpsSensor
Comando de MATLAB
Ha hecho clic en un enlace que corresponde a este comando de MATLAB:
Ejecute el comando introduciéndolo en la ventana de comandos de MATLAB. Los navegadores web no admiten comandos de MATLAB.
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)