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
:)
fwiw, I think i got it working in my project, using the DMP: https://github.com/kolosy/ArduSailor/blob/master/firmware/ahrs.cpp
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.
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...
-
1
-
2
of 2 Next