AP_Compass: new compass learning system

this learns compass offsets using magnetic tables and compass
observations

libraries/AP_Compass/AP_Compass.cpp

@@ -1011,7 +1011,10 @@ Compass::use_for_yaw(void) const
 bool
 Compass::use_for_yaw(uint8_t i) const
 {
-    return _state[i].use_for_yaw;
+    // when we are doing in-flight compass learning the state
+    // estimator must not use the compass. The learning code turns off
+    // inflight learning when it has converged
+    return _state[i].use_for_yaw && _learn.get() != LEARN_INFLIGHT;
 }
 
 void

libraries/AP_Compass/AP_Compass.h

@@ -52,6 +52,8 @@ public:
     Compass(const Compass &other) = delete;
     Compass &operator=(const Compass&) = delete;
 
+    friend class CompassLearn;
+
     /// Initialize the compass device.
     ///
     /// @returns    True if the compass was initialized OK, false if it was not
@@ -182,14 +184,14 @@ public:
     // learn offsets accessor
     bool learn_offsets_enabled() const { return _learn; }
 
-    /// Perform automatic offset updates
-    ///
-    void learn_offsets(void);
-
     /// return true if the compass should be used for yaw calculations
     bool use_for_yaw(uint8_t i) const;
     bool use_for_yaw(void) const;
 
+    void set_use_for_yaw(uint8_t i, bool use) {
+        _state[i].use_for_yaw.set(use);
+    }
+
     /// Sets the local magnetic field declination.
     ///
     /// @param  radians             Local field declination.
@@ -301,7 +303,8 @@ public:
     enum LearnType {
         LEARN_NONE=0,
         LEARN_INTERNAL=1,
-        LEARN_EKF=2
+        LEARN_EKF=2,
+        LEARN_INFLIGHT=3
     };
 
     // return the chosen learning type
@@ -309,6 +312,15 @@ public:
         return (enum LearnType)_learn.get();
     }
 
