2x1=10

When div­ing into the field of atti­tude detec­tion from sen­sor read­ings a cou­ple of answers seem omnipresent:

• You need an accelerom­e­ter and a mag­ne­tome­ter.
• You need to tilt com­pen­sate.
• You need to low-pass fil­ter.
• Or use the com­pli­men­ta­ry fil­ter.
• Or bet­ter still, use the mys­ti­cal Kalman fil­ter (but it’s too com­pli­cat­ed to explain).

Start­ing from the above and hav­ing an accelerom­e­ter and a mag­ne­tome­ter at hand, the old ques­tion a wise man once asked is:

To tilt com­pen­sate, or not to tilt com­pen­sate, that is the ques­tion—
Whether ’tis Nobler in the mind to suf­fer
The Slings and Arrows of out­ra­geous Stack­Over­flow answers,
Or to take Arms against a Sea of trou­bles,
And by using the atti­tude matrix, end them? To tilt: to roll.

In oth­er words: Is tilt com­pen­sa­tion required?

The Tilt Compensation

To make a long sto­ry short, the com­mon way to cal­cu­late yaw angle \(\psi\) (read: psi) by tilt-com­pen­sat­ing a 3-axis mag­ne­tome­ter using pitch angle \(\theta\) (read: theta) and roll angle \(\phi\) (read: phi) is by re-map­ping the sen­sor read­ings to the ref­er­ence X-Y plane by cal­cu­lat­ing

\(
\begin{align}
\begin{bmatrix}
\hat{m}_x \\ \hat{m}_y
\end{bmatrix} =
\begin{bmatrix}
cos(\theta) && sin(\phi) \cdot sin(\theta) && cos(\phi) \cdot sin(\theta) \\
0 && cos(\phi) && -sin(\theta)
\end{bmatrix} \cdot
\begin{bmatrix}
m_x \\ m_y \\ m_z
\end{bmatrix}
\end{align}
\)

In oth­er words:

\(
\begin{align}
\hat{m}_x &= m_x \cdot cos(\theta) + m_y \cdot sin(\phi) sin(\theta) + m_z \cdot cos(\phi) sin(\theta) \\
\hat{m}_y &= m_y \cdot cos(\phi) - m_z \cdot sin(\phi)
\end{align}
\)

And then tak­ing the arc­t­an­gent to get the yaw angle:

\(
\begin{align}
\psi &= tan^{-1}(-\frac{\hat{m}_y}{\hat{m}_x}) \\
&= atan2(-\hat{m}_y, \hat{m}_x)
\end{align}
\)

Obvi­ous­ly, \(\theta\) and \(\phi\) would be need­ed before doing so, and they may be obtained from the accelerom­e­ter — assum­ing aero­nau­tic ref­er­ence sys­tem, i.e. Tait-Bryan XYZ — by cal­cu­lat­ing

\(
\begin{align}
\phi &= tan^{-1}(\frac{a_y}{a_z}) \\
\theta &= tan^{-1}(-\frac{a_x}{\sqrt{a_x^2 + a_z^2}})
\end{align}
\)

Since the denom­i­na­tor in the \(\theta\) cal­cu­la­tion is nev­er neg­a­tive, the angle is con­strained to a range of \(\pm 90^\circ\) , where­as \(\phi\) and \(\psi\) may con­tain val­ues in the range of \(\pm 180^\circ\) .

The ques­tion is, since we do have two axes point­ing down and some­what north or for­ward — just don’t be fooled to assume that a mag­ne­tome­ter will mag­i­cal­ly do that, or you will be bad­ly sur­prised by the read­ings it’ll give you — so, since we have these axes, why the need to tilt com­pen­sate?

A good Attitude

The short answer is: We don’t need to, and the solu­tion smells a lot like the TRIAD algo­rithm. (That might be due to the fact that it is designed to sort of do what we want.)

The solu­tion takes advan­tage out of two facts:

