Electronic – Tilt compensation for yaw calculation from magnetometer and accelerometer

accelerometercompensationmagnetometer

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

I calculate the pitch and roll angle through 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 the tilt compensation, so no matter the position of the x and y axis the yaw angle can be calculated.

I can assure the roll and pitch angle 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 axis are not exactly the same.
The following image show both axis.

enter image description here

Can anyone confirm if the equation is been used correctly?

Thanks!

Best Answer

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.