The above is a plot of the 3 components of estimated magnetometer offsets computed during spin tests at 78 RPM of an improved method for estimating and removing magnetometer offsets. There is a report available with theory and implementation. The method will work equally well for fixed wing aircraft, multicopters, and helicopters. It makes no assumptions about the dynamics of the airframe.
The idea for the new method came to me while I was recently working on a method to detect and compensate for magnetometer misalignment errors. I realized the method that I had been using for offset compensation was sensitive to misalignment between the magnetometer axes and the gyro axes, so I thought about ways to compute offsets that would ignore misalignment. I found a good way to do it that turned out to be more accurate and easier to implement than the method that I was using.
I also figured out a slick way to detect and adjust for magnetometer misalignment, I will be publishing a report in a few days on that subject. The method will compensate in flight for any amount of magnetometer misalignment, including a 180 degree yaw mounting error. In other words, the algorithm can figure out that you mounted your magnetometer backwards and apply a rotation matrix to the magnetometer vectors to put them into the correct reference frame. Stay tuned....
[Here is the technique for doing inflight magnetometer alignment. - WJP]
Best regards,
Bill Premerlani
Comments
Hello William,
Thank's for this post, I know it's 2018 but needs to ask you about how you did about the matrix of alignment, I can't find the mathematic formula.
So if you see my message please help !! :)
Spicy,
A couple of more comments. The gain that I use in my implementation is actually 1/16. That is not obvious from the code, where it looks like it is 1/4. The other factor of 1/4 comes from a factor of 1/2 where I subtract out the offset, I first divide by 2, and I am mixing Q2.14 and Q1.15 integer formats in the computation, which yields another factor of 1/2.
When you combine the effect of the factor of 1/16 and the effect of truncation in integer arithmetic, which will stop when the offset is less than 8, an integer implementation is not as subject to the effect that you describe.
Your setting a threshold of 5 or so is consistent with the built in threshold of 8 in my implementation.
Best regards,
Bill
Spicy,
Yes, magFieldBodyPrevious is set to the previous value of udb_magFieldBody.
udb_magFieldBody is the measured magnetic field, minus the offsets.
MatrixPilot uses integer arithmetic in most places. The +2 and >>2 perform a rounding operation and a divide by 4.
Since you are using floating point, you do not need the +2 for rounding, but you do need to do the divide by 4.
The divide by 4 adjusts the loop gain, and reduces the effect that you report. If you were not doing the divide by 4, try it and see if things improve. You also might try other values of gain other than 1/4.
Lowering the gain improves accuracy, suppresses the tendency for the offset estimates to drift in a hover, but takes longer to converge.
That said, there is a tendency for the effect you report during a hover, so the technique you mentioned is a good idea in any case.
Best regards,
Bill
Fabrizio,
A couple more comments regarding the math of equations 6 and 8...
Another way to see the right hand side of your last equation must be equal to 3 - trace(R21) for b0 perpendicular is that if there is no rotation, R21 is the identity matrix, and trace(R21) = 3.
Regarding parallel and perpendicular components of b0....
We can express b0 as the sum of two vectors, one of them parallel to the axis of rotation of R21, the other perpendicular.
For the right hand side of your last equation, R21 times the parallel component of b0 is b0, so the total multiplier of b0 parallel is zero. For the perpendicular component, the sum of ( 2I - R21 - R21T) times b0 perpendicular simply scales b0 perpendicular by 2*(1 - cos(alpha)).
Best regards,
Bill
Fabrizio,
By the way, the right hand side is (2 - R21T - R21)b0.
Best regards,
Bill
Fabrizio,
Your math is correct up to your last step, but we have to finish it, then you will see where the 3 comes from.
We are interested in the right hand side. Decompose bo into components that are parallel, and perpendicular, to the axis of rotation of R21. R21 times bo|| and R21T times bo|| produces bo||, so right hand side is zero for bo||.
For bo perpendicular, R21 and R21T rotate bo in the perpendicular plane. When you add up the 3 terms on the right hand side, you get 2*(1-cos(alpha))*bo, where alpha is the rotation angle. You can show that for a rotation matrix, cos(alpha) = 1/2*(trace(R21) - 1), so 2*(1-cos(alpha)) = 2*( 1 - 1/2*(trace(R21) - 1)) = 3 - trace(R21).
Best regards,
Bill
Hi Bill, thanks for the nice replies :)
First of all it is kind of honour for me to talk directly to you :).
I was looking at the matrix equations because I like to understand every single passage of what I am reading.
By the way I tried to go on with the math. You can read what I wrote in the image attached. I have some differences from your results. I don´t know how did you define the b0_, which is different from b0 I guess and I have a 2 instead of a 3. Am I doing something wrong? Thanks for your help.
Hi Fabrizio,
I still have not found my notes, but I see how you can show equation 8 is a solution to equation 6. I will get you started:
Multiply equation 6 by R2transpose. Call that equation A.
Multiply equation 6 by R1transpose. Call that equation B.
Subract equation B from equation A. The left hand side of the result is the numerator of equation 8.
I leave it for you to finish...
;-)
Best regards,
Bill
Hi Fabrizio,
Actually, as I mentioned at the beginning of this posting, I recommend using the method described in my 2011 report, instead of the one in my 2010 report. It is simpler to implement, and the math is easier to understand.
Regarding the steps involved in going from equation 6 to equation 8 in my 2010 report, I am so sorry to say I cannot find my handwritten math transformations for that. (My desk is covered with piles of my handwritten solutions to various math problems related to UAVs.) I do recall that it took several pages to go from equation 6 to equation 8, and I do recall that equation 8 is an exact solution to equation 6, but sorry to say I do not recall the steps any longer.
I don't think I threw my notes for my 2010 report out. Next time I sort through the piles of papers on my desk, I will look for them, and publish the solution. In the meantime, I suggest you forget about my 2010 report and read my 2011 report.
Best regards,
Bill
Hi William,
your work is great :)
I was reading your small report of 2010
https://www.google.com/url?sa=t&rct=j&q=&esrc=s&sou...
Premerlani Report 2010
and I didn´t understand how do you arrive from equation 6 to equation 8. Can you please explain it to me?
Thanks a lot,
Fabrizio.
-
1
-
2
of 2 Next