• The cross prod­uct of two vec­tors is a vec­tor orthog­o­nal to them (in layman’s terms: It is in a \(90^\circ\) angle to both of them)
• The dot prod­uct of two vec­tors is the cosine of the angle between them.

Assum­ing the accelerom­e­ter mea­sures pos­i­tive up, i.e. gives a neg­a­tive read­ing for the grav­i­ty when the top side is actu­al­ly on the top, and that both sen­sors are some­what cal­i­brat­ed to not total­ly bork up our cal­cu­la­tions, then here’s the gen­er­al pro­ce­dure:

First, we invert the accelerom­e­ter vec­tor so that it indeed points up. After nor­mal­iza­tion, it then forms our new local \(\vec{z}\) axis:

\(
\begin{align}
\vec{z} = -\frac{\vec{a}}{|\vec{a}|} = \frac{-\vec{a}}{\sqrt{a_x^2 + a_y^2 + a_z^2}}
\end{align}
\)

We then cross \(\vec{z}\) with the mag­ne­tome­ter vec­tor \(\vec{m}\) and nor­mal­ize in order to get our local \(\vec{y}\) axis:

\(
\begin{align}
\vec{y} = \frac{\vec{z} \times \vec{m}}{|\vec{z} \times \vec{m}|}
\end{align}
\)

Since \(\vec{y}\) is orthog­o­nal to \(\vec{m}\) and \(\vec{z}\) , the actu­al pitch of the mag­ne­tome­ter vec­tor does not mat­ter as long as \(\vec{m}\) is not collinear with \(\vec{z}\) . In that case you are stand­ing either on the north or the south pole and there real­ly is no defined north. In addi­tion, you are prob­a­bly freez­ing and have oth­er prob­lems.

Since \(\vec{y}\) and \(\vec{z}\) are now well defined (that is, ortho­nor­mal and point­ing in the gen­er­al direc­tion of suc­cess), we cross them again to get an orthog­o­nal \(\vec{x}\) axis:

\(
\begin{align}
\vec{x} = \vec{y} \times \vec{z}
\end{align}
\)

Renor­mal­iza­tion is sug­gest­ed for prac­ti­cal rea­sons, but not required math­e­mat­i­cal­ly.

We now need to define a set of ref­er­ence vec­tors, say \(\vec{X}\) , \(\vec{Y}\) and \(\vec{Z}\) in upper case nota­tion. To make life easy, we’ll sim­ply assume them to be unit vec­tors, that is

\(
\begin{align}
\vec{X} =
\begin{bmatrix}
1 \\ 0 \\ 0
\end{bmatrix} \quad
\vec{Y} =
\begin{bmatrix}
0 \\ 1 \\ 0
\end{bmatrix} \quad
\vec{Z} =
\begin{bmatrix}
0 \\ 0 \\ 1
\end{bmatrix} \\
\end{align}
\)

Once this is done, we can build the atti­tude (or direc­tion cosine) matrix \(\underline{A}\) from the dot prod­ucts of the vec­tors (remem­ber that the dot prod­uct of two vec­tors is the cosine of the angle between them), as fol­lows:

\(
\begin{align}
\underline{A} &= \begin{bmatrix}
\vec{x} \cdot \vec{X} && \vec{y} \cdot \vec{X} && \vec{z} \cdot \vec{X} \\
\vec{x} \cdot \vec{Y} && \vec{y} \cdot \vec{Y} && \vec{z} \cdot \vec{Y} \\
\vec{x} \cdot \vec{Z} && \vec{y} \cdot \vec{Z} && \vec{z} \cdot \vec{Z}
\end{bmatrix}
\end{align}
\)

TRIAD for­mu­lates this as

