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:
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[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!!!
Replies
Brett Smith,
See my program for magnetometer calibration on diydrones too: http://diydrones.com/profiles/blogs/advanced-hard-and-soft-iron-mag...
I saw your comment on instractables =)
Thanks for that link SM. I too had the same issue with the old magnetometer. Someone suggested me to try implementing the full tilt-compensation but I was not that familiar with the full tilt procedure. But now it’s clear. I think the issue was with the magnetometer sensor.
My newly bought gem advanced magnetometer is working perfect. It hasn’t caused any trouble so far. Has anyone tried a left-handed coordinate system?
Ongoing discussion on hard- and soft-iron calibration:
http://diydrones.com/forum/topics/magnetometer-soft-and-hard-iron-c...
Did you get anywhere with this? I'm using the same sensor, and having what I think is a similar issue. The difference for me is that I've got the DMP working, so that's how I'm getting my quaternion values. My tilt compensation looks like this:
heading = atan2((mz * sin(phi) - my * cos(phi)), mx * cos(theta) + my * sin(theta) * sin(phi) + mz * sin(theta) * cos(phi));
where m_ is the magnetometer value, accounting for the axis inconsistence with the accel/gyro (so mx is actually the magnetometer y value, and so on)
my problem is that the mag values aren't tilt compensated at all (not just when the board is inverted), and drift wildly when pitch or yaw != 0
Looks like my problem was a faulty sensor.
Also - I'll add it here for posterity. I used the MPU's DMP features to get the quaternion + roll angles, and then the FreeScale application note to perform the tilt compensation. In that case, I had to change the sign on Phi and on mag_z to get everything to line up the way the application note expects it. Without doing that, I'd have the sign on my heading flipping at odd points (i.e. not near zero)
Hello. I think that i have the same problem. I try to implement full tilt-compensation for magnetometer readings because the scheme with roll and pitch angles works pretty well only when your device is not turned upside down.
I tried to use quaternion approach, but it gives exactly same results. May be someone more experienced will give us a comment.
Are you programming your own IMU?
Make sure your sensors axes are all inline. For example, I am using an MPU-9150 which consists of two sensors.
So in my case, I switch the x and y values of the magnetometer, and switch the sign on the z-axis.
Did you try using the rotation matrix I derived in the attached PDF?
It looks similar to a lot of implementations out there...
Apologies for digging up an old thread, but I just started working with the MPU-9150 and I also needed to rotate the magnetometer readings into the accel/gyro frame.
I think switching the x and y values and changing the sign on the z is not the correct way to rotate a vector from the mag frame to the accel/gyro frame. Your rotation matrix looks like this:
But it should be this:
What makes you say that? The signs defined on the accel / gyro vs the mag are the same for x and y, and different for z. You're changing all 3.
Not enough coffee, apparently. My transformation is most certainly wrong.
-
1
-
2
of 2 Next