AHRS: re-instate new DCM drift correction code

This reverts commit 078489638d47fbaffde7c51249e36b5a8fc4ef9d.

libraries/AP_AHRS/AP_AHRS.h

@@ -28,7 +28,8 @@ public:
 	// Constructor
 	AP_AHRS(IMU *imu, GPS *&gps):
 		_imu(imu),
-		_gps(gps)
+		_gps(gps),
+		_barometer(NULL)
 	{
 		// base the ki values by the sensors maximum drift
 		// rate. The APM2 has gyros which are much less drift
@@ -89,9 +90,6 @@ protected:
 	// pointer to compass object, if enabled
 	Compass 	* _compass;
 
-	// pointer to compass object, if enabled
-	AP_Baro 	* _barometer;
-
 	// time in microseconds of last compass update
 	uint32_t        _compass_last_update;
 
@@ -99,8 +97,10 @@ protected:
 	//       IMU under us without our noticing.
 	IMU 		*_imu;
 	GPS 		*&_gps;
+	AP_Baro		*_barometer;
 
-	// true if we are doing centripetal acceleration correction
+	// true if we can assume the aircraft will be flying forward
+	// on its X axis
 	bool		_fly_forward;
 
 	// the limit of the gyro drift claimed by the sensors, in

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -23,6 +23,12 @@
 // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
 #define GPS_SPEED_RESET 100
 
+// the limit (in degrees/second) beyond which we stop integrating
+// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
+// which results in false gyro drift. See
+// http://gentlenav.googlecode.com/files/fastRotations.pdf
+#define SPIN_RATE_LIMIT 20
+
 // table of user settable parameters
 const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
 	// @Param: YAW_P
@@ -70,49 +76,13 @@ AP_AHRS_DCM::update(void)
 void
 AP_AHRS_DCM::matrix_update(float _G_Dt)
 {
-	// _omega_integ_corr is used for _centripetal correction
-	// (theoretically better than _omega)
-	_omega_integ_corr = _gyro_vector + _omega_I;
-
 	// Equation 16, adding proportional and integral correction terms
-	_omega = _omega_integ_corr + _omega_P + _omega_yaw_P;
+	_omega = _gyro_vector + _omega_I + _omega_P + _omega_yaw_P;
 
-	// this is a replacement of the DCM matrix multiply (equation
+	_dcm_matrix.rotate(_omega * _G_Dt);
-	// 17), with known zero elements removed and the matrix
-	// operations inlined. This runs much faster than the original
-	// version of this code, as the compiler was doing a terrible
-	// job of realising that so many of the factors were in common
-	// or zero. It also uses much less stack, as we no longer need
-	// two additional local matrices
-
-	Vector3f r = _omega * _G_Dt;
-	_dcm_matrix.rotate(r);
 }
 
 
-// adjust an accelerometer vector for known acceleration forces
-void
-AP_AHRS_DCM::accel_adjust(Vector3f &accel)
-{
-	float veloc;
-	// compensate for linear acceleration. This makes a
-	// surprisingly large difference in the pitch estimate when
-	// turning, plus on takeoff and landing
-	float acceleration = _gps->acceleration();
-	accel.x -= acceleration;
-
-	// compensate for centripetal acceleration
-	veloc = _gps->ground_speed * 0.01;
-
-	// We are working with a modified version of equation 26 as
-	// our IMU object reports acceleration in the positive axis
-	// direction as positive
-
-	// Equation 26 broken up into separate pieces
-	accel.y -= _omega_integ_corr.z * veloc;
-	accel.z += _omega_integ_corr.y * veloc;
-}
-
 /*
   reset the DCM matrix and omega. Used on ground start, and on
   extreme errors in the matrix
@@ -124,7 +94,6 @@ AP_AHRS_DCM::reset(bool recover_eulers)
 	_omega_I.zero();
 	_omega_P.zero();
 	_omega_yaw_P.zero();
-	_omega_integ_corr.zero();
 	_omega.zero();
 
 	// if the caller wants us to try to recover to the current
@@ -259,216 +228,262 @@ AP_AHRS_DCM::normalize(void)
 }
 
 
+// produce a yaw error value. The returned value is proportional
+// to sin() of the current heading error in earth frame
+float
+AP_AHRS_DCM::yaw_error_compass(void)
+{
+	Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
+	// get the mag vector in the earth frame
+	Vector3f rb = _dcm_matrix * mag;
+
+	rb.normalize();
+	if (rb.is_inf()) {
+		// not a valid vector
+		return 0.0;
+	}
+
+	// get the earths magnetic field (only X and Y components needed)
+	Vector3f mag_earth = Vector3f(cos(_compass->get_declination()),
+				      sin(_compass->get_declination()), 0);
+
+	// calculate the error term in earth frame
+	Vector3f error = rb % mag_earth;
+
+	return error.z;
+}
+
+// produce a yaw error value using the GPS. The returned value is proportional
+// to sin() of the current heading error in earth frame
+float
+AP_AHRS_DCM::yaw_error_gps(void)
+{
+	return sin(ToRad(_gps->ground_course * 0.01) - yaw);
+}
+
+
+float
+AP_AHRS_DCM::_P_gain(float spin_rate)
+{
+	if (spin_rate < ToDeg(50)) {
+		return 1.0;
+	}
+	if (spin_rate > ToDeg(500)) {
+		return 10.0;
+	}
+	return spin_rate/ToDeg(50);
+}
+
+// yaw drift correction using the compass or GPS
+// this function prodoces the _omega_yaw_P vector, and also
+// contributes to the _omega_I.z long term yaw drift estimate
+void
+AP_AHRS_DCM::drift_correction_yaw(void)
+{
+	bool new_value = false;
+	float yaw_error;
+	float yaw_deltat;
+
+	if (_compass && _compass->use_for_yaw()) {
+		if (_compass->last_update != _compass_last_update) {
+			yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6;
+			_compass_last_update = _compass->last_update;
+			if (!_have_initial_yaw) {
+				float heading = _compass->calculate_heading(_dcm_matrix);
+				_dcm_matrix.from_euler(roll, pitch, heading);
+				_omega_yaw_P.zero();
+				_have_initial_yaw = true;
+			}
+			new_value = true;
+			yaw_error = yaw_error_compass();
+		}
+	} else if (_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;
+			_gps_last_update = _gps->last_fix_time;
+			if (!_have_initial_yaw) {
+				_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01));
+				_omega_yaw_P.zero();
+				_have_initial_yaw = true;
+			}
+			new_value = true;
+			yaw_error = yaw_error_gps();
+		}
+	}
+
+	if (!new_value) {
+		// 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;
+		return;
+	}
+
+	// the yaw error is a vector in earth frame
+	Vector3f error = Vector3f(0,0, yaw_error);
+
+	// convert the error vector to body frame
+	error = _dcm_matrix.mul_transpose(error);
+
+	// the spin rate changes the P gain, and disables the
+	// integration at higher rates
+	float spin_rate = _omega.length();
+
+	// update the proportional control to drag the
+	// yaw back to the right value. We use a gain
+	// that depends on the spin rate. See the fastRotations.pdf
+	// paper from Bill Premerlani
+	_omega_yaw_P = error * _P_gain(spin_rate) * _kp_yaw.get();
+
+	// don't update the drift term if we lost the yaw reference
+	// for more than 2 seconds
+	if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
+		// also add to the I term
+		_omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
+	}
+
+	_error_yaw_sum += fabs(yaw_error);
+	_error_yaw_count++;
+}
+
+
+
+
 // perform drift correction. This function aims to update _omega_P and
 // _omega_I with our best estimate of the short term and long term
 // gyro error. The _omega_P value is what pulls our attitude solution
 // back towards the reference vector quickly. The _omega_I term is an
 // attempt to learn the long term drift rate of the gyros.
 //
-// This function also updates _omega_yaw_P with a yaw correction term
+// This drift correction implementation is based on a paper
-// from our yaw reference vector
+// by Bill Premerlani from here:
+//   http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
 void
 AP_AHRS_DCM::drift_correction(float deltat)
 {
-	float error_course = 0;
+    Vector3f error;
-	Vector3f accel;
+    Vector3f velocity;
-	Vector3f error;
+    uint32_t last_correction_time;
-	float error_norm = 0;
+    bool use_gps = true;
-	float yaw_deltat = 0;
 
-	accel = _accel_vector;
+    // perform yaw drift correction if we have a new yaw reference
+    // vector
+    drift_correction_yaw();
 
-	// if enabled, use the GPS to correct our accelerometer vector
+    // integrate the accel vector in the earth frame between GPS readings
-	// for centripetal forces
+    _ra_sum += _dcm_matrix * (_accel_vector * deltat);
-	if(_fly_forward &&
-	   _gps != NULL &&
-	   _gps->status() == GPS::GPS_OK) {
-		accel_adjust(accel);
-	}
 
+    // keep a sum of the deltat values, so we know how much time
+    // we have integrated over
+    _ra_deltat += deltat;
 
-	//*****Roll and Pitch***************
+    // check if we have GPS lock
+    if (_gps == NULL || _gps->status() != GPS::GPS_OK) {
+	    use_gps = false;
+    }
 
-	// normalise the accelerometer vector to a standard length
+    // a copter (which has _fly_forward false) which doesn't have a
-	// this is important to reduce the impact of noise on the
+    // compass for yaw can't rely on the GPS velocity lining up with
-	// drift correction, as very noisy vectors tend to have
+    // the earth frame from DCM, so it needs to assume zero velocity
-	// abnormally high lengths. By normalising the length we
+    // in the drift correction
-	// reduce their impact.
+    if (!_fly_forward && !(_compass && _compass->use_for_yaw())) {
-	float accel_length = accel.length();
+	    use_gps = false;
-	accel *= (_gravity / accel_length);
+    }
-	if (accel.is_inf()) {
-		// we can't do anything useful with this sample
-		_omega_P.zero();
-		return;
-	}
 
-	// calculate the error, in m/2^2, between the attitude
+    if (use_gps == false) {
-	// implied by the accelerometers and the attitude
+        // no GPS, or no lock. We assume zero velocity. This at
-	// in the current DCM matrix
+        // least means we can cope with gyro drift while sitting
-	error =  _dcm_matrix.c % accel;
+        // on a bench with no GPS lock
+        if (_ra_deltat < 0.1) {
+            // not enough time has accumulated
+            return;
+        }
+        velocity.zero();
+	_last_velocity.zero();
+        last_correction_time = millis();
+    } else {
+        if (_gps->last_fix_time == _ra_sum_start) {
+            // we don't have a new GPS fix - nothing more to do
+            return;
+        }
+        velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), 0);
+        last_correction_time = _gps->last_fix_time;
+    }
 
-	// Limit max error to limit the effect of noisy values
+    // even without GPS lock we can correct for the vertical acceleration
-	// on the algorithm. This limits the error to about 11
+    if (_barometer != NULL) {
-	// degrees
+	    // Z velocity is down
-	error_norm = error.length();
+	    velocity.z = - _barometer->get_climb_rate();
-	if (error_norm > 2) {
+    }
-		error *= (2 / error_norm);
-	}
 
-	// we now want to calculate _omega_P and _omega_I. The
+    // see if this is our first time through - in which case we
-	// _omega_P value is what drags us quickly to the
+    // just setup the start times and return
-	// accelerometer reading.
+    if (_ra_sum_start == 0) {
-	_omega_P = error * _kp_roll_pitch;
+        _ra_sum_start = last_correction_time;
+        _last_velocity = velocity;
+        return;
+    }
 
-	// the _omega_I is the long term accumulated gyro
+    // get the corrected acceleration vector in earth frame. Units
-	// error. This determines how much gyro drift we can
+    // are m/s/s
-	// handle.
+    Vector3f ge;
-	_omega_I_sum += error * (_ki_roll_pitch * deltat);
+    float v_scale = 1.0/(_ra_deltat*_gravity);
-	_omega_I_sum_time += deltat;
+    ge = Vector3f(0, 0, -1.0) + ((velocity - _last_velocity) * v_scale);
 
-	// these sums support the reporting of the DCM state via MAVLink
+    // calculate the error term in earth frame.
-	_error_rp_sum += error_norm;
+    ge.normalize();
-	_error_rp_count++;
+    _ra_sum.normalize();
+    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;
+    }
 
-	// yaw drift correction
+    _error_rp_sum += error.length();
+    _error_rp_count++;
 
-	// we only do yaw drift correction when we get a new yaw
+    // convert the error term to body frame
-	// reference vector. In between times we rely on the gyros for
+    error = _dcm_matrix.mul_transpose(error);
-	// yaw. Avoiding this calculation on every call to
-	// update_DCM() saves a lot of time
-	if (_compass && _compass->use_for_yaw()) {
-		if (_compass->last_update != _compass_last_update) {
-			yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
-			if (_have_initial_yaw && yaw_deltat < 2.0) {
-				// Equation 23, Calculating YAW error
-				// We make the gyro YAW drift correction based
-				// on compass magnetic heading
-				float heading_x, heading_y;
-				float heading = _compass->calculate_heading(_dcm_matrix);
-				heading_x = cos(heading);
-				heading_y = sin(heading);
-				error_course = (_dcm_matrix.a.x * heading_y) - (_dcm_matrix.b.x * heading_x);
-				_compass_last_update = _compass->last_update;
-			} else {
-				// this is our first estimate of the yaw,
-				// or the compass has come back online after
-				// no readings for 2 seconds.
-				//
-				// construct a DCM matrix based on the current
-				// roll/pitch and the compass heading.
-				// First ensure the compass heading has been
-				// calculated
-				float heading = _compass->calculate_heading(_dcm_matrix);
 
-				// now construct a new DCM matrix
+    // base the P gain on the spin rate
-				_dcm_matrix.from_euler(roll, pitch, heading);
+    float spin_rate = _omega.length();
-				_have_initial_yaw = true;
-				_compass_last_update = _compass->last_update;
-				error_course = 0;
-			}
-		}
-	} else if (_gps && _gps->status() == GPS::GPS_OK) {
-		if (_gps->last_fix_time != _gps_last_update) {
-			// Use GPS Ground course to correct yaw gyro drift
-			if (_gps->ground_speed >= GPS_SPEED_MIN) {
-				yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
-				if (_have_initial_yaw && yaw_deltat < 2.0) {
-					float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
-					float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
-					// Equation 23, Calculating YAW error
-					error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
-					_gps_last_update = _gps->last_fix_time;
-				} else  {
-					// when we first start moving, set the
-					// DCM matrix to the current
-					// roll/pitch values, but with yaw
-					// from the GPS
-					_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course));
-					_have_initial_yaw =  true;
-					error_course = 0;
-					_gps_last_update = _gps->last_fix_time;
-				}
-			} else if (_gps->ground_speed >= GPS_SPEED_RESET) {
-				// we are not going fast enough to use GPS for
-				// course correction, but we won't reset
-				// _have_initial_yaw yet, instead we just let
-				// the gyro handle yaw
-				error_course = 0;
-			} else {
-				// we are moving very slowly. Reset
-				// _have_initial_yaw and adjust our heading
-				// rapidly next time we get a good GPS ground
-				// speed
-				error_course = 0;
-				_have_initial_yaw = false;
-			}
-		}
-	}
 
-	// see if there is any error in our heading relative to the
+    // we now want to calculate _omega_P and _omega_I. The
-	// yaw reference. This will be zero most of the time, as we
+    // _omega_P value is what drags us quickly to the
-	// only calculate it when we get new data from the yaw
+    // accelerometer reading.
-	// reference source
+    _omega_P = error * _P_gain(spin_rate) * _kp;
-	if (yaw_deltat == 0 || error_course == 0) {
-		// we don't have a new reference heading. Slowly
-		// decay the _omega_yaw_P to ensure that if we have
-		// lost the yaw reference sensor completely we don't
-		// keep using a stale offset
-		_omega_yaw_P *= 0.97;
-		goto check_sum_time;
-	}
 
-	// ensure the course error is scaled from -PI to PI
+    // accumulate some integrator error
-	if (error_course > PI) {
+    if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
-		error_course -= 2*PI;
+	    _omega_I_sum += error * _ki * _ra_deltat;
-	} else if (error_course < -PI) {
+	    _omega_I_sum_time += _ra_deltat;
-		error_course += 2*PI;
+    }
-	}
 
-	// Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft
+    if (_omega_I_sum_time >= 5) {
-	// this gives us an error in radians
+	    // limit the rate of change of omega_I to the hardware
-	error = _dcm_matrix.c * error_course;
+	    // reported maximum gyro drift rate. This ensures that
+	    // short term errors don't cause a buildup of omega_I
+	    // beyond the physical limits of the device
+	    float change_limit = _gyro_drift_limit * _omega_I_sum_time;
+	    _omega_I_sum.x = constrain(_omega_I_sum.x, -change_limit, change_limit);
+	    _omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit);
+	    _omega_I_sum.z = constrain(_omega_I_sum.z, -change_limit, change_limit);
+	    _omega_I += _omega_I_sum;
+	    _omega_I_sum.zero();
+	    _omega_I_sum_time = 0;
+    }
 
-	// Adding yaw correction to proportional correction vector. We
+    // zero our accumulator ready for the next GPS step
-	// allow the yaw reference source to affect all 3 components
+    _ra_sum.zero();
-	// of _omega_yaw_P as we need to be able to correctly hold a
+    _ra_deltat = 0;
-	// heading when roll and pitch are non-zero
+    _ra_sum_start = last_correction_time;
-	_omega_yaw_P = error * _kp_yaw.get();
 
-	// add yaw correction to integrator correction vector, but
+    // remember the velocity for next time
-	// only for the z gyro. We rely on the accelerometers for x
+    _last_velocity = velocity;
-	// and y gyro drift correction. Using the compass or GPS for
-	// x/y drift correction is too inaccurate, and can lead to
-	// incorrect builups in the x/y drift. We rely on the
-	// accelerometers to get the x/y components right
-	_omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat);
-
-	// we keep the sum of yaw error for reporting via MAVLink.
-	_error_yaw_sum += error_course;
-	_error_yaw_count++;
-
-check_sum_time:
-	if (_omega_I_sum_time > 10) {
-		// every 10 seconds we apply the accumulated
-		// _omega_I_sum changes to _omega_I. We do this to
-		// prevent short term feedback between the P and I
-		// terms of the controller. The _omega_I vector is
-		// supposed to refect long term gyro drift, but with
-		// high noise it can start to build up due to short
-		// term interactions. By summing it over 10 seconds we
-		// prevent the short term interactions and can apply
-		// the slope limit more accurately
-		float drift_limit = _gyro_drift_limit * _omega_I_sum_time;
-		_omega_I_sum.x = constrain(_omega_I_sum.x, -drift_limit, drift_limit);
-		_omega_I_sum.y = constrain(_omega_I_sum.y, -drift_limit, drift_limit);
-		_omega_I_sum.z = constrain(_omega_I_sum.z, -drift_limit, drift_limit);
-
-		_omega_I += _omega_I_sum;
-
-		// zero the sum
-		_omega_I_sum.zero();
-		_omega_I_sum_time = 0;
-	}
 }
 
 

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -16,15 +16,15 @@ public:
 	// Constructors
 	AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
 	{
-		_kp_roll_pitch = 0.13;
+		_dcm_matrix.identity();
-		_kp_yaw.set(0.2);
-		_dcm_matrix(Vector3f(1, 0, 0),
-			    Vector3f(0, 1, 0),
-			    Vector3f(0, 0, 1));
 
-		// base the ki values on the sensors drift rate
+		// these are experimentally derived from the simulator
-		_ki_roll_pitch = _gyro_drift_limit * 5;
+		// with large drift levels
-		_ki_yaw        = _gyro_drift_limit * 8;
+                _ki = 0.0087;
+		_ki_yaw = 0.01;
+
+                _kp = 0.4;
+		_kp_yaw.set(0.4);
 	}
 
 	// return the smoothed gyro vector corrected for drift
@@ -46,18 +46,20 @@ public:
 	AP_Float	_kp_yaw;
 
 private:
-	float		_kp_roll_pitch;
+	float		_kp;
-	float		_ki_roll_pitch;
+	float		_ki;
 	float		_ki_yaw;
 	bool		_have_initial_yaw;
 
 	// Methods
-	void 		accel_adjust(Vector3f &accel);
 	void 		matrix_update(float _G_Dt);
 	void 		normalize(void);
 	void		check_matrix(void);
 	bool	 	renorm(Vector3f const &a, Vector3f &result);
 	void 		drift_correction(float deltat);
+	void 		drift_correction_yaw(void);
+	float		yaw_error_compass();
+	float		yaw_error_gps();
 	void 		euler_angles(void);
 
 	// primary representation of attitude
@@ -66,14 +68,16 @@ private:
 	Vector3f 	_gyro_vector;		// Store the gyros turn rate in a vector
 	Vector3f 	_accel_vector;		// current accel vector
 
-	Vector3f	_omega_P;		// accel Omega Proportional correction
+	Vector3f	_omega_P;		// accel Omega proportional correction
-	Vector3f	_omega_yaw_P;		// yaw Omega Proportional correction
+	Vector3f	_omega_yaw_P;		// proportional yaw correction
 	Vector3f 	_omega_I;		// Omega Integrator correction
-	Vector3f 	_omega_I_sum;		// summation vector for omegaI
+	Vector3f	_omega_I_sum;
 	float		_omega_I_sum_time;
-	Vector3f 	_omega_integ_corr;	// Partially corrected Gyro_Vector data - used for centrepetal correction
 	Vector3f 	_omega;			// Corrected Gyro_Vector data
 
+	// P term gain based on spin rate
+	float 		_P_gain(float spin_rate);
+
 	// state to support status reporting
 	float		_renorm_val_sum;
 	uint16_t	_renorm_val_count;
@@ -86,6 +90,15 @@ private:
 
 	// time in millis when we last got a GPS heading
 	uint32_t	_gps_last_update;
+
+	// state of accel drift correction
+	Vector3f	_ra_sum;
+	Vector3f	_last_velocity;
+	float		_ra_deltat;
+	uint32_t	_ra_sum_start;
+
+	// current drift error in earth frame
+	Vector3f	_drift_error_earth;
 };
 
 #endif // AP_AHRS_DCM_H