Orientation from magnetometer and accelerometer readings

`orientation = ecompass(accelerometerReading,magnetometerReading)`

`orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat)`

returns a `orientation`

= ecompass(`accelerometerReading`

,`magnetometerReading`

)`quaternion`

that can rotate quantities from a parent (NED) frame to a child (sensor)
frame.

specifies the orientation format as `orientation`

= ecompass(`accelerometerReading`

,`magnetometerReading`

,`orientationFormat`

)`quaternion`

or rotation
matrix.

The `ecompass`

function returns a quaternion or rotation matrix
that can rotate quantities from a parent (NED) frame to a child (sensor) frame. For both
output orientation formats, the rotation operator is determined by computing the
rotation matrix.

The rotation matrix is first calculated with an intermediary:

$$R=\left[\begin{array}{ccc}& & \\ (a\times m)\times a& a\times m& a\\ & & \end{array}\right]$$

and then normalized column-wise. *a* and *m* are the
`accelerometerReading`

input and the
`magnetometerReading`

input, respectively.

To understand the rotation matrix calculation, consider an arbitrary point on the
Earth and its corresponding local NED frame. Assume a sensor body frame,
[*x*,*y*,*z*], with the same
origin.

Recall that orientation of a sensor body is defined as the rotation operator (rotation matrix or quaternion) required to rotate a quantity from a parent (NED) frame to a child (sensor body) frame:

$$\left[\begin{array}{ccc}& & \\ & R& \\ & & \end{array}\right]\left[\begin{array}{c}\\ {p}_{\text{parent}}\\ \end{array}\right]=\left[\begin{array}{c}\\ {p}_{\text{child}}\\ \end{array}\right]$$

where

*R*is a 3-by-3 rotation matrix, which can be interpreted as the orientation of the child frame.*p*_{parent}is a 3-by-1 vector in the parent frame.*p*_{child}is a 3-by-1 vector in the child frame.

For a stable sensor body, an accelerometer returns the acceleration due to gravity. If
the sensor body is perfectly aligned with the NED coordinate system, all acceleration
due to gravity is along the *z*-axis, and the accelerometer reads [0 0
1]. Consider the rotation matrix required to rotate a quantity from the NED coordinate
system to a quantity indicated by the accelerometer.

$$\left[\begin{array}{ccc}{r}_{11}& {r}_{21}& {r}_{31}\\ {r}_{12}& {r}_{22}& {r}_{32}\\ {r}_{13}& {r}_{23}& {r}_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}{a}_{1}\\ {a}_{2}\\ {a}_{3}\end{array}\right]$$

The third column of the rotation matrix corresponds to the accelerometer reading:

$$\left[\begin{array}{c}{r}_{31}\\ {r}_{32}\\ {r}_{33}\end{array}\right]=\left[\begin{array}{c}{a}_{1}\\ {a}_{2}\\ {a}_{3}\end{array}\right]$$

A magnetometer reading points toward magnetic north and is in the
*N*-*D* plane. Again, consider a sensor body frame
aligned with the NED coordinate system.

By definition, the *E*-axis is perpendicular to the
*N*-*D* plane, therefore *N* ⨯
*D* = *E*, within some amplitude scaling. If the
sensor body frame is aligned with NED, both the acceleration vector from the
accelerometer and the magnetic field vector from the magnetometer lie in the
*N*-*D* plane. Therefore *m* ⨯
*a* = *y*, again with some amplitude
scaling.

Consider the rotation matrix required to rotate NED to the child frame,
[*x*
*y*
*z*].

$$\left[\begin{array}{ccc}{r}_{11}& {r}_{21}& {r}_{31}\\ {r}_{12}& {r}_{22}& {r}_{32}\\ {r}_{13}& {r}_{23}& {r}_{33}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}{a}_{1}\\ {a}_{2}\\ {a}_{3}\end{array}\right]\times \left[\begin{array}{c}{m}_{1}\\ {m}_{2}\\ {m}_{3}\end{array}\right]$$

The second column of the rotation matrix corresponds to the cross product of the accelerometer reading and the magnetometer reading:

$$\left[\begin{array}{c}{r}_{21}\\ {r}_{22}\\ {r}_{23}\end{array}\right]=\left[\begin{array}{c}{a}_{1}\\ {a}_{2}\\ {a}_{3}\end{array}\right]\times \left[\begin{array}{c}{m}_{1}\\ {m}_{2}\\ {m}_{3}\end{array}\right]$$

By definition of a rotation matrix, column 1 is the cross product of columns 2 and 3:

$$\begin{array}{l}\left[\begin{array}{c}{r}_{11}\\ {r}_{12}\\ {r}_{13}\end{array}\right]=\left[\begin{array}{c}{r}_{21}\\ {r}_{22}\\ {r}_{23}\end{array}\right]\times \left[\begin{array}{c}{r}_{31}\\ {r}_{32}\\ {r}_{33}\end{array}\right]\\ \text{\hspace{1em}}\text{\hspace{0.05em}}\text{\hspace{0.05em}}=\left(a\times m\right)\times a\end{array}$$

Finally, the rotation matrix is normalized column-wise:

$${R}_{ij}=\frac{{R}_{ij}}{\sqrt{{\displaystyle \sum _{i=1}^{3}{R}_{ij}^{2}}}}\text{\hspace{1em}},\text{\hspace{0.17em}}\forall j$$

The `ecompass`

algorithm uses magnetic north, not true north,
for the NED coordinate system.

[1] Open Source Sensor Fusion. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs