diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index d5bd1b9c0d..f04f11ea19 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -77,11 +77,9 @@ void
 AP_AHRS_DCM::matrix_update(float _G_Dt)
 {
 	// Equation 16, adding proportional and integral correction terms
-	_omega = _gyro_vector + _omega_I;
+	_omega = _gyro_vector + _omega_I + _omega_P + _omega_yaw_P;
 
-	// add in P correction terms
-	Vector3f r = (_omega + _omega_P + _omega_yaw_P) * _G_Dt;
-	_dcm_matrix.rotate(r);
+	_dcm_matrix.rotate(_omega * _G_Dt);
 }
 
 
@@ -95,6 +93,7 @@ AP_AHRS_DCM::reset(bool recover_eulers)
 	// reset the integration terms
 	_omega_I.zero();
 	_omega_P.zero();
+	_omega_yaw_P.zero();
 	_omega.zero();
 
 	// if the caller wants us to try to recover to the current
@@ -279,7 +278,7 @@ AP_AHRS_DCM::_P_gain(float spin_rate)
 // 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(float deltat)
+AP_AHRS_DCM::drift_correction_yaw(void)
 {
 	bool new_value = false;
 	float yaw_error;
@@ -370,7 +369,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
 
     // perform yaw drift correction if we have a new yaw reference
     // vector
-    drift_correction_yaw(deltat);
+    drift_correction_yaw();
 
     // integrate the accel vector in the earth frame between GPS readings
     _ra_sum += _dcm_matrix * (_accel_vector * deltat);
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 91aedb527e..2493133a6d 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -57,7 +57,7 @@ private:
 	void		check_matrix(void);
 	bool	 	renorm(Vector3f const &a, Vector3f &result);
 	void 		drift_correction(float deltat);
-	void 		drift_correction_yaw(float deltat);
+	void 		drift_correction_yaw(void);
 	float		yaw_error_compass();
 	float		yaw_error_gps();
 	void 		euler_angles(void);