+    // set the learning type
+    void set_learn_type(enum LearnType type, bool save) {
+        if (save) {
+            _learn.set_and_save((int8_t)type);
+        } else {
+            _learn.set((int8_t)type);
+        }
+    }
+    
     // return maximum allowed compass offsets
     uint16_t get_offsets_max(void) const {
         return (uint16_t)_offset_max.get();

libraries/AP_Compass/Compass_learn.cpp

@@ -1,129 +1,203 @@
 #include <AP_Math/AP_Math.h>
+#include <AP_AHRS/AP_AHRS.h>
 
-#include "AP_Compass.h"
+#include <AP_Compass/AP_Compass.h>
+#include <AP_Declination/AP_Declination.h>
+#include <DataFlash/DataFlash.h>
 
-// don't allow any axis of the offset to go above 2000
+#include "Compass_learn.h"
-#define COMPASS_OFS_LIMIT 2000
+
+#include <stdio.h>
+
+extern const AP_HAL::HAL &hal;
+
+// constructor
+CompassLearn::CompassLearn(AP_AHRS &_ahrs, Compass &_compass) :
+    ahrs(_ahrs),
+    compass(_compass)
+{
+}
 
 /*
- *  this offset learning algorithm is inspired by this paper from Bill Premerlani
+  update when new compass sample available
- *
- *  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
+void CompassLearn::update(void)
-Compass::learn_offsets(void)
 {
-    if (_learn == 0) {
+    if (converged || compass.get_learn_type() != Compass::LEARN_INFLIGHT ||
-        // auto-calibration is disabled
+        !hal.util->get_soft_armed() || ahrs.get_time_flying_ms() < 3000) {
+        // only learn when flying and with enough time to be clear of
+        // the ground
         return;
     }
 
-    // this gain is set so we converge on the offsets in about 5
+    if (!have_earth_field) {
-    // minutes with a 10Hz compass
+        Location loc;
-    const float gain = 0.01f;
+        if (!ahrs.get_position(loc)) {
-    const float max_change = 10.0f;
+            // need to wait till we have a global position
-    const float min_diff = 50.0f;
+            return;
-
-    if (!_null_init_done) {
-        // first time through
-        _null_init_done = true;
-        for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
-            const Vector3f &field = _state[k].field;
-            const Vector3f &ofs = _state[k].offset.get();
-            for (uint8_t i=0; i<_mag_history_size; i++) {
-                // fill the history buffer with the current mag vector,
-                // with the offset removed
-                _state[k].mag_history[i] = Vector3i(roundf(field.x) - ofs.x, 
-                                                    roundf(field.y) - ofs.y, 
-                                                    roundf(field.z) - ofs.z);
-            }
-            _state[k].mag_history_index = 0;
         }
+
+        // setup the expected earth field at this location
+        float declination_deg=0, inclination_deg=0, intensity_gauss=0;
+        AP_Declination::get_mag_field_ef(loc.lat*1.0e-7, loc.lng*1.0e-7, intensity_gauss, declination_deg, inclination_deg);
+
+        // create earth field
+        mag_ef = Vector3f(intensity_gauss*1000, 0.0, 0.0);
+        Matrix3f R;
+
+        R.from_euler(0.0f, -ToRad(inclination_deg), ToRad(declination_deg));
+        mag_ef = R * mag_ef;
+
+        sem = hal.util->new_semaphore();
+
+        have_earth_field = true;
+
+        // form eliptical correction matrix and invert it. This is
+        // needed to remove the effects of the eliptical correction
+        // when calculating new offsets
+        const Vector3f &diagonals = compass.get_diagonals(0);
+        const Vector3f &offdiagonals = compass.get_offdiagonals(0);
+        mat = Matrix3f(
+            diagonals.x, offdiagonals.x, offdiagonals.y,
+            offdiagonals.x,    diagonals.y, offdiagonals.z,
+            offdiagonals.y, offdiagonals.z,    diagonals.z
+            );
+        if (!mat.invert()) {
+            // if we can't invert, use the identity matrix
+            mat.identity();
+        }
+
+        // set initial error to field intensity
+        for (uint16_t i=0; i<num_sectors; i++) {
+            errors[i] = intensity_gauss*1000;
+        }
+        
+        hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void));
+    }
+
+    if (sample_available) {
+        // last sample still being processed by IO thread
         return;
     }
 
-    for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
+    Vector3f field = compass.get_field(0);
-        const Vector3f &ofs = _state[k].offset.get();
+    Vector3f field_change = field - last_field;
-        const Vector3f &field = _state[k].field;
+    if (field_change.length() < min_field_change) {
-        Vector3f b1, diff;
+        return;
-        float length;
+    }
+    
+    if (sem->take_nonblocking()) {
+        // give a sample to the backend to process
+        new_sample.field = field;
+        new_sample.offsets = compass.get_offsets(0);
+        new_sample.attitude = Vector3f(ahrs.roll, ahrs.pitch, ahrs.yaw);
+        sample_available = true;
+        last_field = field;
+        num_samples++;
+        sem->give();
+    }
 
-        if (ofs.is_nan()) {
+    if (sample_available) {
-            // offsets are bad possibly due to a past bug - zero them
+        DataFlash_Class::instance()->Log_Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
-            _state[k].offset.set(Vector3f());
+                                               AP_HAL::micros64(),
+                                               best_offsets.x,
+                                               best_offsets.y,
+                                               best_offsets.z,
+                                               best_error,
+                                               best_yaw_deg,
+                                               worst_error,
+                                               num_samples);
+    }
+
+    if (!converged && sem->take_nonblocking()) {
+        // stop updating the offsets once converged
+        compass.set_offsets(0, best_offsets);
+        if (num_samples > 30 && best_error < 50 && worst_error > 65) {
+            // set the offsets and enable compass for EKF use. Let the
+            // EKF learn the remaining compass offset error
+            compass.save_offsets(0);
+            compass.set_use_for_yaw(0, true);
+            compass.set_learn_type(Compass::LEARN_EKF, true);
+            converged = true;
         }
-
+        sem->give();
-        // get a past element
+    }
-        b1 = Vector3f(_state[k].mag_history[_state[k].mag_history_index].x,
+}
-                      _state[k].mag_history[_state[k].mag_history_index].y,
+
-                      _state[k].mag_history[_state[k].mag_history_index].z);
+/*
-
+  we run the math intensive calculations in the IO thread
-        // the history buffer doesn't have the offsets
+ */
-        b1 += ofs;
+void CompassLearn::io_timer(void)
-
+{
-        // get the current vector
+    if (!sample_available) {
-        const Vector3f &b2 = field;
+        return;
-
+    }
-        // calculate the delta for this sample
+    struct sample s;
-        diff = b2 - b1;
+    if (!sem->take_nonblocking()) {
-        length = diff.length();
+        return;
-        if (length < min_diff) {
+    }
-            // the mag vector hasn't changed enough - we don't get
+    s = new_sample;
-            // enough information from this vector to use it.
+    sample_available = false;
-            // Note that we don't put the current vector into the mag
+    sem->give();
-            // history here. We want to wait for a larger rotation to
+
-            // build up before calculating an offset change, as accuracy
+    process_sample(s);
-            // of the offset change is highly dependent on the size of the
+}
-            // rotation.
+
-            _state[k].mag_history_index = (_state[k].mag_history_index + 1) % _mag_history_size;
+/*
-            continue;
+  process a new compass sample
-        }
+ */
-
+void CompassLearn::process_sample(const struct sample &s)
-        // put the vector in the history
+{
-        _state[k].mag_history[_state[k].mag_history_index] = Vector3i(roundf(field.x) - ofs.x, 
+    uint16_t besti = 0;
-                                                                      roundf(field.y) - ofs.y, 
+    float bestv = 0, worstv=0;
-                                                                      roundf(field.z) - ofs.z);
+
-        _state[k].mag_history_index = (_state[k].mag_history_index + 1) % _mag_history_size;
+    /*
-
+      we run through the 72 possible yaw error values, and for each
-        // equation 6 of Bills paper
+      one we calculate a value for the compass offsets if that yaw
-        diff = diff * (gain * (b2.length() - b1.length()) / length);
+      error is correct. 
-
+     */
-        // limit the change from any one reading. This is to prevent
+    for (uint16_t i=0; i<num_sectors; i++) {
-        // single crazy readings from throwing off the offsets for a long
+        float yaw_err_deg = i*(360/num_sectors);
-        // time
+
-        length = diff.length();
+        // form rotation matrix for the euler attitude
-        if (length > max_change) {
+        Matrix3f dcm;
-            diff *= max_change / length;
+        dcm.from_euler(s.attitude.x, s.attitude.y, wrap_2PI(s.attitude.z + radians(yaw_err_deg)));
-        }
+
-
+        // calculate the field we would expect to get if this yaw error is correct
-        Vector3f new_offsets = _state[k].offset.get() - diff;
+        Vector3f expected_field = dcm.transposed() * mag_ef;
-
+
-        if (new_offsets.is_nan()) {
+        // calculate a value for the compass offsets for this yaw error
-            // don't apply bad offsets
+        Vector3f v1 = mat * s.field;
-            continue;
+        Vector3f v2 = mat * expected_field;
-        }
+        Vector3f offsets = (v2 - v1) + s.offsets;
-
+        float delta = (offsets - predicted_offsets[i]).length();
-        // constrain offsets
+
-        new_offsets.x = constrain_float(new_offsets.x, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
+        if (num_samples == 1) {
-        new_offsets.y = constrain_float(new_offsets.y, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
+            predicted_offsets[i] = offsets;
-        new_offsets.z = constrain_float(new_offsets.z, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
+        } else {
-            
+            // lowpass the predicted offsets and the error
-        // set the new offsets
+            const float learn_rate = 0.92;
-        _state[k].offset.set(new_offsets);
+            predicted_offsets[i] = predicted_offsets[i] * learn_rate + offsets * (1-learn_rate);
+            errors[i] = errors[i] * learn_rate + delta * (1-learn_rate);
+        }
+
+        // keep track of the current best prediction
+        if (i == 0 || errors[i] < bestv) {
+            besti = i;
+            bestv = errors[i];
+        }
+        // also keep the worst error. This is used as part of the convergence test
+        if (i == 0 || errors[i] > worstv) {
+            worstv = errors[i];
+        }
+    }
+
+    if (sem->take_nonblocking()) {
+        // pass the current estimate to the front-end
+        best_offsets = predicted_offsets[besti];
+        best_error = bestv;
+        worst_error = worstv;
+        best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors));
+        sem->give();
     }
 }

libraries/AP_Compass/Compass_learn.h

@@ -0,0 +1,57 @@
+#pragma once
+
+/*
+  compass learning using magnetic field tables from AP_Declination
+ */
+
+class CompassLearn {
+public:
+    CompassLearn(AP_AHRS &ahrs, Compass &compass);
+
+    // called on each compass read
+    void update(void);
+
+private:
+    // reference to AHRS library. Needed for attitude, position and compass
+    const AP_AHRS &ahrs;
+    Compass &compass;
+    bool have_earth_field;
+
+    // 5 degree resolution
+    static const uint16_t num_sectors = 72; 
+    
+    Vector3f predicted_offsets[num_sectors];
+    float errors[num_sectors];
+    uint32_t num_samples;
+
+    // earth field
+    Vector3f mag_ef;
+
+    // semaphore for access to shared data with IO thread
+    AP_HAL::Semaphore *sem;    
+    
+    struct sample {
+        // milliGauss body field and offsets
+        Vector3f field;
+        Vector3f offsets;
+
+        // euler radians attitude
+        Vector3f attitude;
+    };
+
+    Matrix3f mat;
+    
+    struct sample new_sample;
+    bool sample_available;
+    Vector3f last_field;
+    static const uint32_t min_field_change = 60;
+
+    Vector3f best_offsets;
+    float best_error;
+    float best_yaw_deg;
+    float worst_error;
+    bool converged;
+
+    void io_timer(void);
+    void process_sample(const struct sample &s);
+};

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp

@@ -62,7 +62,6 @@ static void loop()
             // use roll = 0, pitch = 0 for this example
             dcm_matrix.from_euler(0, 0, 0);
             heading = compass.calculate_heading(dcm_matrix, i);
-            compass.learn_offsets();
 
             const Vector3f &mag = compass.get_field(i);