Hello All!

I have searched far and wide for a good tilt-compensation algorithm, and a decent tutorial  to accompany it. I have read application notes and looked at lots of source code, but I never really understood where it was coming from, and none of the algorithms ever quite worked for me.

So I decided to put together my own tutorial showing how to generate a rotation matrix to rotate the magnetometer vector to the x-y plane using an IPython notebook. It all seems right to me, but I am looking for help and criticism (its a work in progress).

Embarrassingly, after all that, my tilt-compensation is still not dialed in. 

I have performed a rough hard-iron offset, no soft-iron. I would be glad if someone could point me in the right direction in that regard.

Here are the planar cross sections of my raw magnetometer data. As you can (hopefully) see, they are off-center but still circular.


 After finding the offsets, my magnetometer values are more centered:

3691119018?profile=originalThis doesn't look terribly ellipsoidal to me, so for the moment, I don't think that soft-iron interference is my biggest source of error.

So I started plotting my yaw from raw, tilt-compensate (supposedly) magnetometer values. No filtering whatsoever. I also plotted pitch & roll (from a small complementary filter). Here it is:


The red line is yaw, blue is pitch, and green is roll.

At the beginning I held yaw and roll constant, and varied the pitch. Clearly, a small drop in pitch led to a large drop and yaw.

And a small rise in pitch led to a small rise in yaw.

A little over halfway through, I held pitch and yaw constant, and varied roll.

A sizeable increase in roll seemed to have no effect on yaw,

But a small decrease in roll seemed to cause a small rise in yaw.

I am tempted to start changing some of the signs around of my trig functions, but even if it works I will not be content.

I don't understand why the rotation matrix I calculated using pythons sympy module did not work.

I am using an MPU-9150. As I understand it, the accelerometer/gyroscope are coaxial. While the AK8975 magnetometer is not.

So I flipped some of the values around to make sure the AK8975 is coaxial with the MPU6050.

I notice that the resulting z-axis points in the opposite direction of the reference frames defined in my ipython notebook.

But switching it just makes the yaw increase with an increase of roll...

Here is my actual code (which should match with the matrix in the attached PDF):

//My Math: Rotation Matrix from body frame to vehicle 1 frame
magComp[0] = (magVector[0]*cosPitch) + (magVector[2]*sinPitch);
magComp[1] = (magVector[0]*sinRoll*sinPitch) + (magVector[1]*cosRoll) magVector[2]*sinRoll*cosPitch);
magComp[2] = (-magVector[0]*cosRoll*sinPitch) + (magVector[1]*sinRoll) + (magVector[2]*cosRoll*cosPitch);

Any ideas gang? I am kinda disappointed because I felt like I really did my homework on this one, but to be honest my linear algebra is not the best. Any ideas would be much appreciated!!!


You need to be a member of diydrones to add comments!

