First pass at adding the magnetometer to the comp filt.

Author: iandareid

include/estimator.h

@@ -82,6 +82,7 @@ class Estimator : public ParamListenerInterface
 
   bool is_initialized_ = false;
   uint64_t last_acc_update_us_;
+  uint64_t last_mag_update_us_;
   uint64_t last_extatt_update_us_;
 
   turbomath::Vector w1_;
@@ -91,18 +92,29 @@ class Estimator : public ParamListenerInterface
 
   turbomath::Vector accel_LPF_;
   turbomath::Vector gyro_LPF_;
+  turbomath::Vector mag_LPF_;
+  turbomath::Vector mag_dir_;
+
+  bool mag_direction_found_ = false; // TODO: Init correctly
+
+  float mag_[3];
 
   turbomath::Vector w_acc_;
 
   bool extatt_update_next_run_;
   turbomath::Quaternion q_extatt_;
 
-  void run_LPF();
+  void run_accel_LPF();
+  void run_mag_LPF();
 
   bool can_use_accel() const;
   bool can_use_extatt() const;
+  bool can_use_mag();
+  bool mag_disturbance_is_large();
   turbomath::Vector accel_correction() const;
   turbomath::Vector extatt_correction() const;
+  turbomath::Vector mag_correction() const;
+  turbomath::Vector update_correction(turbomath::Vector w_err, float kp, float ki, float dt, const uint64_t now_us);
   turbomath::Vector smoothed_gyro_measurement();
   void integrate_angular_rate(turbomath::Quaternion & quat, const turbomath::Vector & omega,
                               const float dt) const;

include/param.h

@@ -280,20 +280,23 @@ enum : uint16_t
   /*** ESTIMATOR CONFIGURATION ***/
   /*******************************/
   PARAM_INIT_TIME,
-  PARAM_FILTER_KP_ACC,
   PARAM_FILTER_KI,
+  PARAM_FILTER_KP_ACC,
   PARAM_FILTER_KP_EXT,
+  PARAM_FILTER_KP_MAG,
   PARAM_FILTER_ACCEL_MARGIN,
 
   PARAM_FILTER_USE_QUAD_INT,
   PARAM_FILTER_USE_MAT_EXP,
   PARAM_FILTER_USE_ACC,
+  PARAM_FILTER_USE_MAG,
 
   PARAM_CALIBRATE_GYRO_ON_ARM,
 
   PARAM_GYRO_XY_ALPHA,
   PARAM_GYRO_Z_ALPHA,
   PARAM_ACC_ALPHA,
+  PARAM_MAG_ALPHA,
 
   PARAM_GYRO_X_BIAS,
   PARAM_GYRO_Y_BIAS,
@@ -317,6 +320,8 @@ enum : uint16_t
   PARAM_MAG_X_BIAS,
   PARAM_MAG_Y_BIAS,
   PARAM_MAG_Z_BIAS,
+  PARAM_MAG_INCLINATION,
+  PARAM_MAG_DECLINATION,
 
   PARAM_BARO_BIAS,
   PARAM_GROUND_LEVEL,

src/estimator.cpp

@@ -99,7 +99,7 @@ void Estimator::init()
 
 void Estimator::param_change_callback(uint16_t param_id) { (void) param_id; }
 
-void Estimator::run_LPF()
+void Estimator::run_accel_LPF()
 {
   float alpha_acc = RF_.params_.get_param_float(PARAM_ACC_ALPHA);
   accel_LPF_.x = (1.0f - alpha_acc) * RF_.sensors_.get_imu()->accel[0] + alpha_acc * accel_LPF_.x;
@@ -113,6 +113,14 @@ void Estimator::run_LPF()
   gyro_LPF_.z = (1.0f - alpha_gyro_z)  * RF_.sensors_.get_imu()->gyro[2] + alpha_gyro_z * gyro_LPF_.z;
 }
 
