Compass: implement noise resistant varient of offset learning

This adds a large amount of noise robustness to the compass offset
learning algorithm, at a cost of 120 bytes of memory. The changes are
based on a long discussion with Bill Premerlani.

libraries/AP_Compass/Compass.cpp

@@ -184,10 +184,24 @@ Compass::calculate(const Matrix3f &dcm_matrix)
 
 
 /*
-  this offset nulling algorithm is based on a paper from
-   Bill Premerlani
+  this offset nulling algorithm is inspired by this paper from Bill Premerlani
 
   http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
+
+  The base algorithm works well, but is quite sensitive to
+  noise. After long discussions with Bill, the following changes were
+  made:
+
+    1) we keep a history buffer that effectively divides the mag
+       vectors into a set of N streams. The algorithm is run on the
+       streams separately
+
+    2) within each stream we only calculate a change when the mag
+       vector has changed by a significant amount.
+
+  This gives us the property that we learn quickly if there is no
+  noise, but still learn correctly (and slowly) in the face of lots of
+  noise.
  */
 void
 Compass::null_offsets(void)
@@ -197,47 +211,67 @@ Compass::null_offsets(void)
         return;
     }
 
-    Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);
-
     // this gain is set so we converge on the offsets in about 5
     // minutes with a 10Hz compass
-    const float gain = 0.5;
-    const float max_change = 2.0;
+    const float gain = 0.01;
+    const float max_change = 10.0;
+    const float min_diff = 50.0;
+    Vector3f ofs;
+
+    ofs = _offset.get();
 
     if (!_null_init_done) {
         // first time through
         _null_init_done = true;
-        _mag_body_last = mag_body_new;
+        for (uint8_t i=0; i<_mag_history_size; i++) {
+            // fill the history buffer with the current mag vector,
+            // with the offset removed
+            _mag_history[i] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z);
+        }
+        _mag_history_index = 0;
         return;
     }
 
-    Vector3f delta, diff;
-    float diff_length, delta_length;
+    Vector3f b1, b2, diff;
+    float length;
 
-    diff = mag_body_new - _mag_body_last;
-    diff_length = diff.length();
-    if (diff_length == 0.0) {
-        // the mag vector hasn't changed - we don't get any
-        // information from this
+    // get a past element
+    b1 = Vector3f(_mag_history[_mag_history_index].x,
+                  _mag_history[_mag_history_index].y,
+                  _mag_history[_mag_history_index].z);
+    // the history buffer doesn't have the offsets
+    b1 += ofs;
+
+    // get the current vector
+    b2 = Vector3f(mag_x, mag_y, mag_z);
+
+    // calculate the delta for this sample
+    diff = b2 - b1;
+    length = diff.length();
+    if (length < min_diff) {
+        // the mag vector hasn't changed enough - we don't get
+        // enough information from this vector to use it
+        _mag_history_index = (_mag_history_index + 1) % _mag_history_size;
         return;
     }
 
+    // put the vector in the history
+    _mag_history[_mag_history_index] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z);
+    _mag_history_index = (_mag_history_index + 1) % _mag_history_size;
+
     // equation 6 of Bills paper
-    delta = diff * (mag_body_new.length() - _mag_body_last.length()) / diff_length;
+    diff = diff * (gain * (b2.length() - b1.length()) / length);
 
     // limit the change from any one reading. This is to prevent
     // single crazy readings from throwing off the offsets for a long
     // time
-    delta_length = delta.length();
-    if (delta_length > max_change) {
-        delta *= max_change / delta_length;
+    length = diff.length();
+    if (length > max_change) {
+        diff *= max_change / length;
     }
 
     // set the new offsets
-    _offset.set(_offset.get() - (delta * gain));
-
-    // remember the last mag vector
-    _mag_body_last = mag_body_new;
+    _offset.set(_offset.get() - diff);
 }
 
 

libraries/AP_Compass/Compass.h

@@ -132,6 +132,10 @@ protected:
 
 	bool                _null_enable;        	///< enabled flag for offset nulling
 	bool                _null_init_done;        ///< first-time-around flag used by offset nulling
-	Vector3f            _mag_body_last;         ///< used by offset correction
+
+    ///< used by offset correction
+    static const uint8_t _mag_history_size = 20;
+    uint8_t				_mag_history_index;
+	Vector3i            _mag_history[_mag_history_size];
 };
 #endif