2
\$\begingroup\$

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!

\$\endgroup\$
4
  • \$\begingroup\$ 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 ? \$\endgroup\$
    – AJN
    Oct 8 '20 at 12:09
  • \$\begingroup\$ 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. \$\endgroup\$
    – AJN
    Oct 8 '20 at 12:12
  • \$\begingroup\$ 1) roll would be the rotation on the x axis, pitch on y axis and yaw on z axis \$\endgroup\$
    – MariaC
    Oct 8 '20 at 15:42
  • \$\begingroup\$ 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 \$\endgroup\$
    – MariaC
    Oct 8 '20 at 15:50
0
\$\begingroup\$

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.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.