Quaternion IMU Drift Compensation: Magnetometer

  One of the most important sensors on a vehicle that needs to navigate between physical locations is a magnetometer.  Like a person who uses a compass for land navigation, the magnetometer provides a static reference toward magnetic north allowing for movement in a desired direction.  The information from this sensor comes in the the form of 3 orthogonal values that give a representation of the magnetic field surrounding it.  In the following post I will explain its benefits, its challenges and how I use it in my complementary filter.

Basic Function

  As I noted in my introduction, the magnetometer provides a static reference towards magnetic north.  This allows us to cancel the gyro drift that accumulates in the yaw direction.  In addition to correcting for drift, it also allows us to be able to navigate with a GPS once we add that to the system as it provides a reference direction to allow us to determine which way to move if the GPS indicates we are out of position.

Challenges

  Below are some of the challenges of dealing with an magnetometer on a flying vehicle.  I will give a short description of each and how they effect the usability of the sensor.  There are a number of good articles on each of these items already available on the web, but this is a quick overview to help present the issues involved.

Inclination

  This is the angle that the magnetic field in your area points into or out of the ground.  In most places on the earth, this component of the magnetic field is much stronger than the component that points north.  In my area (Minnesota, USA), this points into the ground at over an 70 degree angle.  This vertical component of the magnetic field must be eliminated before computing the yaw error to prevent this inclination from producing erroneous corrections.

Declination

  This is the angle difference between true north and magnetic north.  If all that is desired is countering yaw  drift, then it can be ignored.  Even though the vector does not point toward true north, it will consistently point in a single direction allowing gyro drift to be eliminated.

  If it is important to know where true north is, then it must be manually compensate for.  I go to this link to determine the declination close to where I will be flying.  NOAA Magnetic Declination 

Hard Iron Effects

  This is an distortion of the magnetic field that results from the the magnetometer itself or other objects that are stationary to the sensor even when it is moving.  This may include items that are magnetized.  This usually looks like an offset in the results where pointing one direction gives a different magnitude result than pointing 180 degrees offset.  This can be visualized as an offset of the data where rotating the sensor in a circle results in a circle of data that has a center point offset from the origin.  This can be eliminated by adding bias offsets to the sensor results that brings the data to origin.  This is critical in almost all magnetometers to get data that is usable. 

Soft Iron Effects

  This is an distortion of the magnetic field around the sensor that varies with orientation.  It can be caused by objects very near the sensor that bend the magnetic field, such as ferrous materials.  When viewing data from a magnetometer that is effected by soft iron effects, it will often make an ellipsoidal shape when viewed in 3D.  If this effect is minor and the application is non-critical, it may be ignored.  In cases where higher accuracy is desired, a large number of points can be gathered and used to create a 3x3 rotation matrix that will transform the ellipsoidal shape to a sphere.  In many cases this can be ignored if perfect accuracy is not needed and the distortion effect is minor.  

Varying Effects

  Items on an aircraft that generate magnetic fields can be problematic for the sensor.  Wires from the batteries will create magnetic fields that vary with current draw.  Electronic speed controls emit a large amount of electromagnetic noise.  Motors are basically a magnetometers nightmare with very powerful permanent magnets and coils that generate fields that vary with motor load and wires that generate fields which also vary with load and RPM.  Electronic noise generated by one device and travel in the power wires that supply the sensor and effect its readings.  These items can be addressed with a number of measures:

1. Physical separation --  Keep the magnetometer away from magnets, high current wires, and ESCs.  The effect of these sources drops by the inverse square of the distance.  This is why it is common to mount the magnetometer on the GPS board and put them on a post above the flying machine (or in the case of the DJI Phantom and 3DR Sole, in the one of the legs far below the drone).  Moving the sensor that is 1" away from a noise source to 4" away reduces the effect of the noise by roughly 16x.
2. Add shielding to noisy devices and wires.
3. Adjust magnetometer data based on current draw to motors.  
4. Make the drift compensation from the magnetometer work slowly such that temporary effects produce a minimal amount of error but over a long period of time the average magnetometer vector is acceptable accurate.  This only works if the sources of error are truly temporary.

  With the above description of the common challenges encountered when using a magnetometer,  we deal with some of them in this implementation and some we will ignore for now.

Implementation

  With this complementary filter we will build off of the structure of the previous Gyro+Accel filter and add in the magnetometer.  Now that we are adding the magnetometer this complimentary filter our sensors have transitioned from pure inertial to what is often referred to as an MARG (magnetic, angular rate, gravity) sensor array.  We will continue to compute the yaw and pitch drift compensation in a similar fashion to as before but now we will also use the magnetometer data to counter yaw drift.  One benefit of this algorithm compared to some other low processing power complimentary filters is that the yaw compensation can be done in any attitude.  The body does not need to be nearly perpendicular to gravity to gain accurate results.

  Here is a depiction of the data flow and operations that are performed by this filter:

  I'm  going to focus on the operations relating to the magnetometer.  Those interested in more detail on the gyro and accel operations should read my last post here.  Having said that, many of the same concepts are being reused.

  Here is how the magnetometer data is processed to give us the correction for yaw drift.

1. Data is read from the magnetometer.  This data should have the hard iron compensation applied in the library.  The magnetometer library I wrote corrects for hard iron, but not soft iron (in many cases this is good enough).  Adjusting for soft iron should be done at this point, if it's going to be done.  The units are not important as we will be normalizing the vector later on.
2. Translate the data from the body frame to the world frame using the current attitude estimate.  This is essentially what is often referred to as tilt compensation.  This is critical to the following operations.
3. Next inclination is removed.  This is done by setting the Z component of the data (now in the world frame) to zero.
4. The vector is now normalized to form an unit vector.
5. A vector that I have labeled above as "corrected north" is a constant.  It is a vector that points slight away from true north by the difference between magnetic north and true north in the area of the world that you are flying.  By using a skewed vector, we correct for the difference between magnetic and true north and solve the declination problem.
6. We then take the cross product of the the corrected north and our magnetometer reading.  If both of them are exactly the same, then the result is zero correction (no error).  As they diverge, the magnitude of the result increases, to correct for the error.  For small deviations, the output units are radians.  Although the results from large errors are not truly radians, the output produces a result that will insure convergence.
7. At this point the yaw correction and Pitch/Roll correction can be summed, rotated to the body frame using the attitude estimate and incorporated into the next gyro reading to correct the observed error.  Since rotating the vectors is fairly expensive (~300us on an Arduino), combining the two corrections before rotation and application saves computation time.
     This will only correct a small fraction of the error detected.  In my code I integrate gyro data at 400Hz so the error correction is effectively divided by 400.  By doing this, the effect that noise from the magnetometer has on the attitude estimate is reduced.  Over long periods of time, many small corrections will "push" the attitude estimate towards zero error.  This system is effectively a proportional controller with low gain.  

  It should be noted that if the calibration of the magnetometer data is not completed accurately, this method (and almost any other) will produce poor results.  Magnetometers can be noisy and produce bad data without proper care.  A large number of issues that consumers have with off the shelf drones flying poorly or flying away is often related to bad calibration of the magnetometer.

  In my next post I will be reviewing the code I use to accomplish the above operations.  I am in the process of making this into a library that should make adding this complimentary filter to most projects easier and cleaner.  It will also allow higher power micro controllers to use multiple IMU objects simultaneously with very little work.

  Please let me know if you have any questions relating to any of the above material.  I will be trying to release post more frequently, but this is an hobby and some times life gets in the way.

Phillip