EKF: enable scaling of range observation variance with height

EKF/common.h

@@ -235,6 +235,7 @@ struct parameters {
 	float range_innov_gate;		// range finder fusion innovation consistency gate size (STD)
 	float rng_gnd_clearance;	// minimum valid value for range when on ground (m)
 	float rng_sens_pitch;		// Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
+	float range_noise_scaler;	// scaling from range measurement to noise (m/m)
 
 	// vision position fusion
 	float ev_innov_gate;		// vision estimator fusion innovation consistency gate size (STD)
@@ -339,6 +340,7 @@ struct parameters {
 		range_innov_gate = 5.0f;
 		rng_gnd_clearance = 0.1f;
 		rng_sens_pitch = 0.0f;
+		range_noise_scaler = 0.0f;
 
 		// vision position fusion
 		ev_innov_gate = 5.0f;

EKF/terrain_estimator.cpp

@@ -99,7 +99,7 @@ void Ekf::fuseHagl()
 		_hagl_innov = pred_hagl - meas_hagl;
 
 		// calculate the observation variance adding the variance of the vehicles own height uncertainty and factoring in the effect of tilt on measurement error
-		float obs_variance = fmaxf(P[9][9], 0.0f) + sq(_params.range_noise / _R_rng_to_earth_2_2);
+		float obs_variance = fmaxf(P[9][9], 0.0f) + (sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * _R_rng_to_earth_2_2;
 
 		// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
 		_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);

EKF/vel_pos_fusion.cpp

@@ -161,8 +161,7 @@ void Ekf::fuseVelPosHeight()
 			_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
 							     _params.rng_gnd_clearance));
 			// observation variance - user parameter defined
-			R[5] = fmaxf(_params.range_noise, 0.01f);
-			R[5] = R[5] * R[5];
+			R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
 			// innovation gate size
 			gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
 		} else if (_control_status.flags.ev_hgt) {