Compute yaw from magnetometer and accelerometer

Question

I use an Arduino and a 9 DOF sensor (gyroscope, accelerometer and magnetometer) and I'm trying to use the pitch, roll and yaw that the sensor gives me to rotate an object in unity.

I managed to correctly compute pitch and roll (the z and x axis in unity) from the accelerometer but it seems that I can't get the Yaw right. By that I mean that when I rotate my sensor pitch or roll it rotates my yaw too, in a weird way.

Code in the arduino to get the heading

void getHeading(void)
{
  heading=180*atan2(Mxyz[0],Mxyz[1])/PI;
  if(heading <0) heading +=360;
}

void getTiltHeading(void)
{
  //float pitch = asin(-Axyz[0]);
  //float roll = asin(Axyz[1]/cos(pitch));
  float pitch = atan( Axyz[0] / sqrt( Axyz[1] * Axyz[1] + Axyz[2] * Axyz[2] ) );
  float roll = atan( Axyz[1] / sqrt(Axyz[0] * Axyz[0] + Axyz[2] * Axyz[2]));

  float xh = Mxyz[0] * cos(pitch) + Mxyz[2] * sin(pitch);
  float yh = Mxyz[0] * sin(roll) * sin(pitch) + Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
  float zh = -Mxyz[0] * cos(roll) * sin(pitch) + Mxyz[1] * sin(roll) + Mxyz[2] * cos(roll) * cos(pitch);
  tiltheading = 180 * atan2(yh, xh)/PI;
  if(yh<0)    tiltheading +=360;
}

Pitch and roll

    float _Pitch  = (float)(180 / Math.PI * Math.Atan( m_ResultX / Math.Sqrt( m_ResultY * m_ResultY + m_ResultZ * m_ResultZ ) ) );
    float _Roll = (float)(180 / Math.PI * Math.Atan(m_ResultY / Math.Sqrt(m_ResultX * m_ResultX + m_ResultZ * m_ResultZ)));
    float _Yaw = (float)(m_TiltHeadingResult);

Don't hesitate to ask for details.

did u figure out why your yaw changes if you pitch/roll the sensor? and how was it addressed? any information would help me too. — ems, Oct 29 '18 at 20:43

BrettFolkins · Accepted Answer · 2016-01-09T18:06:10.940

I don't know how all your sensor's axis are lined up, but it looks like you should level the magnetometer readings by rotating them by -pitch and -roll to get back to the global reference frame, instead of rotating the magnetometer readings by the positive angles.

However, The pitch and roll angles in the code are both relative the horizon, not a set of euler angles that define a rotation between two states; because of this, they do not compose to rotate the magnetometer back into the global xy frame like I assume the equations are trying to do. The code is also only using atan for pitch and roll instead of atan2, so their range is only 180 degrees.

You need to make sure the rotation matrix you multiply your magnetometer reading by is based on the same conventions as the equations you use to get pitch and roll from the accelerometer, and that it cancels the observed rotation.

To do this correctly, I recommend you read into euler angles. You might also try searching around for a good library that handles 9-DOF sensor fusion for you.

I've read a bit about the euler angles but my math isn't that great nor had I time to investigate further but I managed to find a very good 9-DOF sensor fusion. Everything is ok now. — BAmadeusJ, Jan 09 '16 at 18:50

Compute yaw from magnetometer and accelerometer

1 Answers1