[Перевод] MEMS accelerometers, magnetometers and orientation angles27.04.2020 10:03

When it’s necessary to evaluate the orientation angles of an object you may have the question — which MEMS sensor to choose. Sensors manufacturers provide a great amount of different parameters and it may be hard to understand if the sensor fit your needs.

Brief: this article is the description of the Octave/Matlab script which allows to estimate the orientation angles evaluation errors, derived from MEMS accelerometers and magnetometers measurements. The input data for the script are datasheet parameters for the sensors. Article can be useful for those who start using MEMS sensors in their devices. You can find the project on GitHub.
We are using the following conditions:

We’re going to estimate the attitude of the stationary object.
Why?
The orientation of the moving object cannot be calculated reliably by using following formulas and you’ll have to use more sophisticated algorithms.
We will use measurements of the MEMS accelerometers and magnetometers to evaluate orientation angles.

1. A bit of theory

Orientation angles

Here, orientation angles is the three Euler Angles — Roll, Pitch and Yaw. They connect the body frame coordinate system XYZ and the local coordinate system East-North-Up (ENU). The Roll, Pitch and Yaw angles denote the rotation that the XYZ axes must make in order to move in the ENU axis. Accordingly, zero angles mean that the X axis of the object is oriented to the east, the Y axis of the object is oriented to the north, the Z axis is oriented up. The order of axes rotation is following: first Yaw (around Z axis), then Pitch (around Y axis) and then Roll (around X axis).

Accelerometer

This sensor measures the projection of proper acceleration onto the sensitivity axis. It’s proper acceleration because the accelerometer measures gravity too (readings not zero even if the sensor is stationary). You can imagine the accelerometer as the weight on a spring. Its measured values are proportional to the stretching degree of the spring. If the accelerometer is stationary the spring is stretched only by gravity. If the accelerometer is in a dynamic state then there’ll be a sum of forces: caused by inertia of the weight $\left(F=m\overrightarrow{a}\right)$ and gravity $\left (F_{g}=m\overrightarrow{g}\right)$ .

We use the following measurements model for the orthogonal (mutually perpendicular) triad of accelerometers:

$a_{XYZ} = m_{a}\cdot A_{XYZ} + b_{a} + n_{a},$

where $a_{XYZ}$ is the measured acceleration vector in body frame coordinates XYZ, $m_{a}$ is the matrix of axes misalignments and scale factors of the accelerometers triad, $A_{XYZ}$ is the true acceleration vector in body frame coordinates XYZ, $b_{a}$ is the accelerometers zero displacement vector (biases), $n_{a}$ is the measurements noise.
The matrix of the axes misalignment and scale factors is:

$m_{a} =\begin{bmatrix} 1 + m_{a, 1,1} & m_{a, 1,2} & m_{a, 1,3} \\ m_{a, 2,1} & 1 + m_{a, 2,2} & m_{a, 2,3} \\ m_{a, 3,1} & m_{a, 3,2} & 1 + m_{a, 3,3} \end {bmatrix},$

where the elements located on the main diagonal ( $1 + m_{a, 1,1}, 1 + m_{a, 2,2}, 1 + m_{a, 3,3}$ ) are scale factors and their errors along three axes of the accelerometers and the remaining elements of the matrix are axes misalignments of the accelerometers triad.

Selecting accelerometers parameters from the datasheet

MPU-9250 accelerometers:

When you know spectral density of the noise power and the passband of the sensor you can calculate the standard deviation of the output noise:

$\sigma _ {noise}=G_{0} \cdot \sqrt{\Pi_{noise}};$

ADIS16488A accelerometers:

Magnetometer

The sensor that measures the projection of the magnetic field induction onto the sensitivity axis. The magnetometer is characterized by hard-iron and soft-iron distortions. Hard-iron distortion is an additive effect: a constant component is added to the measurement. For example, the reason may be the influence of a permanent magnet or a zero offset of the sensor. Soft-iron distortion is a multiplier effect: it reflects a change in direction and/ or attenuation of the magnetic induction vector. This effect can be caused by the presence of a metal object in the immediate vicinity of the magnetometer or by the sensor«s own distortions — an error in the scale factor or its axes misalignments.
We use the following measurements model for the triad of magnetometers:

