Team,
The above picture shows an interesting side effect of magnetometer misalignment: complete reversal of the measured horizontal component of the earth's magnetic field.
The effect was discovered by Peter Hollands during his analysis of the data from one of Ric Kuebler's flights. Ric was using the UAV DevBoard (UDB) with MatrixPilot autopilot software. The airplane icons in the picture indicate the estimated orientation of the plane. The blue arrows indicate the estimated direction of the horizontal component of the measured magnetic field, in the earth frame of reference.
The magnetometer is mounted separately from the UDB. It turns out that there was a small roll misalignment of the magnetometer in Ric's setup. Ric aligned his magnetometer more carefully, and the flipping disappeared on subsequent flights. But there was a nagging issue.
Because of the large vertical component of the earth's magnetic field in many parts of the world, magnetometer-based yaw information is particularly vulnerable to misalignment. In my neighborhood, the vertical magnetic field is three times as large as the horizontal field. As a result, a little bit of mechanical misalignment of the magnetometer, and certain attitudes of the aircraft can cause complete flipping of the measured magnetic field.
It is not all that easy to align a separately mounted magnetometer. Even a 5 degree misalignment is too much. I wondered if there might be a way to solve the problem with software....
While I was analyzing the data from Ric's flight, on a hunch, I plotted the measured horizontal magnetic field against the heading:
The plot told me two things. First, it told me that there was a misalignment issue. But it also told me how much the misalignment was. If that could be determined from the flight data from a post-flight analysis, it should be possible to figure it out in flight as well. So I set out to find a way to determine magnetometer misalignment in flight, and automatically compensate for it. The theory, implementation, and testing are reported here.
The method works much better than I originally thought it might. It will automatically and exactly compensate for any amount of roll, pitch, and yaw misalignment between a magnetometer and its partner IMU, including 180 degrees.
Best regards,
Bill Premerlani
Comments
Bill, could you repost your original document on a location that is not google code?
Thanks,
Phillip
Hi Alec,
You are correct, I am assuming the shortest rotation. I am using the same idea that is used in the PI feedback controller for the gyro drift computation in the DCM algorithm: the misalignment rotation vector for a pair of vectors is perpendicular to the plane that they lie in.
The matrix A itself is unique. The method that I describe computes A iteratively via a sequence of small rotations of the identity matrix. The process converges and stops with the correct value of A when the magnetic field in the earth frame converges to a constant value.
Best regards,
Bill
Sorry - one rotation for each angle theta and 2pi-theta and two possible rotations for every angle in between.
Bill,
I think your equation 11 is an insufficient constraint on A to define it.
For vectors a, b of equal length that subtend theta there is a rotation from a to b for every angle {theta, 2pi-theta}. You're assuming the shortest rotation (theta) is correct, am I right?
Hi Paolo,
It sounds like you are interested in vector and matrix representations of rotations. I suggest your next step is to read up on rotation matrices. The wiki entry on the subject is pretty good. Read it a couple of times, paying special attention to the rigorous conversions among matrix, quaternion, and axis-angle representations.
You can in general move freely between matrix, quaternion, and axis-angle representations. The vex operation converts from matrix to angle-axis, assuming the angle is less than 90 degrees. Quaternions and axis-angle representations can easily be converted to matrices, using the equations in the wiki article. In my implementation of the magnetometer alignment, I sometimes use matrices (such as when realigning the mag vector), and sometimes vectors.
The conversions from matrix to quaternion and axis-angle representations look complicated, but you can simplify them if you make assumptions about signs. In our case, we can use the simpler equations because the algorithm will converge until the residual misalignment is zero. We really do not need to compute the exact misalignment at each step, we just have to keep moving closer to the correct answer, and to be able to stop when we get there. The key is to always apply the latest estimate of the alignment matrix to the mag measurements before computing the residual misalignment.
Although quaternions have 4 components, only 3 of them are independent. The main difference between quaternions and axis-angle is that quaternions are based on the sine and cosine of half the rotation angle, while axis angle representation is based on sine and cosine of the rotation angle.
Anyway, I will do my best to answer some of your questions.
1. The vex operator computes the following axis-angle parameters from the rotation matrix:
n1*sin(alpha) = ( x32 - x23 ) / 2
n2*sin(alpha) = ( x13 - x31 ) / 2
n3*sin(alpha) = ( x21 - x12 ) / 2
where n1, n2, and n3 are the direction cosines of the rotation axis, and alpha is the rotation angle.
For small angles, the equations become:
n1*alpha = ( x32 - x23 ) / 2
n2*alpha = ( x13 - x31 ) / 2
n3*alpha = ( x21 - x12 ) / 2
2. Refer to the wiki article on how to convert angle-axis or quaternions back onto a rotation matrix. There is more to it than just putting elements into the right places.
3. Regarding the updating the quaternion, I found a way that worked reasonably well was to do an incremental quaternion rotation, ignoring the cross product. Here is a link to the implementation.
Best regards,
Bill
Dear Premerlani,
I'm very interested in your article, which I read very carefully.
But I still have few points not very clear.
(1) First point is what you call vex operation. Giving a rotation matrix R vex operator is as follow:
vex(R) = 1/2 ( R - Rt ) = [ n1 n2 n3 ]
where Rt is the transpose of R. So giving xij the matrix element in row i and column j we would have:
n1 = 1/2 ( x12 - x21 );
n2 = 1/2 ( x13 - x31 );
n3 = 1/2 ( x23 - x32 );
Is this correct?
(2) Within the steps to implement the algorithm (from point 1 to pint 9), in point 5 you compute E as inverse vex operation. But vex operation just obtain x12 - x21, x13 - x31 and x23 - x32 and all zeros on the diagonal. Is this ok to compute E? could you better explain vex operation and its inverse?
(3) finally, in step 9 you tell to compensate align with following equation:
align = align - gain * adjust
But align is a quaternion representation which is a 4 dimensions array, meanwhile adjust is 3 dimensional array, how each single element of align have to be compensated using adjust array?
Pleas could you clarify aforementioned points?
Thanks for any suggestion.
Paolo.
@Mike, which is why the UDB / MatrixPilot will work out the alignment of the magnetometer automatically ! (Thanks Bill P). You don't really have to worry anymore. Just give the magnetometer alignment your best shot, and then MatrixPilot will do the rest. The magnetometer should be mounted right way up and in the same level position as the main level flight of the plane. However you can orientate it into 4 positions, and give the magnetometer options file a hint as to what you want. The UDB will always align the mag, but the more you mis-align, the longer it takes to figure out what you've done (as you turn the plane around).
In the case of planes, right now I am considering fixing the magnetometer underneath the APM, where it has only some foam between it and the earth most of the time, and where it is four inches or more removed from the stock magnets (if I keep them) and also farther away from the ESC and motor, the xbee and video transmitter, and farther away from the servos. But how to ensure it is aligned, that is always a challenge.
LOL. Actually, this is a major reason why I am so very interested in the magnetometer calibration routines. I do not like to mount the magnetometer on the APM, in part because I use the i2c bus, and also because I wanted the freedom to move it farther away from an ESC (in the case of one of my smaller quads) and center it closer to the XY sensors, and I also foresaw that I might want to move it farther from the magnets on my planes (if I leave them in place... jury is still out there....
But once the magnetometer is mounted separate, alignment becomes a more serious issue.
Pete,
Thanks for the response. That's what I thought. Just wanted to make sure. Real time calibration of the mag can definitely be useful. People always find ways to mess up a mag calibration.
Customer: "Look I installed a new hatch closure mechanism."
Me: "Are those rare earth magnets right next to the magnetometer?"
Customer: "Yup...is that a problem?"
Me: Scratching head.
-
1
-
2
-
3
-
4
of 4 Next