diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 10e2b90381a92bfcca0826a59bc338a1edb5e0ed..90499e2bf9f6a31a0a957d4349ac6dcd87295df1 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -102,6 +102,9 @@ protected:
 	// the limit of the gyro drift claimed by the sensors, in
 	// radians/s/s
 	float           _gyro_drift_limit;
+
+	// acceleration due to gravity in m/s/s
+	static const float _gravity = 9.80665;
 };
 
 #include <AP_AHRS_DCM.h>
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 2cf29b3249ef2adfe679ca98220ff730b63edf51..3b0075a73103ed2ab33bae1c2d1f2a26d58c0815 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -279,7 +279,6 @@ AP_AHRS_DCM::drift_correction(float deltat)
 	Vector3f accel;
 	Vector3f error;
 	float error_norm = 0;
-	const float gravity_squared = (9.80665*9.80665);
 	float yaw_deltat = 0;
 
 	accel = _accel_vector;
@@ -295,58 +294,50 @@ AP_AHRS_DCM::drift_correction(float deltat)
 
 	//*****Roll and Pitch***************
 
-	// calculate the z component of the accel vector assuming it
-	// has a length of 9.8. This discards the z accelerometer
-	// sensor reading completely. Logs show that the z accel is
-	// the noisest, plus it has a disproportionate impact on the
-	// drift correction result because of the geometry when we are
-	// mostly flat. Dropping it completely seems to make the DCM
-	// algorithm much more resilient to large amounts of
-	// accelerometer noise.
-	float zsquared = gravity_squared - ((accel.x * accel.x) + (accel.y * accel.y));
-	if (zsquared < 0) {
+	// normalise the accelerometer vector to a standard length
+	// this is important to reduce the impact of noise on the
+	// drift correction, as very noisy vectors tend to have
+	// abnormally high lengths. By normalising the length we
+	// reduce their impact.
+	float accel_length = accel.length();
+	accel *= (_gravity / accel_length);
+	if (accel.is_inf()) {
+		// we can't do anything useful with this sample
 		_omega_P.zero();
-	} else {
-		if (accel.z > 0) {
-			accel.z = sqrt(zsquared);
-		} else {
-			accel.z = -sqrt(zsquared);
-		}
-
-		// calculate the error, in m/2^2, between the attitude
-		// implied by the accelerometers and the attitude
-		// in the current DCM matrix
-		error =  _dcm_matrix.c % accel;
-
-		// error from the above is in m/s^2 units.
+		return;
+	}
 
-		// Limit max error to limit the effect of noisy values
-		// on the algorithm. This limits the error to about 11
-		// degrees
-		error_norm = error.length();
-		if (error_norm > 2) {
-			error *= (2 / error_norm);
-		}
+	// calculate the error, in m/2^2, between the attitude
+	// implied by the accelerometers and the attitude
+	// in the current DCM matrix
+	error =  _dcm_matrix.c % accel;
+
+	// Limit max error to limit the effect of noisy values
+	// on the algorithm. This limits the error to about 11
+	// degrees
+	error_norm = error.length();
+	if (error_norm > 2) {
+		error *= (2 / error_norm);
+	}
 
-		// we now want to calculate _omega_P and _omega_I. The
-		// _omega_P value is what drags us quickly to the
-		// accelerometer reading.
-		_omega_P = error * _kp_roll_pitch;
+	// we now want to calculate _omega_P and _omega_I. The
+	// _omega_P value is what drags us quickly to the
+	// accelerometer reading.
+	_omega_P = error * _kp_roll_pitch;
 
-		// the _omega_I is the long term accumulated gyro
-		// error. This determines how much gyro drift we can
-		// handle.
-		Vector3f omega_I_delta = error * (_ki_roll_pitch * deltat);
+	// the _omega_I is the long term accumulated gyro
+	// error. This determines how much gyro drift we can
+	// handle.
+	Vector3f omega_I_delta = error * (_ki_roll_pitch * deltat);
 
-		// limit the slope of omega_I on each axis to
-		// the maximum drift rate
-		float drift_limit = _gyro_drift_limit * deltat;
-		omega_I_delta.x = constrain(omega_I_delta.x, -drift_limit, drift_limit);
-		omega_I_delta.y = constrain(omega_I_delta.y, -drift_limit, drift_limit);
-		omega_I_delta.z = constrain(omega_I_delta.z, -drift_limit, drift_limit);
+	// limit the slope of omega_I on each axis to
+	// the maximum drift rate
+	float drift_limit = _gyro_drift_limit * deltat;
+	omega_I_delta.x = constrain(omega_I_delta.x, -drift_limit, drift_limit);
+	omega_I_delta.y = constrain(omega_I_delta.y, -drift_limit, drift_limit);
+	omega_I_delta.z = constrain(omega_I_delta.z, -drift_limit, drift_limit);
 
-		_omega_I += omega_I_delta;
-	}
+	_omega_I += omega_I_delta;
 
 	// these sums support the reporting of the DCM state via MAVLink
 	_error_rp_sum += error_norm;
@@ -470,7 +461,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
 	float omega_Iz_delta = error.z * (_ki_yaw * yaw_deltat);
 
 	// limit the slope of omega_I.z to the maximum gyro drift rate
-	float drift_limit = _gyro_drift_limit * yaw_deltat;
+	drift_limit = _gyro_drift_limit * yaw_deltat;
 	omega_Iz_delta = constrain(omega_Iz_delta, -drift_limit, drift_limit);
 
 	_omega_I.z += omega_Iz_delta;