From e79ccf1fccbdcf29cd7eeab588dbb56af4982a50 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 10 Apr 2015 18:02:26 +1000
Subject: [PATCH] AP_NavEKF: Fix bug allowing terrain to be above vehicle
 position

The terrain state and vehicle state need to be compared at the same time horizon.
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 442f4b3d08..0444f67886 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3938,6 +3938,8 @@ void NavEKF::ConstrainStates()
     for (uint8_t i=16; i<=18; i++) states[i] = constrain_float(states[i],-1.0f,1.0f);
     // body magnetic field limit
     for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f);
+    // constrain the terrain offset state
+    terrainState = max(terrainState, state.position.z + RNG_MEAS_ON_GND);
 }
 
 // update IMU delta angle and delta velocity measurements