19
\$\begingroup\$

I am working on a Magnetometer AK8975 being a part of an IMU. Which seems to be very tricky for me. This chip gives a 3D vector as output describing earth's magnetic field at any place on earth or near it.

I tried two types of heading calculation algorithms: One is simple arctan(-y/x) and another is inclination (pitch) and bank (roll) canceled maths as mentioned in below. Both on inclination and banks give wrong output.

I am able to get the correct heading w.r.t the earth (using simple available open study resources) when it is rotated keeping horizontal w.r.t the ground plan using any of the two algos.

I tried calibration for soft and hard iron errors. I Could plot it in 3D and shows a perfect 3D sphere. Still doesn't work on inclination or declination.

Any pointer will be helpful.

The code and its implementations are as below:

void Compass_Heading()
{
  double MAG_X;
  double MAG_Y;
  double cos_roll;
  double sin_roll;
  double cos_pitch;
  double sin_pitch;

  cos_roll = cos(roll);
  sin_roll = sin(roll);
  cos_pitch = cos(pitch);
  sin_pitch = sin(pitch); 

  //// Tilt compensated Magnetic filed X:
  MAG_X = magnetom_x*cos_pitch + magnetom_y*sin_roll*sin_pitch + magnetom_z*cos_roll*sin_pitch;
  //// Tilt compensated Magnetic filed Y:
  MAG_Y = magnetom_y*cos_roll-magnetom_z*sin_roll;
  //// Magnetic Heading


  MAG_Heading = atan2(-MAG_Y, MAG_X) ;

}

Where magnetom_x, #_y and #_z are components of a 3D vector which actually are RAW values from the Magnetometer. roll and pitch are from a mysterious Kalman-filter output from onboard accelerometer and gyroscope. These three sensors are in ATAVRSBIN1. The roll and pitch are ok till this stage.

Now a simple heading calculation according to journal_of_sensors_renaudin et al_2010c.pdf should have been MAG_Heading = atan2(-magnetom_y, magnetom_x) ; and with compensation as above.

Overall code is simply from OPEN AHRS.


Data in format Roll, Pitch and Yaw. I rotated the device by my hand only. First three have been concentrated on only Roll, Pitch and Yaw respectively. Rest two are first rotated the device around 45 degrees along X (Rolled) then rotated along Magnetometer's local Z. Then same has been repeated with around 45 Degrees rotation along Y (pitched) then rotated along Magnetometer's local Z.

The graphs plotted within the range of -180 to 180 degrees.

Roll Angles in degrees in a file The YAW characteristics on Roll.

Pitch Angles in degrees in a file The YAW characteristics on Pitch.

Yaw Angles in degrees in a file The YAW characteristics on Yaw itself.

Yaw w.r.t 45 degrees inclined (rolled) Angles in degrees in a file The YAW characteristics on Yaw with 45 degrees rolled.

Yaw w.r.t 45 degrees banked (pitched) Angles in degrees in a file The YAW characteristics on Yaw with 45 degrees pitched.

Note: For last 2 pictures: First kept in home position, that is same for all (refer txt files). Then Rolled 45 degrees then using the plane device (with magnetometer) has been rotated along Magnetometer's Z-axis.

Similarly for last image the device has been pitched 45 degrees then along Magnetometer's Z-axis.

I hope these will help solving my issue.


New developments are as follows:

I worked some on the Heading. I got following output. Roll csv

Pitch csv

Yaw csv


\$\endgroup\$
20
  • \$\begingroup\$ no answer yet !! \$\endgroup\$
    – Rahul2047
    Oct 17, 2011 at 13:33
  • 4
    \$\begingroup\$ I think you'll get more response if you show the maths you are attempting to implement and the code you've used to implement it. There's very little for us to go on otherwise other than "it doesn't work, help" - which is how your question reads. Sorry! \$\endgroup\$ Oct 17, 2011 at 14:57
  • \$\begingroup\$ Magnetometer use is a very specialized area that comparatively few people are going to have expertise with. Reading through your question a few times, I'm still not sure exactly what's wrong. You say it gives the "wrong output" but that's pretty vague. Maybe some numerical examples? \$\endgroup\$
    – Jason R
    Oct 18, 2011 at 13:14
  • 1
    \$\begingroup\$ Is this a question about how to interpret the sensor outputs or how to compute navigationally useful measures from the x,y,z vector that the sensor provides? Are your measurements repeatable with another instance of the same sensor? \$\endgroup\$
    – vicatcu
    Oct 21, 2011 at 19:59
  • 1
    \$\begingroup\$ @Rahul - I'm surprised that this isn't getting more attention! \$\endgroup\$ Nov 11, 2011 at 16:00