+void Estimator::run_mag_LPF()
+{
+  float alpha_mag = RF_.params_.get_param_float(PARAM_MAG_ALPHA);
+  mag_LPF_.x = (1.0f - alpha_mag) * RF_.sensors_.get_mag()->flux[0] + alpha_mag * mag_LPF_.x;
+  mag_LPF_.y = (1.0f - alpha_mag) * RF_.sensors_.get_mag()->flux[1] + alpha_mag * mag_LPF_.y;
+  mag_LPF_.z = (1.0f - alpha_mag) * RF_.sensors_.get_mag()->flux[2] + alpha_mag * mag_LPF_.z;
+}
+
 void Estimator::set_external_attitude_update(const turbomath::Quaternion & q)
 {
   extatt_update_next_run_ = true;
@@ -136,23 +144,80 @@ void Estimator::run(const float dt)
   }
 
   // Low-pass filter accel and gyro measurements
-  run_LPF();
+  run_accel_LPF();
 
   //
   // Gyro Correction Term (werr)
   //
 
   float kp = 0.0f;
-  float ki = RF_.params_.get_param_float(PARAM_FILTER_KI);
+  float ki = RF_.params_.get_param_float(PARAM_FILTER_KI); // TODO: Should this be specific to the sensor?
 
   turbomath::Vector w_err;
+  turbomath::Vector correction_term;
 
   if (can_use_accel()) {
     // Get error estimated by accelerometer measurement
     w_err = accel_correction();
     kp = RF_.params_.get_param_float(PARAM_FILTER_KP_ACC);
 
     last_acc_update_us_ = now_us;
+
+    correction_term += update_correction(w_err, kp, ki, dt, now_us);
+  }
+
+  if (can_use_mag()) {
+
+    run_mag_LPF();
+
+    // Get error estimated by magnetometer measurement
+    w_err = mag_correction();
+    kp = RF_.params_.get_param_float(PARAM_FILTER_KP_MAG);
+    
+    last_mag_update_us_ = now_us;
+
+    correction_term += update_correction(w_err, kp, ki, dt, now_us);
+    
+  } else if (RF_.params_.get_param_float(PARAM_MAG_INCLINATION) > 0.0f) {
+
+    // If we know the inclination we can find the mag direction.
+
+    turbomath::Vector e_0(0.0, 0.0, 1.0);
+
+    float inclination = RF_.params_.get_param_float(PARAM_MAG_INCLINATION);
+    float declination = RF_.params_.get_param_float(PARAM_MAG_DECLINATION);
+
+    turbomath::Quaternion mag_frame_to_inertial(0.0, inclination, declination);
+
+    // Use the magnetometer measurement in the inertial frame as the reference vector.
+    mag_dir_ = mag_frame_to_inertial.rotate(e_0);
+
+  }
+  else if (!mag_direction_found_ && now_us > static_cast<uint64_t>(RF_.params_.get_param_int(PARAM_INIT_TIME)) * 1000) {
+    
+    // If the mag direction found has not been found, use the assumption that 
+    // we have the correct attitude after the init period to backout what the mag_dir is.
+    
+    float roll = 0.0;
+    float pitch = 0.0;
+    float yaw = 0.0;
+
+    state_.attitude.get_RPY(&roll, &pitch, &yaw);
+
+    turbomath::Quaternion tilt_compensation(roll, pitch, 0.0);
+    
+    turbomath::Vector tilt_adjusted_mag = tilt_compensation.rotate(mag_LPF_);
+
+    yaw = atan2(tilt_adjusted_mag.y, tilt_adjusted_mag.x);
+
+    float declination = RF_.params_.get_param_float(PARAM_MAG_DECLINATION);
+    
+    turbomath::Quaternion body_to_inertial(roll, pitch, yaw + declination);
+
+    mag_dir_ = body_to_inertial.rotate(mag_LPF_);
+    mag_dir_.normalize();
+
+    mag_direction_found_ = true;
   }
 
   if (can_use_extatt()) {
@@ -171,26 +236,19 @@ void Estimator::run(const float dt)
 
     last_extatt_update_us_ = now_us;
     extatt_update_next_run_ = false;
-  }
 
-  // Crank up the gains for the first few seconds for quick convergence
-  if (now_us < static_cast<uint64_t>(RF_.params_.get_param_int(PARAM_INIT_TIME)) * 1000) {
-    kp = RF_.params_.get_param_float(PARAM_FILTER_KP_ACC) * 10.0f;
-    ki = RF_.params_.get_param_float(PARAM_FILTER_KI) * 10.0f;
+    correction_term += update_correction(w_err, kp, ki, dt, now_us);
+    
   }
 
   //
   // Composite Bias-Free Angular Rate (wfinal)
   //
 
