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 !! :)
Bill,
Thanks for the thorough reply. So it seems like I had it right all along, I just wasn't aware of the various scaling factors at play. I'll play with it some more, I think doing some simple things like keeping a back buffer of fields and thresholding will have it working well.
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
I just came across this. Interesting stuff, I just had to try it. I collected magneto data from a flight sim and used matlab to test the algo. It works, but has a problem.
For testing purposes I added arbitrary offsets to the fields before processing, in the hopes that the algo would converge to these known values. It does indeed do this, but only seems to work when the vehicle is experiencing large movements. During times with considerable attitude change the algo does converge to my offsets. However during times of only slight movement (i.e. hover) the algo just converges to the field measurement at that time. Put another way it thinks that the reported field measurement IS the offset... not good. I used a threshold on the magnitude of vectorBuffer, only running when this is greater than 5 (or so) seems to produce reasonable results.
I'm hoping there is a simple explanation. Is magFieldBodyPrevious set to udb_magFieldBody or udb_magFieldBody-udb_magOffset? What is the purpose of the +2 and >>2 in the final lines of the algo? I'm guessing this is an implementation specific thing. Matlab uses all floats so I just ignored this, maybe I need to do some things fixed point?
Any help would be much appreciated!
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
-
1
-
2
of 2 Next