EKF: Clean up terrain estimation functionality.

This is a functionally equivalent. It moves all of the code for the terrain estimator into a single function call from the main filter update, making it clear that it is independent of the main filter.

EKF/control.cpp

@@ -744,12 +744,6 @@ void Ekf::controlRangeFinderFusion()
 		Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 		_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
 
-		// always fuse available range finder data into a terrain height estimator if the estimator has been initialised
-		if (_terrain_initialised) {
-			fuseHagl();
-
-		}
-
 		// only use range finder as a height observation in the main filter if specifically enabled
 		if (_control_status.flags.rng_hgt) {
 			_fuse_height = true;

EKF/ekf.cpp

@@ -214,14 +214,8 @@ bool Ekf::update()
 		predictState();
 		predictCovariance();
 
-		// perform state and variance prediction for the terrain estimator
-		checkRangeDataContinuity();
-		if (!_terrain_initialised) {
-			_terrain_initialised = initHagl();
-
-		} else {
-			predictHagl();
-		}
+		// run a separate filter for terrain estimation
+		runTerrainEstimator();
 
 		// control fusion of observation data
 		controlFusionModes();

EKF/ekf.h

@@ -376,8 +376,8 @@ private:
 	// return true if the initialisation is successful
 	bool initHagl();
 
-	// predict the terrain vertical position state and variance
-	void predictHagl();
+	// run the terrain estimator
+	void runTerrainEstimator();
 
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	void fuseHagl();

EKF/terrain_estimator.cpp

@@ -70,20 +70,35 @@ bool Ekf::initHagl()
 	}
 }
 
-void Ekf::predictHagl()
+void Ekf::runTerrainEstimator()
 {
-	// predict the state variance growth
-	// the state is the vertical position of the terrain underneath the vehicle
+	// Perform a continuity check on range finder data
+	checkRangeDataContinuity();
 
-	// process noise due to errors in vehicle height estimate
-	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
+	// Perform initialisation check
+	if (!_terrain_initialised) {
+		_terrain_initialised = initHagl();
 
-	// process noise due to terrain gradient
-	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
-				1)));
+	} else {
 
-	// limit the variance to prevent it becoming badly conditioned
-	_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
+		// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
+
+		// process noise due to errors in vehicle height estimate
+		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
+
+		// process noise due to terrain gradient
+		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
+					1)));
+
+		// limit the variance to prevent it becoming badly conditioned
+		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
+
+		// Fuse range finder data if available
+		if (_range_data_ready) {
+			fuseHagl();
+
+		}
+	}
 }
 
 void Ekf::fuseHagl()