diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index c9f085d633..1792f42e2e 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -256,15 +256,6 @@ AP_AHRS_DCM::yaw_error_compass(void)
     return rb % _mag_earth;
 }
 
-// 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 sinf(ToRad(_gps->ground_course * 0.01f) - yaw);
-}
-
-
 // the _P_gain raises the gain of the PI controller
 // when we are spinning fast. See the fastRotations
 // paper from Bill.
@@ -330,6 +321,9 @@ AP_AHRS_DCM::drift_correction_yaw(void)
     float yaw_deltat;
 
     if (use_compass()) {
+        /*
+          we are using compass for yaw
+         */
         if (_compass->last_update != _compass_last_update) {
             yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6f;
             _compass_last_update = _compass->last_update;
@@ -346,17 +340,43 @@ AP_AHRS_DCM::drift_correction_yaw(void)
             yaw_error = yaw_error_compass();
         }
     } else if (_flags.fly_forward && have_gps()) {
+        /*
+          we are using GPS for yaw
+         */
         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-3f;
             _gps_last_update = _gps->last_fix_time;
-            if (!_flags.have_initial_yaw) {
-                _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01f));
+            new_value = true;
+            float gps_course_rad = ToRad(_gps->ground_course * 0.01f);
+            float yaw_error_rad = gps_course_rad - yaw;
+            yaw_error = sinf(yaw_error_rad);
+
+            /* reset yaw to match GPS heading under any of the
+               following 3 conditions:
+
+               1) if we have reached GPS_SPEED_MIN and have never had
+               yaw information before
+
+               2) if the last time we got yaw information from the GPS
+               is more than 20 seconds ago, which means we may have
+               suffered from considerable gyro drift
+
+               3) if we are over 3*GPS_SPEED_MIN (which means 9m/s)
+               and our yaw error is over 60 degrees, which means very
+               poor yaw. This can happen on bungee launch when the
+               operator pulls back the plane rapidly enough then on
+               release the GPS heading changes very rapidly
+            */
+            if (!_flags.have_initial_yaw || 
+                yaw_deltat > 20 ||
+                (_gps->ground_speed >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
+                // reset DCM matrix based on current yaw
+                _dcm_matrix.from_euler(roll, pitch, gps_course_rad);
                 _omega_yaw_P.zero();
                 _flags.have_initial_yaw = true;
+                yaw_error = 0;
             }
-            new_value = true;
-            yaw_error = yaw_error_gps();
         }
     }
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 286148c38e..09de106d5f 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -74,7 +74,6 @@ private:
     void            drift_correction(float deltat);
     void            drift_correction_yaw(void);
     float           yaw_error_compass();
-    float           yaw_error_gps();
     void            euler_angles(void);
     void            estimate_wind(Vector3f &velocity);
     bool            have_gps(void);