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 =)
Ongoing discussion on hard- and soft-iron calibration:
http://diydrones.com/forum/topics/magnetometer-soft-and-hard-iron-c...
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:
Not enough coffee, apparently. My transformation is most certainly wrong.
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.
TL;DR
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.
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????