-  // Integrate biases driven by measured angular error
-  // eq 47b Mahony Paper, using correction term w_err found above
-  bias_ -= ki * w_err * dt;
-
   // Build the composite omega vector for kinematic propagation
   // This the stuff inside the p function in eq. 47a - Mahony Paper
   turbomath::Vector wbar = smoothed_gyro_measurement();
-  turbomath::Vector wfinal = wbar - bias_ + kp * w_err;
+  turbomath::Vector wfinal = wbar - bias_ + correction_term;
 
   //
   // Propagate Dynamics
@@ -218,10 +276,24 @@ void Estimator::run(const float dt)
   }
 }
 
+turbomath::Vector Estimator::update_correction(turbomath::Vector w_err, float kp, float ki, float dt, const uint64_t now_us)
+{
+  // Crank up the gains for the first few seconds for quick convergence
+  if (now_us < static_cast<uint64_t>(RF_.params_.get_param_int(PARAM_INIT_TIME)) * 1000) {
+    kp *= 10.;
+    ki *= 10.;
+  }
+  
+  // Integrate biases driven by measured angular error
+  // eq 47b Mahony Paper, using correction term w_err found above
+  bias_ -= ki * w_err * dt;
+  return kp * w_err;
+}
+
 bool Estimator::can_use_accel() const
 {
-  // if we are not using accel, just bail
-  if (!RF_.params_.get_param_int(PARAM_FILTER_USE_ACC)) { return false; }
+  // if we are not using accel or we are using extatt, just bail
+  if (!RF_.params_.get_param_int(PARAM_FILTER_USE_ACC) || can_use_extatt()) { return false; }
 
   // current magnitude of LPF'd accelerometer
   const float a_sqrd_norm = accel_LPF_.sqrd_norm();
@@ -240,6 +312,32 @@ bool Estimator::can_use_accel() const
   return (lowerbound < a_sqrd_norm && a_sqrd_norm < upperbound);
 }
 
+bool Estimator::can_use_mag() // TODO: make const
+{
+  // if we are not using mag, just bail
+  if (!RF_.params_.get_param_int(PARAM_FILTER_USE_MAG)
+      || mag_disturbance_is_large()
+      || !mag_direction_found_
+      || can_use_extatt())
+      { return false; }
+
+  if (mag_ == RF_.sensors_.get_mag()->flux) {
+    return false;
+  }
+  else {
+    for (int i = 0; i < 3; i++) {
+      mag_[i] = RF_.sensors_.get_mag()->flux[i];
+    }
+  }
+
+  return true;
+}
+
+bool Estimator::mag_disturbance_is_large() {
+  // TODO: Somehow check if the disturbance is large.
+  return false;
+}
+
 bool Estimator::can_use_extatt() const { return extatt_update_next_run_; }
 
 turbomath::Vector Estimator::accel_correction() const
@@ -265,6 +363,29 @@ turbomath::Vector Estimator::accel_correction() const
   return w_acc;
 }
 
