diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp
index 8b1b0d2800..200c507d97 100644
--- a/EKF/estimator_interface.cpp
+++ b/EKF/estimator_interface.cpp
@@ -221,7 +221,11 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 		gps_sample_new.hgt = (float)gps->alt * 1e-3f;
 
 		gps_sample_new.yaw = gps->yaw;
-		_gps_yaw_offset = gps->yaw_offset;
+		if (ISFINITE(gps->yaw_offset)) {
+			_gps_yaw_offset = gps->yaw_offset;
+		} else {
+			_gps_yaw_offset = 0.0f;
+		}
 
 		// Only calculate the relative position if the WGS-84 location of the origin is set
 		if (collect_gps(time_usec, gps)) {