Join diydrones


            • :)


              fwiw, I think i got it working in my project, using the DMP: https://github.com/kolosy/ArduSailor/blob/master/firmware/ahrs.cpp

              Contribute to kolosy/ArduSailor development by creating an account on GitHub.
              • Hey guys, it has been a while since I worked on this.
                I am glad other people are looking at it. I struggled for a long time with magnetometers, and really could not find any decent explanation on why the rotation matrix is constructed this way.

                Were you looking at my code or the attached PDF on the original post?
                I do not have  a lot experience with matrix math or 3d rotations, and it would be awesome if someone could confirm or correct my little derivation. 

                • Your equations [15] and [20] look fine at a quick glance.

                  I have no idea what you're doing in [17], [18], and [19]. 

                  Here's some things that may be helpful (or maybe painfully obvious):

                  V_mag,inertial = M_rotation * V_mag,body

                  Notes: Multiplying a vector by a rotation matrix will rotate the vector from one frame to the other (but repeating will not rotate it back, see below).  No new special rotation matrix is required to do this, just use the one you show as [15].  The actual direction for rotation is dependent on how the rotation matrix is constructed (so this might be inertial ==> body, I don't have time to check right now).

                  To revers the last rotation:

                  V_mag,body = M_rotation^T * V_mag,inertial

                  Once the magnetometer vector has been rotated to the inertial frame you must compensate for local magnetic inclination.  This is very simple if all you want to know is heading; you can just ignore the Z and use the X and Y.


                  A simple multiplication of the magnetometer vector and rotation matrix will rotate the sensor reading to the body frame.  Use the X and Y components to determine heading.

      • Yes, for now i use Mahony quaternion filter to estimate the orientation of my sensor relative to the global coordinate frame. To get tilt-compensated magnetometer readings i extract euler angles from my quaternion and then use them to level the magnetometer readings relative to the plane which is perpendicular to the gravite vector:
        However, there are a problem. This code works when until device turns upside down, after that the magnetometer readings changes the sign and heading is not correct anymore. Your rotation matrices give the same results, you can check it by yourself.

        mag_g(1) = mag(1)*cos(pitch) + mag(2)*sin(pitch)*sin(roll) + mag(3)*sin(pitch)*cos(roll);
        mag_g(2) = mag(2)*cos(roll) - mag(3)*sin(roll);

        heading = atan2(-mag_g(2), mag_g(1));

        • Hmm, my tilt-compensation looks exactly the same:

          //My Math: Rotation Matrix from body frame to vehicle 1 frame
          magComp[0] = (magVector[0]*cosPitch) + (magVector[1]*sinRoll*sinPitch) + (magVector[2]*sinPitch*cosRoll);
          magComp[1] = (magVector[1]*cosRoll) - (magVector[2]*sinRoll);
          magComp[2] = -(magVector[0]*sinPitch) + (magVector[1]*sinRoll*sinPitch) + (magVector[2]* cosRoll*cosPitch);

          However, I still get the same behavior as above.

          For your quaternion filter, I assume a positive pitch is a nose up and a positive roll brings the right wingtip up????

          • Yes, if you are interested. I discuss this problem here.

            I think that it is tricky question, but can not figure out what is wrong...

This reply was deleted.


Gremsy liked Gremsy's profile
Mar 12
DIY Robocars via Twitter
RT @chr1sa: Donkeycar 4.4 released with tons of new features, including path learning (useful with GPS outdoors), better Web and Lidar supp…
Nov 27, 2022
DIY Robocars via Twitter
RT @NXP: We are already biting our nails in anticipation of the #NXPCupEMEA challenge! 😉 Did you know there are great cash prizes to be won…
Nov 24, 2022
DIY Robocars via Twitter
RT @gclue_akira: レースまであと3日。今回のコースは激ムズかも。あと一歩 #jetracer https://t.co/GKcEjImQ3t
Nov 24, 2022
DIY Robocars via Twitter
UC Berkeley's DIY robocar program https://roar.berkeley.edu/
Nov 24, 2022
DIY Robocars via Twitter
RT @chr1sa: The next @DIYRobocars autonomous car race at @circuitlaunch will be on Sat, Dec 10. Thrills, spills and a Brazilian BBQ. Fun…
Nov 24, 2022
DIY Robocars via Twitter
RT @arthiak_tc: Donkey car platform ... Still training uses behavioral cloning #TCXpo #diyrobocar @OttawaAVGroup https://t.co/PHBYwlFlnE
Nov 20, 2022
DIY Robocars via Twitter
RT @emurmur77: Points for style. @donkeycar racing in @diyrobocars at @UCSDJacobs thanks @chr1sa for taking the video. https://t.co/Y2hMyj1…
Nov 20, 2022
DIY Robocars via Twitter
RT @SmallpixelCar: Going to @diyrobocars race at @UCSDJacobs https://t.co/Rrf9vDJ8TJ
Nov 8, 2022
DIY Robocars via Twitter
RT @SmallpixelCar: Race @diyrobocars at @UCSDJacobs thanks @chr1sa for taking the video. https://t.co/kK686Hb9Ej
Nov 8, 2022
DIY Robocars via Twitter
RT @PiWarsRobotics: Presenting: the Hacky Racers Robotic Racing Series in collaboration with #PiWars. Find out more and register your inter…
Oct 23, 2022
DIY Robocars via Twitter
RT @Hacky_Racers: There will be three classes at this event: A4, A2, and Hacky Racer! A4 and A2 are based around UK paper sizing and existi…
Oct 23, 2022
DIY Robocars via Twitter
Oct 23, 2022
DIY Robocars via Twitter
Oct 19, 2022
DIY Robocars via Twitter
Oct 18, 2022
DIY Robocars via Twitter
RT @NeaveEng: Calling all UK based folks interested in @diyrobocars, @f1tenth, @donkey_car, and similar robot racing competitions! @hacky_r…
Oct 13, 2022