2
\$\begingroup\$

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:

enter image description here

Can anyone confirm the equation is used correctly?

\$\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, 2020 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, 2020 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, 2020 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, 2020 at 15:50

1 Answer 1

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