🏛️ 🦅 〰️ MEMS accelerometers, magnetometers and orientation angles 👩🏽‍🤝‍👩🏻 🔨 💅🏼

When developing devices in which it is necessary to evaluate orientation angles, the question may arise - which MEMS sensor to choose. Sensor manufacturers provide many parameters from which we need to obtain useful information about the quality of the sensor.

To understand what kind of angle accuracy we can count on, you need to make a certain amount of effort.

TL; DR : A small script for Octave / MATLAB is described, which allows to estimate errors in calculating orientation angles from MEMS measurements of accelerometers and magnetometers. At the script input - parameters of sensors from datasheets (and / or calibration errors). The article may be useful to those who begin to use inertial sensors in their devices. A small educational program on the sensors is included. Link to github too.

We immediately accept the following conditions:

We want to evaluate the orientation angles of a stationary device.

Why?
, .
To estimate angles, we will use MEMS measurements of accelerometers and magnetometers.

1. Brief educational program

Orientation angles

We will understand the orientation angles of the object as the Euler angles - roll, pitch, yaw, connecting the object’s own XYZ coordinate system and the local east-north-top coordinate system (ENU - East North Up). The roll, pitch, 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 facing east, the Y axis of the object is looking north, and the Z axis is up.

The rotation order of the axes is from the last angle: first on yaw (around the Z axis), then on pitch (around the Y axis), then on roll (around the X axis).

Accelerometer

This is a sensor that measures the projection of apparent acceleration onto the sensitivity axis. It seems - because it measures gravity too, even while the accelerometer is stationary. It is easiest to imagine an accelerometer as a weight on a spring; its issued measurements are proportional to the degree of tension of the spring. If the accelerometer is at rest, the spring is stretched only by gravity. If it accelerates, then there will be a sum of forces: inertia of the weight

(F = m \vec{a})

$\left (F=m\overrightarrow{a}\right )$ and gravity

(F_{g} = m \vec{g})

$\left (F_{g}=m\overrightarrow{g}\right )$

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

a_{X Y Z} = m_{a} \cdot A_{X Y Z} + b_{a} + n_{a},

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

Where

a_{X Y Z}

$a_{XYZ}$ - measured acceleration in SSC (own coordinate system) XYZ,

m_{a}

$m_{a}$ - skew matrix of axes and scale factors of the accelerometer,

A_{X Y Z}

$A_{XYZ}$ Is the vector of true acceleration in CCK XYZ,

b_{a}

$b_{a}$ Is the zero displacement vector of the accelerometer,

n_{a}

$n_{a}$ - measurement noise.

The skew matrix of the axes and scale factors is as follows:

m_{a} = [\begin{matrix} 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{matrix}],

$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 are located on the main diagonal (

1 + m_{a, 1, 1}, 1 + m_{a, 2, 2}, 1 + m_{a, 3, 3}

$1+m_{a,1,1}, 1+m_{a,2,2}, 1+m_{a,3,3}$ ) Are the scale factors and their errors along the three axes of the accelerometer, and the remaining elements of the matrix are the distortions of the axes of the accelerometer.

Accelerometer selection from datasheet

MPU-9250

— Zero-G Initial Calibration Tolerance ( $\pm60mg$ $X, Y$ , $\pm80mg$ $Z$ ) — $g$ $10^{-3};$
— Initial Tolerance ( $\pm3\%$ ) — . , $10^{-2};$
— Cross Axis Sensitivity ( $\pm2\%$ ) — $10^{-2};$
— Noise Power Spectral Density $\left (300\frac{\mu g}{\sqrt{Hz}} \right )$ — $g$ $10^{-6};$
— Low Pass Filter Response $\left (5-260 Hz\right )$ — , . . ;

σ_{n o i s e} = G_{0} \cdot \sqrt{Π_{n o i s e}};

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

ADIS16488A:

— Bias Repeatability ( $\pm 16mg$ ) — $g$ $10^{-3};$
— (Sensitivity) Repeatability ( $\pm 0.5\%$ ) — ;
— Misalignment Axis to frame ( $\pm1^{\circ}$ ) — , (, );
— Noise Density $\left ( 0.063\frac{mg}{\sqrt{Hz}}rms \right )$ — $g;$
— $\left (3dB Bandwidth\right)$ — , MPU-9250;

