diff --git a/EKF/control.cpp b/EKF/control.cpp
index 61018c4ae0..e208706666 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -124,7 +124,7 @@ void Ekf::controlFusionModes()
 	}
 
 	// check if we should fuse flow data for terrain estimation
-	if (!_flow_for_terrain_data_ready && _flow_data_ready) {
+	if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
 		// only fuse flow for terrain if range data hasn't been fused for 5 seconds
 		_flow_for_terrain_data_ready = (_time_last_imu - _time_last_hagl_fuse) > 5 * 1000 * 1000;
 		// only fuse flow for terrain if the main filter is not fusing flow and we are using gps
diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp
index 6d759aca33..0804dee322 100644
--- a/EKF/terrain_estimator.cpp
+++ b/EKF/terrain_estimator.cpp
@@ -56,7 +56,8 @@ bool Ekf::initHagl()
 		// success
 		return true;
 	} else if (_flow_for_terrain_data_ready) {
-		_terrain_vpos = _state.pos(2);
+		// initialise terrain vertical position to origin as this is the best guess we have
+		_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
 		_terrain_var = 100.0f;
 		return true;
 	} else if (!_control_status.flags.in_air) {
@@ -214,6 +215,9 @@ void Ekf::fuseFlowForTerrain()
 	// Calculate observation matrix for flow around the vehicle x axis
 	float Hx = vel_body(1) * t0 /(pred_hagl * pred_hagl);
 
+	// Constrain terrain variance to be non-negative
+	_terrain_var = fmaxf(_terrain_var, 0.0f);
+
 	// Cacluate innovation variance
 	_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;