Add missing const qualifier

EKF/terrain_estimator.cpp

@@ -135,16 +135,16 @@ void Ekf::fuseHagl()
 	// If the vehicle is excessively tilted, do not try to fuse range finder observations
 	if (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
 		// get a height above ground measurement from the range finder assuming a flat earth
-		float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2;
+		const float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2;
 
 		// predict the hagl from the vehicle position and terrain height
-		float pred_hagl = _terrain_vpos - _state.pos(2);
+		const float pred_hagl = _terrain_vpos - _state.pos(2);
 
 		// calculate the innovation
 		_hagl_innov = pred_hagl - meas_hagl;
 
 		// calculate the observation variance adding the variance of the vehicles own height uncertainty
-		float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
+		const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
 				     + sq(_params.range_noise)
 				     + sq(_params.range_noise_scaler * _range_sample_delayed.rng);
 
@@ -152,7 +152,7 @@ void Ekf::fuseHagl()
 		_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
 
 		// perform an innovation consistency check and only fuse data if it passes
-		float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
+		const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
 		_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
 
 		if (_hagl_test_ratio <= 1.0f) {
@@ -190,38 +190,38 @@ void Ekf::fuseFlowForTerrain()
 	const Vector2f opt_flow_rate = Vector2f{_flowRadXYcomp} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias};
 
 	// get latest estimated orientation
-	float q0 = _state.quat_nominal(0);
-	float q1 = _state.quat_nominal(1);
-	float q2 = _state.quat_nominal(2);
-	float q3 = _state.quat_nominal(3);
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);
 
 	// calculate the optical flow observation variance
-	float R_LOS = calcOptFlowMeasVar();
+	const float R_LOS = calcOptFlowMeasVar();
 
 	// get rotation matrix from earth to body
-	Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
+	const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
 
 	// calculate the sensor position relative to the IMU
-	Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
+	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
 
 	// calculate the velocity of the sensor relative to the imu in body frame
 	// Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign
-	Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body;
+	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body;
 
 	// calculate the velocity of the sensor in the earth frame
-	Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
+	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
 
 	// rotate into body frame
-	Vector3f vel_body = earth_to_body * vel_rel_earth;
+	const Vector3f vel_body = earth_to_body * vel_rel_earth;
 
-	float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
+	const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
 
 	// constrain terrain to minimum allowed value and predict height above ground
 	_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
-	float pred_hagl = _terrain_vpos - _state.pos(2);
+	const float pred_hagl = _terrain_vpos - _state.pos(2);
 
 	// Calculate observation matrix for flow around the vehicle x axis
-	float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl);
+	const float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl);
 
 	// Constrain terrain variance to be non-negative
 	_terrain_var = fmaxf(_terrain_var, 0.0f);
@@ -230,19 +230,19 @@ void Ekf::fuseFlowForTerrain()
 	_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;
 
 	// calculate the kalman gain for the flow x measurement
-	float Kx = _terrain_var * Hx / _flow_innov_var[0];
+	const float Kx = _terrain_var * Hx / _flow_innov_var[0];
 
 	// calculate prediced optical flow about x axis
-	float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl;
+	const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl;
 
 	// calculate flow innovation (x axis)
 	_flow_innov[0] = pred_flow_x - opt_flow_rate(0);
 
 	// calculate correction term for terrain variance
-	float KxHxP =  Kx * Hx * _terrain_var;
+	const float KxHxP =  Kx * Hx * _terrain_var;
 
 	// innovation consistency check
-	float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
+	const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
 	float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]);
 
 	// do not perform measurement update if badly conditioned
@@ -254,22 +254,22 @@ void Ekf::fuseFlowForTerrain()
 	}
 
 	// Calculate observation matrix for flow around the vehicle y axis
-	float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl);
+	const float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl);
 
 	// Calculuate innovation variance
 	_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;
 
 	// calculate the kalman gain for the flow y measurement
-	float Ky = _terrain_var * Hy / _flow_innov_var[1];
+	const float Ky = _terrain_var * Hy / _flow_innov_var[1];
 
 	// calculate prediced optical flow about y axis
-	float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl;
+	const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl;
 
 	// calculate flow innovation (y axis)
 	_flow_innov[1] = pred_flow_y - opt_flow_rate(1);
 
 	// calculate correction term for terrain variance
-	float KyHyP =  Ky * Hy * _terrain_var;
+	const float KyHyP =  Ky * Hy * _terrain_var;
 
 	// innovation consistency check
 	flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]);
@@ -290,11 +290,11 @@ bool Ekf::isTerrainEstimateValid() const
 void Ekf::updateTerrainValidity()
 {
 	// we have been fusing range finder measurements in the last 5 seconds
-	bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6;
+	const bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6;
 
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
 	// this can only be the case if the main filter does not fuse optical flow
-	bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < (uint64_t)5e6)
+	const bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < (uint64_t)5e6)
 					      && !_control_status.flags.opt_flow;
 
 	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));