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:
This 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 = (magVector*cosPitch) + (magVector*sinPitch);
magComp = (magVector*sinRoll*sinPitch) + (magVector*cosRoll) magVector*sinRoll*cosPitch);
magComp = (-magVector*cosRoll*sinPitch) + (magVector*sinRoll) + (magVector*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!!!