\(
\begin{align}
\underline{A} &=
\begin{bmatrix}
\vec{x} \; \vdots \; \vec{y} \; \vdots \; \vec{z}\end{bmatrix} \cdot
\begin{bmatrix}
\vec{X} \; \vdots \; \vec{Y} \; \vdots \; \vec{Z}\end{bmatrix} \\
&=
\begin{bmatrix}
x_x && y_x && z_x \\
x_y && y_y && z_y \\
x_z && y_z && z_z
\end{bmatrix} \cdot
\begin{bmatrix}
X_x && Y_x && Z_x \\
X_y && Y_y && Z_y \\
X_z && Y_z && Z_z
\end{bmatrix}
\end{align}
\)

Now since we just defined \(\vec{X}\) , \(\vec{Y}\) and \(\vec{Z}\) to be unit vec­tors, we have

\(
\begin{align}
\underline{A} &=
\begin{bmatrix}
x_x && y_x && z_x \\
x_y && y_y && z_y \\
x_z && y_z && z_z
\end{bmatrix} \cdot
\begin{bmatrix}
1 \; && 0 \; && 0 \\
0 \; && 1 \; && 0 \\
0 \; && 0 \; && 1
\end{bmatrix} = \begin{bmatrix}
x_x && y_x && z_x \\
x_y && y_y && z_y \\
x_z && y_z && z_z
\end{bmatrix}
\end{align}
\)

It can’t get more con­ve­nient than that. No mat­ter how we get there, this matrix \(\underline{A}\) con­tains all angu­lar rela­tion­ships between all axes, so it is just a mat­ter of extract­ing them. Sim­i­lar to the method described in the tilt com­pen­sa­tion algo­rithm, we can get our angles from the matrix as fol­lows:

\(
\begin{align}
\theta &= -sin^{-1}(\underline{A}_{1, 3}) \\
\phi &= tan^{-1}(\underline{A}_{2, 3}, \underline{A}_{3, 3}) = atan2(\underline{A}_{2, 3}, \underline{A}_{3, 3}) \\
\psi &= tan^{-1}(\underline{A}_{1, 2}, \underline{A}_{1, 1}) = atan2(\underline{A}_{1, 2}, \underline{A}_{1, 1})
\end{align}
\)

This gives us

\(
\begin{align}
\theta &= -sin^{-1}(\vec{z} \cdot \vec{X}) \\
\phi &= atan2(\vec{z} \cdot \vec{Y}, \vec{z} \cdot \vec{Z}) \\
\psi &= atan2(\vec{y} \cdot \vec{X}, \vec{x} \cdot \vec{X})
\end{align}
\)

And, again, since our ref­er­ence axes are unit vec­tors, that reduces to:

\(
\begin{align}
\theta &= -sin^{-1}(z_x) \\
\phi &= atan2(z_y, z_z) \\
\psi &= atan2(y_x, x_x)
\end{align}
\)

Again, because of the (arc)sine in the \(\theta\) cal­cu­la­tion, pitch angle can­not exceed \(\pm 90^\circ\) .
So, did we tilt com­pen­sate? Sort of, by encod­ing roll and pitch infor­ma­tion in \(\vec{y}\) and \(\vec{x}\) (mind you, we hat to cal­cu­late these). But then again we didn’t. In a way.

Final Words

One inter­est­ing (read: nasty) behav­iour is that head­ing and roll angle seem to jump about \(180^\circ\) when the device’s actu­al pitch angle exceeds \(\pm 90^\circ\) , that is, rolls over. In this case, the cal­cu­lat­ed pitch angle will go down or up again, respec­tive­ly, since it’s, by def­i­n­i­tion, con­strained to a \(\pm 90^\circ\) range. The head­ing and roll angles, on the oth­er hand, will be flipped by \(180^\circ\) , since the device is now — essen­tial­ly — in a reverse posi­tion. If you nev­er rotate a device con­ti­nous­ly for \(180^\circ\) and watch the the out­put, this will not be a prob­lem. But if you do, say, if you intend to use your derived angles to update a Kalman fil­ter, then sim­ply take care to han­dle this case in order to not mess up it’s state esti­mate with seem­ing­ly bogus read­ings.