# Tilt compensation for yaw calculation from magnetometer and accelerometer

I am working on a project where I have to calculate the roll, pitch, and yaw angles using an accelerometer and a magnetometer.

I calculate the pitch and roll angles using the accelerometer and I am trying to calculate the yaw using tha magnetometer with the following equation:

 xM2 = xM * cos(pitch) + zM * sin(pitch);
yM2 = xM * sin(roll) * sin(pitch) + yM * cos(roll) - zM * sin(roll) * cos(pitch);
compHeading = (atan2(yM2, xM2) * 180 / Pi);


I am trying to implement tilt compensation, so that no matter the position of the x and y axis, the yaw angle can be calculated.

I can assure the roll and pitch angles from the accelerometer are right and the magnetometer compensation is correct. The pitch and roll are [-180 180].

The problem I am facing, is that I think I am using the equation wrong, as the accelerometer and magnetometer axes are not exactly the same.

The following image show both axes:

Can anyone confirm the equation is used correctly?

• 1) Which set of axes (accelerometer or magnetometer) are the axes to which pitch , yaw, and roll are defined ? 2) Can you add a diagram ? 3) "i think i am using the equation wrong". What makes you think so ? If the hardware is already set up, can you simply test the results at known positions ?
– AJN
Oct 8, 2020 at 12:09
• Since YA = -YM and ZA = -ZM and XA = XM, simplest scheme is to invert the magneto meter y and z readings so that the new, virtual, axes of magnetometer and the axes of the accelerometer become the same.
– AJN
Oct 8, 2020 at 12:12
• 1) roll would be the rotation on the x axis, pitch on y axis and yaw on z axis Oct 8, 2020 at 15:42
• 3) It makes me think I'm using it wrong since I can't get consistent yaw angle data once I tilt the accelerometer and magnetometer. when working without moving the xy plane the results of the yaw are consistent but once I tilt the xy plane a little the data varies when it shouldn't Oct 8, 2020 at 15:50

## Assumptions

1. AFAIK, the sequence of the rotations are also important. It is not given in the question. I will assume the sequence to go from inertial frame to body frame is (Yaw, Pitch, Roll) = (Z, Y, X). So the sequence to go from body to inertial is the reverse. Since these details are not present in the question, I assume the convention given in Link 1 and Link2.
2. Accelerometer frame is same as IMU reference

## The transformation

The measured vectors are obtained in IMU (accelerometer) frame. To take a vector resolved in IMU frame, to inertial(?) frame, the transformation as given in the above reference is

$$\begin{bmatrix} v \end{bmatrix}^I_{3\times1} = \begin{bmatrix} C\psi & -S\psi & 0\\ S\psi & C\psi & 0\\ 0 & 0 & 1\\ \end{bmatrix} \color{red}{ \begin{bmatrix} C\theta & 0 & S\theta\\ 0 & 1 & 0\\ -S\theta & 0 & C\theta\\ \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & C\phi & -S\phi\\ 0 & S\phi & C\phi\\ \end{bmatrix} } \begin{bmatrix} v \end{bmatrix}^{IMU}_{3\times1}$$

The red matrices indicate what I assume is the transformation equation set shown in the question.

Assume the magnetometer data was available in the same frame of reference as the accelerometer. Let that reading be $$\[x_M', y_M'z_M']^T\$$.

$$\begin{bmatrix} x_{M2}\\ y_{M2}\\ z_{M2} \end{bmatrix} = \begin{bmatrix} C\theta & S\phi S\theta & C\phi S\theta\\ 0 & C\phi & -S\phi\\ \dots & \dots & \dots \end{bmatrix} \begin{bmatrix} x_{M}'\\ y_{M}'\\ z_{M}' \end{bmatrix}$$

Since the Y and Z axes are inverted for the magnetometer, the above equation changes to $$\begin{bmatrix} x_{M2}\\ y_{M2}\\ z_{M2} \end{bmatrix} = \begin{bmatrix} C\theta & S\phi S\theta & C\phi S\theta\\ 0 & C\phi & -S\phi\\ \dots & \dots & \dots \end{bmatrix} \begin{bmatrix} x_{M}\\ \color{red}{-}y_{M}\\ \color{red}{-}z_{M} \end{bmatrix}$$

The above is significantly different from your equations.

## Sanity check

You have mentioned in the comments that "pitch on y axis". This means that a rotation about pitch should leave the Y component of a vector unchanged (if it was the last operation performed). Equation for yM in the question doesn't seem to satisfy that logic. Of course, This check is only correct assuming a certain sequence of rotations.

## Note

I see that your equations seem almost correct if the sequence of rotations to go from inertial frame to body frame is (Yaw, Roll, Pitch). The negation on Y and Z components before applying the equations are still required. So, see if your output becomes correct if you insert

yM = -yM;
zM = -zM;


just before the transformation.