• ## To tilt compensate, or not to tilt compensate

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}

\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.