19

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

Rick2047
  • 739
  • 8
  • 13
  • no answer yet !! –  Oct 17 '11 at 13:33
  • 4
    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! – Martin Thompson Oct 17 '11 at 14:57
  • 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? – Jason R Oct 18 '11 at 13:14
  • I think this might be better at [electronics.se]. I'm migrating it there to see if you get better responses. –  Oct 18 '11 at 15:09
  • Thanks '**yoda**' and '**Jason R**' .. however I am seeing if I can put something here for (as in) numerical examples. Hey 'Yoda' is an Intelligent Agent. wow. – Rick2047 Oct 19 '11 at 06:12
  • Typing just to attract ppl here. – Rick2047 Oct 20 '11 at 07:41
  • I'm not exactly sure what you're asking for here? Are you saying that you have the (x,y,z) field vector, and you want to convert that to (heading,elevation) ? – Rocketmagnet Oct 21 '11 at 15:17
  • 1
    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? – vicatcu Oct 21 '11 at 19:59
  • @Rocketmagnet: yes. – Rick2047 Nov 01 '11 at 10:26
  • @vicatcu: To compute navigationally useful measures for sure. You mean whether it shows the same value at different time at same place, right? Yes those are repeatable at any time. – Rick2047 Nov 01 '11 at 10:32
  • Typing just to attract ppl here. :) – Rick2047 Nov 09 '11 at 14:02
  • 1
    @Rahul - I'm surprised that this isn't getting more attention! – Kevin Vermeer Nov 11 '11 at 16:00
  • @KevinVermeer: I agree, so I'm adding a bounty to this question. – davidcary Mar 18 '12 at 05:44
  • Have you considered the use of quaternions to avoid the "gimbal lock"? – clabacchio Mar 19 '12 at 09:49
  • I saw it now. I'll be back .. m shifting to another place. – Rick2047 Mar 23 '12 at 06:15
  • I am disappointed that no one was able to give a better answer to this question. When [Robotics](http://area51.stackexchange.com/proposals/40020/robotics) goes public, perhaps moving this question there would get a better response? – davidcary Nov 06 '12 at 16:10
  • Yeah it should be migrated to the place where, ppl are closely related to **flying** robotics or similar areas. I paused this project. because I think the magnetometer is faulty or not correctly documented or not for the purpose of evaluating Yaw, pitch n roll. What else wud I say? :\ – Rick2047 Nov 06 '12 at 16:47
  • http://electronics.stackexchange.com/questions/21007/magnetometer-dynamic-calibration –  Apr 28 '16 at 04:50
  • http://electronics.stackexchange.com/questions/21007/magnetometer-dynamic-calibration –  Apr 28 '16 at 04:51
  • by the way what is "heading w.r.t"? – pazel1374 Mar 14 '18 at 00:38

2 Answers2

8

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.

davidcary
  • 17,426
  • 11
  • 66
  • 115
  • I saw it now .. I'll b back. – Rick2047 Nov 01 '11 at 09:44
  • \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. – Rick2047 Nov 01 '11 at 10:18
  • \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. – Rick2047 Nov 01 '11 at 10:19
  • 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. :) – Rick2047 Nov 01 '11 at 10:24
  • \1/ I'll work for [ Roll, pitch, yaw and ( Mx, My, Mz ) (heading, declination) ] rather. – Rick2047 Nov 01 '11 at 10:40
  • The attached CSV files are nice, but they only tell me 3 values at each instant in time (pitch, roll, yaw). It would be nice if you could attach a CSV file with all 7 values at each instant: pitch, roll, yaw, Mx, My, Mz, and the mag_heading calculated from those 6 inputs. – davidcary Nov 02 '11 at 01:16
  • k Big work. I have to repeat this work all over again. I'll put those here once completed. or if you have any shortcut. – Rick2047 Nov 02 '11 at 07:56
  • I observed that the Declination is coming 'Not a number' (may be infinity) here. Trying to overcome otherwise will remove declination and put here. – Rick2047 Nov 03 '11 at 12:26
  • I put it in format: RPY[, 38.074, 8.5236,-100.80,] MAG[,-36.000, 32.000,-132.00,] ACC[,-138.00, 216.00, 392.00,] GYR[,-253.00,-203.00,-1580.0,MH:,-90.434 – Rick2047 Nov 03 '11 at 14:00
  • Excellent! That file with all 13 values per timestep tells us exactly what's going on. I have to go on a trip soon -- perhaps someone else, now that all the data is available, can figure this out while I'm offline. – davidcary Nov 04 '11 at 14:14
  • Anyone apart from @davidcary. – Rick2047 Nov 07 '11 at 05:46
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.

geometrikal
  • 4,921
  • 2
  • 23
  • 41