$m_{XYZ} = S_{m} \cdot M_{XYZ} + b_{m} + n_{m},$

where $m_{XYZ}$ is the magnetometers measurements in body frame coordinates XYZ, $S_{m}$ is the diagonal matrix of the axes misalignments and scale factors (which also describes the soft–iron effect), $M_{XYZ}$ is the true magnetic induction vector in body frame coordinates, $b_{m}$ is the sensors biases (also describes the hard–iron effect), $n_{m}$ is the measurement noise.
Matrix of the axis misalignment and scale factors of the magnetometer:

$S_{m} = \begin {bmatrix} 1 + S_{m, 1,1} & S_{m, 1,2} & S_{m, 1,3} \\ S_{m, 2,1} & 1 + S_{m, 2,2} & S_{m, 2,3} \\ S_{m, 3,1} & S_{m, 3,2} & 1 + S_{m, 3,3} \end {bmatrix},$

elements located on the main diagonal ( $1 + S_{m, 1,1}, 1 + S_{m, 2,2}, 1 + S_{m, 3,3}$ ) are the scale factors and their errors along the three axes of the magnetometer, the remaining elements of the matrix are axes misalignments of the magnetometers triad.

Selecting magnetometer parameters from a datasheet

MPU-9250 magnetometers:

Datasheet does not contain all required parameters. So we supposed that the magnetometers is calibrated and took the following:

Zero offset — $(1 \mu T);$
Scale factors error — $(5 \%);$
Axis misalignment — suppose that they are the same as accelerometers — $(\pm2 \%);$
Output noise — $(0.6 \mu T);$

ADIS16488A magnetometers:

Orientation angles calculation

Due to the the Earth gravity accelerometers «sense» the down direction. Their measurements are used to calculate roll and pitch angles. Here you’ll find the formulas to calculate them. The yaw angle (in this case it’s the magnetic azimuth) can be determined due to the Earth«s magnetic field. The induction vector of the magnetic field is measured by magnetometers and their measurements are involved in the calculation of the yaw angle.
We should notice that to calculate the magnetic azimuth it’s necessary to use the magnetometer measurements translated to the horizontal plane. Here you’ll find the formula for calculating the magnetic azimuth.

$roll=atan\left(\frac{a_{Y}}{a_{Z}}\right),$

$pitch=atan\left(\frac{-a_{X}}{\sqrt{a_{Y}^{2} + a_{Z}^{2}}}\right),$

$yaw=atan2\left(\frac{m_{E}}{m_{N}}\right),$

where $ atan2 $ is the full arctangent function, $a_{X}$ , $a_{Y}$ , $a_{Z}$ — measurements of the accelerometers along three axes in the body frame, $m_{E}$ , $m_{N}$ — measurements of the magnetometers along the axes X', Y' (measurements of magnetometers are translated to the horizontal plane).

2. Orientation angles estimation errors

Algorithm description

We create an arrays of random Euler angles roll, pitch, yaw. They specify sets of the true orientation of the object in the model.
Why do we use so many angles?
Errors depend on the values of the orientation angles and if we want to get an idea of the errors magnitude over the entire range then this is the easiest way.
Using created angles we evaluate transformation matrixes from body frame to the local coordinate system:
$C_{XYZ}^{ENU}=\begin{vmatrix}cy\cdot cp& -cr\cdot sy + sr\cdot cy\cdot sp & sr\cdot sy + cr\cdot cy\cdot sp\\ sy\cdot cp & cr\cdot cy + sr\cdot sy\cdot sp & -sr\cdot cy + cr\cdot sy\cdot sp\\ -sp & sr\cdot cp & cr\cdot cp \end{vmatrix},$
where $cr = \cos(roll)$ , $sr = \sin(roll)$ , $cp = \cos (pitch)$ , $sp = \sin(pitch)$ , $cy = \cos(yaw)$ , $sy = \sin(yaw)$ .
Using this matrix we got an expression for true accelerations in body frame coordinate system:
$A_ {XYZ}=\left (C_{XYZ}^{ENU}\right)^{T}\cdot \begin {vmatrix} 0 \\ 0 \\ 1\\ \end {vmatrix},$

