T3

Automatic nulling of magnetometer offsets

[There is now a better method for removing offsets in flight. See this discussion. - WJP]

The picture shows a UAV DevBoard connected to Jordi's magnetometer breakout board.

First, since my discussions tend to get long winded, if you want to "cut to the chase", you might want to read this.

I recently integrated Jordi's HMC5843 magnetometer breakout board with the UAV DevBoard (UDB).

Jordi's board is really great, having the onboard bidirectional level shifters and builtin pullup resistors made it possible to simply connect Jordi's board directly to the I2C pins in the programming connector. Thank you very much, Jordi.

So, heli and quadcopter pilots who are using the UDB would do well to buy Jordi's board, my plan is to integrate it into the heli firmware that is being developed for the UDB. There is now a magnetometer demo and instructions for the UDB.

But that is all besides the point of this discussion....

Along the way to implementing the magnetometer demo, I realized the hardest part was going to be determining the offsets of the magnetometer. The integration of the magnetometer itself into the DCM algorithm was trivial, but what to do about the offsets?

The HMC5843 has a self calibration feature, so you can determine the gains just fine. But the first magnetometer I bought had rather large offsets. So I bought 3 more (1 more from Jordi, and 2 more from Sparkfun). It turned out that all 4 units had some offsets. Even if they did not have offsets, it would still be possible for stray magnetic fields in the aircraft to create offsets.

So, some sort of procedure is needed to null the offsets.

I thought of several manual ways to do that, and I read several postings on the subject....

Then I wondered if there might be some way to use the direction cosine matrix to automatically compute the offsets in flight. And there is....even for dynamically changing offsets, such as from power leads to the motors...

The basic idea is the following...

When the aircraft is rotating, the offset fields rotate with the aircraft, while the earth's magnetic field is stationary. So it should be possible to use the direction cosines to separate the two fields.

The basis of the idea is the same as flipping the magnetometer exactly 180 degrees along one of the axis and averaging the two readings. The result is the offset.

The idea can be extended to rotations other than 180 degrees, even small rotations, by using the direction cosine matrix and some vector algebra.

The resulting theory and implementation is described here, and it has been tested. It works rather well.

It was interesting to watch the computations of the offsets during the tests. A few random turns along a couple of the axes was all that was needed to compute the offsets. So, you can either null the offsets prior to takeoff by rotating your aircraft a bit, or simply let it happen automatically during the first few turns of the flight.

It was also interesting to see what would happen if I deliberately manually set the offsets to the wrong values, they would quickly reset to the correct values after a few turns.

The bottom line is that now both the calibration and the nulling of offsets can be done completely automatically, without any manual operations or entry of measurements. It is possible to extend any DCM-based IMU, such as the ArduIMU, to use this method to simplify the integration of a magnetometer.

