EKF: Adjust terrain process noise for gradient effect

EKF/common.h

@@ -166,6 +166,7 @@ struct parameters {
 	float mag_p_noise;		// process noise for magnetic field prediction (Guass/sec)
 	float wind_vel_p_noise;		// process noise for wind velocity prediction (m/sec/sec)
 	float terrain_p_noise;		// process noise for terrain offset (m/sec)
+	float terrain_gradient;		// gradient of terrain used to estimate process noise due to changing position (m/m)
 
 	// position and velocity fusion
 	float gps_vel_noise;		// observation noise for gps velocity fusion (m/sec)
@@ -234,6 +235,7 @@ struct parameters {
 		mag_p_noise = 2.5e-2f;
 		wind_vel_p_noise = 1.0e-1f;
 		terrain_p_noise = 0.5f;
+		terrain_gradient = 0.5f;
 
 		// position and velocity fusion
 		gps_vel_noise = 5.0e-1f;

EKF/terrain_estimator.cpp

@@ -73,8 +73,13 @@ void Ekf::predictHagl()
 {
 	// predict the state variance growth
 	// 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);
 }