diff --git a/libraries/AP_NavEKF/NavEKF.cpp b/libraries/AP_NavEKF/NavEKF.cpp
index ac6bb0cfb6..a7d05540d1 100644
--- a/libraries/AP_NavEKF/NavEKF.cpp
+++ b/libraries/AP_NavEKF/NavEKF.cpp
@@ -118,18 +118,22 @@ void SelectVelPosFusion()
 			fuseVelData = false;
 			fusePosData = false;
 		}
-	// Fuse height measurements with GPS measurements for efficiency
-	// Don't wait longer than HGTmsecTgt msec between height fusion steps
+	// Fuse height measurements at the same time as GPS measurements for efficiency
+	// Don't wait longer than HGTmsecTgt msec between height updates
 	if (statesInitialised && (newDataGps || ((IMUmsec - HGTmsecPrev) >= HGTmsecTgt)))
 	{
+		HGTmsecPrev = IMUmsec;
 		fuseHgtData = true;
 		readHgtData();
 	}
 	else
 	{
 		fuseHgtData = false;
+		// protect against wrap-around
+		if(IMUmsec < HGTmsecPrev) HGTmsecPrev = IMUmsec;
+
 	}
-	FuseVelposNED();
+	FuseVelPosNED();
 	fuseHgtData = false;
 }
 
@@ -1050,7 +1054,7 @@ void CovariancePrediction()
 		zeroCols(nextP,16,17);
 	}
 
-	// If the total position variance exceds 1E6 (1000m), then stop covariance
+	// If the total position variance exceeds 1E6 (1000m), then stop covariance
 	// growth by setting the predicted to the previous values
 	// This prevent an ill conditioned matrix from occurring for long periods
 	// without GPS
@@ -1079,7 +1083,7 @@ void CovariancePrediction()
 	}
 }
 
-void FuseVelposNED()
+void FuseVelPosNED()
 {
 
 // declare variables used by fault isolation logic
diff --git a/libraries/AP_NavEKF/NavEKF.h b/libraries/AP_NavEKF/NavEKF.h
index 2fd683da15..977ae61b2e 100644
--- a/libraries/AP_NavEKF/NavEKF.h
+++ b/libraries/AP_NavEKF/NavEKF.h
@@ -59,11 +59,11 @@ public:
 
 private:
 
-void  UpdateStrapdownEquationsNED();
+void UpdateStrapdownEquationsNED();
 
 void CovariancePrediction();
 
-void FuseVelposNED();
+void FuseVelPosNED();
 
 void FuseMagnetometer();