2 Answers 2

8
+125
\$\begingroup\$

I like your graphs. They clearly show that roll, pitch, and yaw seem to be working. Congratulations! That's already more progress than most people make.

I'm guessing that the code you presented is calculating "the wrong" MAG_Heading value, different from the MAG_Heading value you expected.

It would be a lot easier for us to help you if you gave us: (This is the "describe the symptoms" section of "How To Ask Questions The Smart Way" )

  • the AK8975 magnetometer output values m_x, m_y, and m_z at some single point in time.
  • The pitch and roll values at the same instant
  • the allegedly wrong MAG_Heading output value calculated from those values
  • what you expected the correct MAG_Heading to be

So I'm left to speculate that perhaps you're running into the same sorts of problems I create for myself :-).

  • What angle format do your sin() and cos() and atan2() functions expect? Do you need to do some sort of conversion between the format pitch and roll are stored in to that format? Do you need to convert from that format to what you need MAG_heading? (brads, degrees, or radians? floating-point or fixed-point?)
  • Is there an offset in the raw m_x, m_y, m_z values that needs to be subtracted off?
  • Are all the parts lined up in the way assumed by the code? In particular, is the pitch and roll axis lined up with the magnetometer axis? (Is m_x supposed to point forward, along the roll axis? Is m_y supposed to point to the right, along the pitch axis?)
  • Maybe some sensor value or another -- perhaps m_z -- needs to be negated before feeding into this code?
  • Is maybe this code being interrupted by one interrupt or another that corrupts its internal values? I seem to recall a different project that, after someone put a "divide" in an interrupt routine, every trig function calculation everywhere else in the program would often give the wrong result.
  • Is maybe interrupts firing so often that this code never actually finishes running?

There seems to be other people discussing very similar code elsewhere: http://diydrones.com/forum/topics/heading-from-3d-magnetometer ; http://diydrones.ning.com/profiles/blogs/dcm-imu-theory-first-draft ; http://aeroquad.com/showthread.php?1138-REVOLUTION!!!-New-IMU!!! ; http://www.rcgroups.com/forums/showthread.php?t=1436742&page=6 ; http://aeroquad.com/showthread.php?691-Hold-your-heading-with-HMC5843-Magnetometer ; etc.

\$\endgroup\$
11
  • \$\begingroup\$ I saw it now .. I'll b back. \$\endgroup\$
    – Rick2047
    Nov 1, 2011 at 9:44
  • \$\begingroup\$ \1/ I am creating a csv file for [ Roll, pitch, yaw and ( Mx, My, Mz ) ]. \2/ I expect that the MAG_Heading should not change with change in roll and pitch at least till two successive quadrants. The heading means, if it is heading NE then it should keep pointing NE till it crosses the 90 degrees from horizon on either up or down rotation directions for inclination and should be same in case of banking or combination. \$\endgroup\$
    – Rick2047
    Nov 1, 2011 at 10:18
  • \$\begingroup\$ \3/ All internal calculations have been done in radians and all consine functions expect radians only. For display only the values are copied and converted to degrees. \4/ Floating-point. \5/ All sensors are aligned and also all the possible relevant combinations have been tried for alignment using SENSOR_SIGN[9].\6/ I am doing it in polling so interrupt based all issues will not be here. \$\endgroup\$
    – Rick2047
    Nov 1, 2011 at 10:19
  • \$\begingroup\$ For my point \1/ here for all the graphs I have also attached the corresponding csv files. Or you asked for something else. Thanks for this good response. :) I'll implement "How To Ask Questions The Smart Way" as much as my effort and time permits. :) \$\endgroup\$
    – Rick2047
    Nov 1, 2011 at 10:24
  • \$\begingroup\$ \1/ I'll work for [ Roll, pitch, yaw and ( Mx, My, Mz ) (heading, declination) ] rather. \$\endgroup\$
    – Rick2047
    Nov 1, 2011 at 10:40
2
\$\begingroup\$

The application note for the LSM303 has a useful guide to calibrating a tilt-compensated compass which is applicable to your problem. It is quite detailed, otherwise I would have re-written the calculations here. Note that the accelerometer values are necessary for full pitch, roll, and yaw calculations as a rotation around the axis of the magnetic field lines results in no change in magnetometer values. Likewise for gravity with the accelerometer.

\$\endgroup\$
1
  • \$\begingroup\$ The link shows different sign in the same equation. I'll try all other combinations. \$\endgroup\$
    – Rick2047
    Nov 17, 2011 at 9:46

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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