Magnetometer

A sensor that measures the projection of magnetic field induction onto the sensitivity axis. The magnetometer is characterized by distortions of hard-iron and soft-iron. Hard-iron distortion is an additive effect when a constant component is added to the measured field. The reason may be, for example, the action of a permanent magnet or a self-zero offset of the sensor. Soft-iron distortion is a multiplier effect reflecting 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 a distortion of its sensitivity axis.
We accept the measurement model of the triad of magnetometers:

m_{X Y Z} = S_{m} \cdot M_{X Y Z} + b_{m} + n_{m},

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

Where

m_{X Y Z}

$m_{XYZ}$ - magnetometer measurements in CCK XYZ,

S_{m}

$S_{m}$ - diagonal skew matrix of axes and scale factors (which describes the soft – iron effect),

M_{X Y Z}

$M_{XYZ}$ Is the vector of true magnetic induction in SSK,

b_{m}

$b_{m}$ - displacement of the zeros of the magnetometer (describes the hard – iron effect),

n_{m}

$n_{m}$ - measurement noise.
The matrix of the skew axis and scale factors of the magnetometer:

S_{m} = [\begin{matrix} 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{matrix}],

$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 (

S_{m, 1, 1}, S_{m, 2, 2}, S_{m, 3, 3}

$S_{m,1,1}, S_{m,2,2}, 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 the distortions of the axes of the magnetometer. All matrix elements also take into account the effect of soft-iron.

Choice of magnetometer parameters from a datasheet

MPU-9250

, , :

— $(1\mu T);$
— $(5\%);$
— , , — $(\pm2\%);$
— $(0.6\mu T);$

ADIS16488A

— Initial Bias Error $\left (\pm15 mgauss = 1.5 \mu T \right )$ — , $(0.5 \mu T)$ ;
— Initial Sensitivity Tolerance $\left (2\% \right );$
— Misalignment Axis to axis $\left (0.35^{\circ} \right )$ — , (, );
— Noise Density $\left (0.042\frac{mgauss}{\sqrt{Hz}} \right )$ — $\left (\frac{Tesla}{\sqrt{Hz}} \right );$
— $260 Hz;$

Calculation of orientation angles

Due to the presence of gravity on the Earth, accelerometers “sense” the downward direction. Their measurements are used to calculate roll and pitch angles. Formulas for calculation can be found here . The third is the yaw angle (and in this case, the magnetic azimuth), can be determined due to the presence of the Earth’s magnetic field. The magnetic field induction vector is measured by magnetometers and their measurements are involved in the calculation of the yaw angle. It should be noted that in calculating the magnetic azimuth, magnetometer measurements are used, calculated in the plane. Here you can find the formula for calculating the magnetic azimuth.

r o l l = a t a n (\frac{a_{Y}}{a_{Z}}),

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

p i t c h = a t a n (\frac{- a_{X}}{\sqrt{a_{Y}^{2} + a_{Z}^{2}}}),

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

y a w = a t a n 2 (\frac{m_{E}}{m_{N}}),

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

Where

a t a n 2

$atan2$ - full arc tangent function,

a_{X}

$a_{X}$ ,

a_{Y}

$a_{Y}$ ,

a_{Z}

$a_{Z}$ - measurement of the accelerometer along three axes in the CCK,

m_{E}

$m_{E}$ ,

m_{N}

$m_{N}$ - measurements of the magnetometer along the X ', Y' axes (the measurements of the magnetometers are counted in the plane).

2.

roll, pitch, yaw. .
?
, — .
roll, pitch, yaw XYZ ENU:

$C_{X Y Z}^{E N U} = | \begin{matrix} c y \cdot c p & - c r \cdot s y + s r \cdot c y \cdot s p & s r \cdot s y + c r \cdot c y \cdot s p \\ s y \cdot c p & c r \cdot c y + s r \cdot s y \cdot s p & - s r \cdot c y + c r \cdot s y \cdot s p \\ - s p & s r \cdot c p & c r \cdot c p \end{matrix} |,$
$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},$
$cr=\cos (roll)$ , $sr=\sin(roll)$ , $cp=\cos(pitch)$ , $sp=\sin(pitch)$ , $cy=\cos(yaw)$ , $sy=\sin(yaw)$ .
:
$A_{X Y Z} = {(C_{X Y Z}^{E N U})}^{T} \cdot | \begin{matrix} 0 \\ 0 \\ - 1 \end{matrix} |,$
$A_{XYZ}=\left ( C_{XYZ}^{ENU} \right )^{T}\cdot \begin{vmatrix} 0\\ 0\\ -1\\ \end{vmatrix},$

