Magnetometer dynamic calibration

Question

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.

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

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

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

Angles in degrees in a file The YAW characteristics on Yaw with 45 degrees rolled.

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. csv

csv

csv

---

Answer 1

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.

Answer 2

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.