AHRS: make DCM drift correction not rely on accurate yaw

this uses a new formulation of the GPS based drift correction from
Bill Premerlani that rotates the error vector to avoid relying on
accurate yaw. This means we should get accurate roll/pitch correction
even with lots of magnetometer interference

It also makes it possible to fly a multicopter with no compass. It can
even navigate and correct yaw (slowly!)

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -267,6 +267,9 @@ AP_AHRS_DCM::yaw_error_gps(void)
 }
 
 
+// the _P_gain raises the gain of the PI controller
+// when we are spinning fast. See the fastRotations 
+// paper from Bill. 
 float
 AP_AHRS_DCM::_P_gain(float spin_rate)
 {
@@ -302,7 +305,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
 			new_value = true;
 			yaw_error = yaw_error_compass();
 		}
-	} else if (_gps && _gps->status() == GPS::GPS_OK) {
+	} else if (_fly_forward && _gps && _gps->status() == GPS::GPS_OK) {
 		if (_gps->last_fix_time != _gps_last_update &&
 		    _gps->ground_speed >= GPS_SPEED_MIN) {
 			yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3;
@@ -321,7 +324,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
 		// we don't have any new yaw information
 		// slowly decay _omega_yaw_P to cope with loss
 		// of our yaw source
-		_omega_yaw_P.z *= 0.97;
+		_omega_yaw_P *= 0.97;
 		return;
 	}
 
@@ -367,10 +370,8 @@ AP_AHRS_DCM::drift_correction_yaw(void)
 void
 AP_AHRS_DCM::drift_correction(float deltat)
 {
-    Vector3f error;
     Vector3f velocity;
     uint32_t last_correction_time;
-    bool use_gps = true;
 
     // perform yaw drift correction if we have a new yaw reference
     // vector
@@ -383,20 +384,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // we have integrated over
     _ra_deltat += deltat;
 
-    // check if we have GPS lock
     if (_gps == NULL || _gps->status() != GPS::GPS_OK) {
-	    use_gps = false;
-    }
-
-    // a copter (which has _fly_forward false) which doesn't have a
-    // compass for yaw can't rely on the GPS velocity lining up with
-    // the earth frame from DCM, so it needs to assume zero velocity
-    // in the drift correction
-    if (!_fly_forward && !(_compass && _compass->use_for_yaw())) {
-	    use_gps = false;
-    }
-
-    if (use_gps == false) {
         // no GPS, or no lock. We assume zero velocity. This at
         // least means we can cope with gyro drift while sitting
         // on a bench with no GPS lock
@@ -407,6 +395,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
         velocity.zero();
 	_last_velocity.zero();
         last_correction_time = millis();
+	_have_gps_lock = false;
     } else {
         if (_gps->last_fix_time == _ra_sum_start) {
             // we don't have a new GPS fix - nothing more to do
@@ -414,6 +403,12 @@ AP_AHRS_DCM::drift_correction(float deltat)
         }
         velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), 0);
         last_correction_time = _gps->last_fix_time;
+	if (_have_gps_lock == false) {
+		// if we didn't have GPS lock in the last drift
+		// correction interval then set the velocities equal
+		_last_velocity = velocity;
+	}
+	_have_gps_lock = true;
     }
 
 #if 0
@@ -436,30 +431,44 @@ AP_AHRS_DCM::drift_correction(float deltat)
         return;
     }
 
-    // get the corrected acceleration vector in earth frame. Units
+    // equation 9: get the corrected acceleration vector in earth frame. Units
     // are m/s/s
-    Vector3f ge;
+    Vector3f GA_e;
     float v_scale = 1.0/(_ra_deltat*_gravity);
-    ge = Vector3f(0, 0, -1.0) + ((velocity - _last_velocity) * v_scale);
+    GA_e = Vector3f(0, 0, -1.0) + ((velocity - _last_velocity) * v_scale);
-
+    GA_e.normalize();
-    // calculate the error term in earth frame.
+    if (GA_e.is_inf()) {
-    ge.normalize();
+	    // wait for some non-zero acceleration information
-    _ra_sum.normalize();
+	    return;
-    if (_ra_sum.is_inf() || ge.is_inf()) {
-	    // the _ra_sum length is zero - we are falling with
-	    // no apparent gravity. This gives us no information
-	    // about which way up we are, so treat the error as zero
-	    error = Vector3f(0,0,0);
-    } else {
-	    error = _ra_sum % ge;
     }
 
-    _error_rp_sum += error.length();
+    // calculate the error term in earth frame.
-    _error_rp_count++;
+    Vector3f GA_b = _ra_sum / _ra_deltat;
+    GA_b.normalize();
+    Vector3f error = GA_b % GA_e;
+    
+    // step 2 calculate earth_error_Z
+    float earth_error_Z = error.z;
+
+    // equation 10
+    float tilt = sqrt(sq(GA_e.x) + sq(GA_e.y));
+
+    // equation 11
+    float theta = atan2(GA_b.y, GA_b.x);
+    
+    // equation 12
+    Vector3f GA_e2 = Vector3f(cos(theta)*tilt, sin(theta)*tilt, GA_e.z);
+
+    // step 6
+    error = GA_b % GA_e2;
+    error.z = earth_error_Z;
 
     // convert the error term to body frame
     error = _dcm_matrix.mul_transpose(error);
 
+    _error_rp_sum += error.length();
+    _error_rp_count++;
+
     // base the P gain on the spin rate
     float spin_rate = _omega.length();
 

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -99,6 +99,9 @@ private:
 
 	// current drift error in earth frame
 	Vector3f	_drift_error_earth;
+
+	// whether we have GPS lock
+	bool		_have_gps_lock;
 };
 
 #endif // AP_AHRS_DCM_H