diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 2cc3b6e827..c3f3a1ed73 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -553,6 +553,12 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // convert the error term to body frame
     error = _dcm_matrix.mul_transpose(error);
 
+    if (error.is_nan() || error.is_inf()) {
+	    // don't allow bad values
+	    check_matrix();
+	    return;
+    }
+
     _error_rp_sum += error.length();
     _error_rp_count++;