Tilt compensation for yaw calculation from magnetometer and accelerometer

Question

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?

Answer 1

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.