From a3d3dd86cf340786f6c168f8267f72e52c6b696c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 27 Mar 2012 15:16:00 +1100
Subject: [PATCH] Compass: implement Bills new offset nulling algorithm

this seems to work much better than the old algorithm, converging
faster and more accurately. Even better, it has no linkage to DCM, so
no possibility of nasty feedback effects
---
 libraries/AP_Compass/Compass.cpp | 66 ++++++++++++++++++++++----------
 libraries/AP_Compass/Compass.h   |  9 ++---
 2 files changed, 48 insertions(+), 27 deletions(-)

diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp
index ad84d9d03a..eb6fa7441b 100644
--- a/libraries/AP_Compass/Compass.cpp
+++ b/libraries/AP_Compass/Compass.cpp
@@ -182,38 +182,62 @@ Compass::calculate(const Matrix3f &dcm_matrix)
 #endif
 }
 
+
+/*
+  this offset nulling algorithm is based on a paper from
+   Bill Premerlani
+
+  http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
+ */
 void
-Compass::null_offsets(const Matrix3f &dcm_matrix)
+Compass::null_offsets(void)
 {
     if (_null_enable == false || _learn == 0) {
         // auto-calibration is disabled
         return;
     }
 
-    // Update our estimate of the offsets in the magnetometer
-    Vector3f    calc;
-    Matrix3f    dcm_new_from_last;
-    float       weight;
-
     Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);
-    
-    if(_null_init_done) {
-        dcm_new_from_last = dcm_matrix.transposed() * _last_dcm_matrix;      // Note 11/20/2010: transpose() is not working, transposed() is.
 
-        weight = 3.0 - fabs(dcm_new_from_last.a.x) - fabs(dcm_new_from_last.b.y) - fabs(dcm_new_from_last.c.z);
-        if (weight > .001) {
-            calc = mag_body_new + _mag_body_last;                // Eq 11 from Bill P's paper
-            calc -= dcm_new_from_last * _mag_body_last;
-            calc -= dcm_new_from_last.transposed() * mag_body_new;
-            if(weight > 0.5) weight = 0.5;
-            calc = calc * (weight);
-            _offset.set(_offset.get() - calc);
-        }
-    } else {
+    // 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;
+
+    if (!_null_init_done) {
+        // first time through
         _null_init_done = true;
+        _mag_body_last = mag_body_new;
+        return;
     }
-    _mag_body_last = mag_body_new - calc;
-    _last_dcm_matrix = dcm_matrix;
+
+    Vector3f delta, diff;
+    float diff_length, delta_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
+        return;
+    }
+
+    // equation 6 of Bills paper
+    delta = diff * (mag_body_new.length() - _mag_body_last.length()) / diff_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;
+    }
+
+    // set the new offsets
+    _offset.set(_offset.get() - (delta * gain));
+
+    // remember the last mag vector
+    _mag_body_last = mag_body_new;
 }
 
 
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index 770a520d60..9ebc4ffeba 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/libraries/AP_Compass/Compass.h
@@ -97,11 +97,9 @@ public:
 	///
 	void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
 
-	/// Perform automatic offset updates using the results of the DCM matrix.
+	/// Perform automatic offset updates
 	///
-	/// @param  dcm_matrix          The DCM result matrix.
-	///
-	void null_offsets(const Matrix3f &dcm_matrix);
+	void null_offsets(void);
 
 
 	/// Enable/Start automatic offset updates 
@@ -134,7 +132,6 @@ protected:
 
 	bool                _null_enable;        	///< enabled flag for offset nulling
 	bool                _null_init_done;        ///< first-time-around flag used by offset nulling
-	Matrix3f            _last_dcm_matrix;       ///< previous DCM matrix used by offset nulling
-	Vector3f            _mag_body_last;         ///< ?? used by offset nulling
+	Vector3f            _mag_body_last;         ///< used by offset correction
 };
 #endif