+turbomath::Vector Estimator::mag_correction() const
+{
+  // turn measurement into a unit vector
+  turbomath::Vector m = mag_LPF_.normalized();
+
+  // Get the quaternion from accelerometer (low-frequency measure q)
+  // (Not in either paper)
+  turbomath::Quaternion q_mag_inv(mag_dir_, m);
+
+  // Get the error quaternion between observer and low-freq q
+  // Below Eq. 45 Mahony Paper
+  turbomath::Quaternion q_tilde = q_mag_inv * state_.attitude;
+
+  // Correction Term of Eq. 47a and 47b Mahony Paper
+  // w_acc = 2*s_tilde*v_tilde
+  turbomath::Vector w_mag;
+  w_mag.x = -2.0f * q_tilde.w * q_tilde.x;
+  w_mag.y = -2.0f * q_tilde.w * q_tilde.y;
+  w_mag.z = -2.0f * q_tilde.w * q_tilde.z;
+
+  return w_mag;
+}
+
 turbomath::Vector Estimator::extatt_correction() const
 {
   // DCM rows of attitude estimate and external measurement (world w.r.t body).

src/param.cpp

@@ -330,17 +330,20 @@ void Params::set_defaults(void)
   init_param_float(PARAM_FILTER_KP_ACC, "FILTER_KP_ACC", 0.5f); // estimator proportional gain on accel-based error - See estimator documentation | 0 | 10.0
   init_param_float(PARAM_FILTER_KI, "FILTER_KI", 0.01f); // estimator integral gain - See estimator documentation | 0 | 1.0
   init_param_float(PARAM_FILTER_KP_EXT, "FILTER_KP_EXT", 1.5f); // estimator proportional gain on external attitude-based error - See estimator documentation | 0 | 10.0
+  init_param_float(PARAM_FILTER_KP_MAG, "FILTER_KP_MAG", 1.0f); // estimator proportional gain on magnetometer-based error - See estimator documentation | 0 | 10.0
   init_param_float(PARAM_FILTER_ACCEL_MARGIN, "FILTER_ACCMARGIN", 0.1f); // allowable accel norm margin around 1g to determine if accel is usable | 0 | 1.0
 
   init_param_int(PARAM_FILTER_USE_QUAD_INT, "FILTER_QUAD_INT", 1); // Perform a quadratic averaging of LPF gyro data prior to integration (adds ~20 us to estimation loop on F1 processors) | 0 | 1
   init_param_int(PARAM_FILTER_USE_MAT_EXP, "FILTER_MAT_EXP", 1); // 1 - Use matrix exponential to improve gyro integration (adds ~90 us to estimation loop in F1 processors) 0 - use euler integration | 0 | 1
   init_param_int(PARAM_FILTER_USE_ACC, "FILTER_USE_ACC", 1);  // Use accelerometer to correct gyro integration drift (adds ~70 us to estimation loop) | 0 | 1
+  init_param_int(PARAM_FILTER_USE_MAG, "FILTER_USE_MAG", 1);  // Use magnetometer to correct gyro integration drift (adds ?? us to estimation loop) | 0 | 1
 
   init_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, "CAL_GYRO_ARM", false); // True if desired to calibrate gyros on arm | 0 | 1
 
   init_param_float(PARAM_GYRO_XY_ALPHA, "GYROXY_LPF_ALPHA", 0.3f); // Low-pass filter constant on gyro X and Y axes - See estimator documentation | 0 | 1.0
   init_param_float(PARAM_GYRO_Z_ALPHA, "GYROZ_LPF_ALPHA", 0.3f); // Low-pass filter constant on gyro Z axis - See estimator documentation | 0 | 1.0
   init_param_float(PARAM_ACC_ALPHA, "ACC_LPF_ALPHA", 0.5f); // Low-pass filter constant on all accel axes - See estimator documentation | 0 | 1.0
+  init_param_float(PARAM_MAG_ALPHA, "MAG_LPF_ALPHA", 0.3f); // Low-pass filter constant on all mag axes - See estimator documentation | 0 | 1.0
 
   init_param_float(PARAM_GYRO_X_BIAS, "GYRO_X_BIAS", 0.0f); // Constant x-bias of gyroscope readings | -1.0 | 1.0
   init_param_float(PARAM_GYRO_Y_BIAS, "GYRO_Y_BIAS", 0.0f); // Constant y-bias of gyroscope readings | -1.0 | 1.0
@@ -364,6 +367,8 @@ void Params::set_defaults(void)
   init_param_float(PARAM_MAG_X_BIAS,  "MAG_X_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0
   init_param_float(PARAM_MAG_Y_BIAS,  "MAG_Y_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0
   init_param_float(PARAM_MAG_Z_BIAS,  "MAG_Z_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0
+  init_param_float(PARAM_MAG_INCLINATION,  "MAG_INCLINATION", 0.0f); // Inclination of the magnetic field in radians | 0.0 | pi/2
+  init_param_float(PARAM_MAG_DECLINATION,  "MAG_DECLINATION", 0.069132f); // Declination of the magnetic field in radians | -pi | pi
 
   init_param_float(PARAM_BARO_BIAS, "BARO_BIAS", 0.0f); // Barometer measurement bias (Pa) | 0 | inf
   init_param_float(PARAM_GROUND_LEVEL, "GROUND_LEVEL", 1387.0f); // Altitude of ground level (m) | -1000 | 10000