Best regards,
Bill

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  • Hi Bill,

    (Yes, replying in a two-year old thread because I have just found your document on the offset nulling.  I had come to pretty much the same equation but my algebra being as bad as it is I didn't arrive at the final formula for the offset integration, so I'm trying to use yours.)

    In the implementation you shift offsetSum to the right, but you shift magFieldEarth to the left, why's that?  I understand the right shift is the implementation of the filter so that rare spikes from the magnetometer don't introduce too much error.  But the left shift will cause the [b21 * b1 + b12 * b2] part to be 4 times too big.  Or am I misreading?

    Many thanks

  • Hi everyone,

     

    I am getting a BUILD FAILED error when trying to build the demo project to program it to the devboard.  I was able to run the roll/pitch/yaw demo fine, so I don't know why I'm getting an error now.  Here is the error message:

    -------------------------------------------------------------------------------------------------------------------------

    Program Memory  [Origin = 0x100, Length = 0x7f00]

    section                    address   length (PC units)   length (bytes) (dec)
    -------                    -------   -----------------   --------------------
    .text                        0x100              0x23d8          0x35c4  (13764)
    .const                      0x24d8               0x2ca           0x42f  (1071)
    .text                       0x27a2               0xa6c           0xfa2  (4002)
    .dinit                      0x320e               0x414           0x61e  (1566)
    .text                       0x3622              0x21e2          0x32d3  (13011)
    .isr                        0x5804                 0x2             0x3  (3)

    Total program memory used (bytes):         0x8289  (33417) 68%


    Data Memory  [Origin = 0x800, Length = 0x800]

    section                    address      alignment gaps    total length  (dec)
    -------                    -------      --------------    -------------------
    .nbss                        0x800                   0           0x102  (258)
    .ndata                       0x902                   0            0x94  (148)
    .nbss                        0x996                   0            0x90  (144)
    .ndata                       0xa26                   0            0x3c  (60)
    .nbss                        0xa62                   0            0x92  (146)
    .ndata                       0xaf4                   0            0x2a  (42)
    .nbss                        0xb1e                   0            0x24  (36)
    .ndata                       0xb42                   0            0x4a  (74)
    .nbss                        0xb8c                   0             0xc  (12)
    .ndata                       0xb98                   0            0x10  (16)
    .nbss                        0xba8                   0             0xc  (12)
    .ndata                       0xbb4                   0             0x2  (2)
    .data                        0xbb6                   0           0x330  (816)
    .dconst                      0xee6                   0            0x2a  (42)
    .bss                         0xf10                   0            0x18  (24)
    .data                        0xf28                   0             0x2  (2)

    Total data memory used (bytes):          0x72a  (1834) 89%


    Dynamic Memory Usage

    region                     address                      maximum length  (dec)
    ------                     -------                      ---------------------
    heap                             0                                   0  (0)
    stack                        0xf2a                                0xd6  (214)

    Maximum dynamic memory (bytes):           0xd6  (214)

    c:/program files/microchip/mplabc30/v3.25/bin/bin/../../lib\libc-coff.a(vsnprintf.o)(.libc.vsnprintf+0x1e):fake: undefined reference to `assert'
    c:/program files/microchip/mplabc30/v3.25/bin/bin/../../lib\libc-coff.a(vsnprintf.o)(.libc.vsnprintf+0x22):fake: undefined reference to `alloc'
    Link step failed.
    ----------------------------------------------------------------------
    Release build of project `C:\Documents and Settings\Administrator\Desktop\UAV\magnetometer\Magnetometer\MagnetometerDemo.mcp' failed.
    Language tool versions: pic30-as.exe v3.25, pic30-gcc.exe v3.25, pic30-ld.exe v3.25, pic30-ar.exe v3.25
    Wed Dec 22 12:54:57 2010
    ----------------------------------------------------------------------
    BUILD FAILED

     

    -------------------------------------------------------------------------------------------------------------------------

     

    I am a beginner, so please bear with me.  If anyone can help, it would be greatly appreciated.

     

    Thanks,

    -Paul

  • Developer
    Hello, I need some help here about the magnetometer. I have a HMC5843 (the red board) from Sparkfun and I would like to reset de magnetometer to its factory default state, because after a setup error, the magnetomer gives me now bad values (I.e.: MagX:-32, MagY:32, MagZ:-32,)...

    Could someone can give me the procedure to reset the HMC5843 to the factory default setup ?
    Thanks for your help,
    Jean-Louis
  • Hi,

    Ingineous Work.

    Could you enlight me where do you apply the declination vector? I don't find it in the algorithm, just on the code displayed.

    Thks in advance.
  • Hi
    Really thank you for this clear help and guidance.

    I've a basic Problem in this Theory...
    How can I have Direction Cosine Matrix ? is it change or it's constant ?
    Now I have sensor output data but I don't know exactly how should I make DCM to follow the equations on this PDF.

    Thank you very much
    Behnam
  • Hi Bill,

    In the board I am working on I would like to use 3-axis magnetometer in addition to the 3-axis accelerometer to compensate for gyro drift and achieve yaw lock. In implementing this algorithm I am following your outline as discussed in “DCMDraft2” and “MagnetometerOffsetNullingRv2”.

    For the part related to accelerometer I follow the DCMDraft2. For the magnetometer I implement the following algorithm:

    1. When the board powers up I measure magnetic vector M; assuming that the board is stable and horizontal this measurement (in the body frame) is assumed to be the estimate of the true magnetic vector in the Earth frame Me-c.

    2. On each subsequent step n I use the transpose of the rotation matrix from the previous step R(T)n-1 and the stored value of the magnetic vector in the Earth frame Me-c to calculate an estimate of the magnetic vector in the body frame M’b-n.

    3. The vector cross product of this estimate M’b-n and the measured value of the magnetic vector in the body frame from the magnetometer Mb-n represents an error, which is being passed to the PI controller to generate an adjustment based upon the magnetometer.

    4. This adjustment is combined with the roll-pitch adjustment from the accelerometer and the rotation vector from gyros to generate the “infinitesimal” rotation matrix, which is being applied to Rn-1 to generate new rotation matrix Rn in accordance with the algorithm from DCMDraft2.

    5. The New rotation matrix Rn is being normalized (again, in accordance with DCMDraft2) – and the firmware is ready to do the next cycle returning to the Step 2 of the algorithm.

    However I see a drift creeping in due to the magnetometer offset – the measured magnetic vector is not originated at {0, 0, 0}, but has some offset from the beginning of the coordinate system. Naturally I would like to eliminate this offset using the algorithm described in your article “MagnetometerOffsetNullingRv2”. And this is where I got stuck!

    If on the n-th step I have an estimate of the offset as Mo-n, how should I proceed calculating the estimate of the rotation error based upon the magnetometer? It looks from equations (10) and (3) in “MagnetometerOffsetNullingRv2” I need to take a cross product of

    Mb-n = MmagnetometerMo-n
    and
    M’b-n = Mo-n + R(T)n-1*Me-c

    Please advise whether my interpretation is correct.

    Thank you,
    --Alex
  • 1. We want to produce another major release of MatrixPilot for the existing UDB before the new board comes out. I am not sure what features will be there, that will be up to the team. Certainly, we will consider your ideas, but I cannot promise what will be in, and what will be out. There are a lot of things on the list, some things will get cut in the interest of a timely release.

    Do tell :) Is the plan short term or longer ? If longer I'll do it, meaning if you have a reasonably set idea of what you want in there and how you want it connected I can start right now - have some experience in the field and would dearly like to see some more pins with such awesome firmware :)
  • Hi Bill,
    I also read the documentation for the HMC5843 but could not find any I2C commands for the S/R straps.

    But if you look at the pins of the sensor you recognize that you have the the SETN pin available to you. This pin with SETP could be used to provide set and reset pulses to the strap with a simple MOSFET circuit.

    I think that the internal S/R driver is there for initialization purposes and then it has no role.

    I will keep on searching to see if I could find anything else.

    Regards
    Dave
  • Hi Bill,

    In the source code I couldn't exactly find where tau is found and what value you have chosen for it. Could you please tell me which line it is in?

    Regards
    Dave
  • Hi Bill,

    I appreciate your help also. You are always inspiring us.


    Can you please tell me how did youknow that the loop gain is (3 - Trace(R21))/tau) and why is the loop gain the inverse of tau?

    Regards
    Dave
This reply was deleted.

Activity