$\begin {vmatrix} 0 \\ 0 \\ 1\\ \end {vmatrix}$ is the vector that determines the direction of gravity expressed in units of g, ${(C_{XYZ}^{ENU})}^{T}$ is the coordinate transformation matrix from the local coordinate system to the body frame (the inverse of the transformation matrix from the body frame to the local coordinate system).
Accelerometers measurements model:
$a_{XYZ}=\left (I + m_{a}\right)\cdot A_{XYZ} + b_{a} + n_{a},$
Roll and pitch angles estimations calculated using the formulas:
$roll'=atan\left(\frac{a_{Y}}{a_{Z}}\right),$

$pitch'=atan\left(\frac {-a_{X}}{\sqrt{a_{Y}^{2} + a_{Z}^{2}}}\right).$
It’s also necessary to form the conversion matrix to the «horizon» from these angles. We used rpy2mat fuction:
$C_{XYZ}^{XYZ'}=rpy2mat\left(\begin{bmatrix} roll '\\ pitch '\\ 0 \end{bmatrix}^{T}\right),$
where the roll and pitch angles are the angles calculated from the accelerometer measurements and the third angle is zero.
Then we took the vector of true magnetic induction in the local coordinate system ENU and translated it to the body frame XYZ:
$M_{XYZ}={(C_{XYZ}^{ENU})}^{T}\cdot M_{ENU}.$
Magnetometers measurements model:
$m_{XYZ}=S_{m}\cdot M_{XYZ} + b_{m} + n_{m}.$
Then we recalculated the magnetometers measurements from the body frame to the «horizon»:
$m_{XYZ'} = C_{XYZ}^{XYZ'}\cdot m_{XYZ}.$
Using the «horizontal» measurements of the magnetometers we calculated the magnetic azimuth (estimation):
$yaw'=atan2\left(\frac{m_{Y'}}{m_{X'}}\right).$
Orientation angles estimation errors calculated as the difference between the true angles of roll, pitch, yaw and calculated from the sensors measurements — roll', pitch', yaw'.

3. Results

Here is the results for two sensors which we use as an example — ADIS16488A and MPU-9250 (maximum errors).
Note that this results is not a strict verdict. Results depends on supposed calibration errors. Our script should be used to get acquainted with the orientation angles estimation errors magnitude order and dependance on different input parameters.

Accelerometers and magnetometers errors influence on the orientation angles estimation errors

As you can see the magnitude of the errors increases when approaching the boundary of the angles range. Why?

Сonsider the figure below:

Suppose that we rotate the sensitivity axis Z $(z1 \rightarrow z2)$ of the accelerometer so that the projection of gravity on this axis becomes smaller $$(g '\rightarrow g$ . The value of the gravity projection plus the error of the accelerometer will give the range of possible measurement values (pink area). Angle estimation error increases $(\Delta _ {1} \rightarrow \Delta _ {2})$ . So when gravity vector projection on the sensitivity axis decreases — constant accelerometer error begins to introduce a larger error into the angle estimate.

Now let’s play with the input parameters to understand how the errors are defined:

Influence of the ONLY accelerometers errors on the errors of orientation angles estimation:

Influence of the ONLY magnetometers errors on orientation angles estimation errors:

Summary

We have developed the Octave/MATLAB script that allows to estimate errors in the calculation of orientation angles from measurements of MEMS accelerometers and magnetometers.
Errors in estimating orientation angles are the functions of all orientation angles. In this case the maximum values of errors are observed at the boundaries of the selected ranges of angles.

Spoiler — disclaimer:

We don’t recommend buying and using these sensors (they are quite old).
We don’t take into account the influence of non-linearity of measurements, the influence of vibration, instability. That’s only the first approximation to the angles errors.

Literature

Authors

Moscow Power Engineering Institute, Dept. of Radio Systems