$\begin{vmatrix} 0\\ 0\\ -1\\ \end{vmatrix}$ — , , g, ${(C_{XYZ}^{ENU} )}^{T}$ — ( ).
:
$a_{X Y Z} = (I + m_{a}) \cdot A_{X Y Z} + b_{a} + n_{a},$
$a_{XYZ}=\left ( I+m_{a} \right )\cdot A_{XYZ}+b_{a}+n_{a},$
() :
$r o l l^{'} = a t a n (\frac{a_{Y}}{a_{Z}}),$
$roll'=atan \left ( \frac{a_{Y}}{a_{Z}} \right ),$
$p i t c h^{'} = a t a n (\frac{- a_{X}}{\sqrt{a_{Y}^{2} + a_{Z}^{2}}}) .$
$pitch'=atan\left ( \frac{-a_{X}}{\sqrt{a_{Y}^{2}+a_{Z}^{2}}} \right ).$
«» , rpy2mat:
$C_{X Y Z}^{X Y Z^{'}} = r p y 2 m a t ({[\begin{matrix} r o l l^{'} \\ p i t c h^{'} \\ 0 \end{matrix}]}^{T}),$
$C_{XYZ}^{XYZ'}=rpy2mat\left ( \begin{bmatrix} roll'\\ pitch'\\ 0 \end{bmatrix}^{T} \right ),$
roll' pitch' — , , — .
ENU XYZ:
$M_{X Y Z} = {(C_{X Y Z}^{E N U})}^{T} \cdot M_{E N U} .$
$M_{XYZ}= {(C_{XYZ}^{ENU} )}^{T}\cdot M_{ENU}.$
:
$m_{X Y Z} = S_{m} \cdot M_{X Y Z} + b_{m} + n_{m} .$
$m_{XYZ}=S_{m}\cdot M_{XYZ}+b_{m}+n_{m}.$
«»:
$m_{X Y Z^{'}} = C_{X Y Z}^{X Y Z^{'}} \cdot m_{X Y Z} .$
$m_{XYZ'}=C_{XYZ}^{XYZ'}\cdot m_{XYZ}.$
«» c :
$y a w^{'} = a t a n 2 (\frac{m_{Y^{'}}}{m_{X^{'}}}) .$
$yaw'=atan2\left ( \frac{m_{Y'}}{m_{X'}} \right ).$
roll, pitch, yaw — roll', pitch', yaw'.

3. —

For the two sensors that we took as an example - ADIS16488A and MPU-9250, the marginal errors in estimating orientation angles were obtained under the combined influence of the errors of the accelerometer and magnetometer.

The table below shows the maximum values of errors received:

Angle	MPU-9250	ADIS16488A
Roll	$30^{\circ}$ $30^{\circ}$	$8^{\circ}$ $8^{\circ}$
Pitch	$10^{\circ}$ $10^{\circ}$	$2^{\circ}$ $2^{\circ}$
Magnetic azimuth	$30^{\circ}$ $30^{\circ}$	$20^{\circ}$ $20^{\circ}$

The combined effect of the errors of the accelerometer and magnetometer on the errors of estimation of orientation angles:

This is how roll estimation errors look like depending on roll and pitch values:
Pitch estimation errors depending on roll and pitch values:
Errors in estimating magnetic azimuth from roll and pitch angles:
Errors of estimation of magnetic azimuth from roll angles and magnetic azimuth:
Errors in estimating magnetic azimuth from pitch angles and magnetic azimuth: