Add missing const qualifier

EKF/airspeed_fusion.cpp

@@ -46,34 +46,28 @@
 
 void Ekf::fuseAirspeed()
 {
-	// Initialize variables
-	float vn; // Velocity in north direction
-	float ve; // Velocity in east direction
-	float vd; // Velocity in downwards direction
-	float vwn; // Wind speed in north direction
-	float vwe; // Wind speed in east direction
-	float v_tas_pred; // Predicted measurement
-	float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f,
-			 10.0f)); // Variance for true airspeed measurement - (m/sec)^2
 	float SH_TAS[3] = {}; // Variable used to optimise calculations of measurement jacobian
 	float H_TAS[24] = {}; // Observation Jacobian
 	float SK_TAS[2] = {}; // Variable used to optimise calculations of the Kalman gain vector
 	float Kfusion[24] = {}; // Kalman gain vector
 
-	// Copy required states to local variable names
-	vn = _state.vel(0);
-	ve = _state.vel(1);
-	vd = _state.vel(2);
-	vwn = _state.wind_vel(0);
-	vwe = _state.wind_vel(1);
+	const float vn = _state.vel(0); // Velocity in north direction
+	const float ve = _state.vel(1); // Velocity in east direction
+	const float vd = _state.vel(2); // Velocity in downwards direction
+	const float vwn = _state.wind_vel(0); // Wind speed in north direction
+	const float vwe = _state.wind_vel(1); // Wind speed in east direction
 
 	// Calculate the predicted airspeed
-	v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd);
+	const float v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd);
+
+	// Variance for true airspeed measurement - (m/sec)^2
+	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
+			    math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
 
 	// Perform fusion of True Airspeed measurement
 	if (v_tas_pred > 1.0f) {
 		// determine if we need the sideslip fusion to correct states other than wind
-		bool update_wind_only = !_is_wind_dead_reckoning;
+		const bool update_wind_only = !_is_wind_dead_reckoning;
 
 		// Calculate the observation jacobian
 		// intermediate variable from algebraic optimisation
@@ -90,7 +84,7 @@ void Ekf::fuseAirspeed()
 		H_TAS[23] = -SH_TAS[1];
 
 		// We don't want to update the innovation variance if the calculation is ill conditioned
-		float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
+		const float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
 
 		if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation
 			SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
@@ -256,7 +250,7 @@ void Ekf::resetWindStates()
 {
 	// get euler yaw angle
 	Eulerf euler321(_state.quat_nominal);
-	float euler_yaw = euler321(2);
+	const float euler_yaw = euler321(2);
 
 	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
 		// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
@@ -265,7 +259,6 @@ void Ekf::resetWindStates()
 
 	} else {
 		// If we don't have an airspeed measurement, then assume the wind is zero
-		_state.wind_vel(0) = 0.0f;
-		_state.wind_vel(1) = 0.0f;
+		_state.wind_vel.setZero();
 	}
 }

EKF/control.cpp

@@ -51,7 +51,7 @@ void Ekf::controlFusionModes()
 	// monitor the tilt alignment
 	if (!_control_status.flags.tilt_align) {
 		// whilst we are aligning the tilt, monitor the variances
-		Vector3f angle_err_var_vec = calcRotVecVariances();
+		const Vector3f angle_err_var_vec = calcRotVecVariances();
 
 		// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
 		// and declare the tilt alignment complete
@@ -100,7 +100,7 @@ void Ekf::controlFusionModes()
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
 		if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) &&_NED_origin_initialised) {
-			Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
+			const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
 			_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
 			_control_status.flags.synthetic_mag_z = true;
 		} else {
@@ -228,12 +228,11 @@ void Ekf::controlExternalVisionFusion()
 
 				// get initial yaw from the observation quaternion
 				const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
-				Quatf q_obs(ev_newest.quat);
-				Eulerf euler_obs(q_obs);
+				const Eulerf euler_obs(ev_newest.quat);
 				euler_init(2) = euler_obs(2);
 
 				// save a copy of the quaternion state for later use in calculating the amount of reset change
-				Quatf quat_before_reset = _state.quat_nominal;
+				const Quatf quat_before_reset = _state.quat_nominal;
 
 				// calculate initial quaternion states for the ekf
 				_state.quat_nominal = Quatf(euler_init);
@@ -280,11 +279,9 @@ void Ekf::controlExternalVisionFusion()
 			Vector2f ev_pos_innov_gates;
 
 			// correct position and height for offset relative to IMU
-			Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-			Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-			_ev_sample_delayed.pos(0) -= pos_offset_earth(0);
-			_ev_sample_delayed.pos(1) -= pos_offset_earth(1);
-			_ev_sample_delayed.pos(2) -= pos_offset_earth(2);
+			const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			_ev_sample_delayed.pos -= pos_offset_earth;
 
 			// Use an incremental position fusion method for EV position data if GPS is also used
 			if (_params.fusion_mode & MASK_USE_GPS) {
@@ -361,10 +358,10 @@ void Ekf::controlExternalVisionFusion()
 			}
 
 			// correct velocity for offset relative to IMU
-			Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
-			Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-			Vector3f vel_offset_body = ang_rate % pos_offset_body;
-			Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+			const Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
+			const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+			const Vector3f vel_offset_body = ang_rate % pos_offset_body;
+			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 			vel_aligned -= vel_offset_earth;
 
 			_ev_vel_innov(0) = _state.vel(0) - vel_aligned(0);
@@ -697,20 +694,19 @@ void Ekf::controlGpsFusion()
 			Vector3f gps_pos_obs_var;
 
 			// correct velocity for offset relative to IMU
-			Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
-			Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
-			Vector3f vel_offset_body = ang_rate % pos_offset_body;
-			Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+			const Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
+			const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
+			const Vector3f vel_offset_body = ang_rate % pos_offset_body;
+			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 			_gps_sample_delayed.vel -= vel_offset_earth;
 
 			// correct position and height for offset relative to IMU
-			Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_gps_sample_delayed.pos(0) -= pos_offset_earth(0);
 			_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
 			_gps_sample_delayed.hgt += pos_offset_earth(2);
 
-			// calculate observation process noise
-			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
+			const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 
 			if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) {
 				// if we are using other sources of aiding, then relax the upper observation
@@ -805,13 +801,13 @@ void Ekf::controlHeightSensorTimeouts()
 		if (_control_status.flags.baro_hgt) {
 			// check if GPS height is available
 			const gpsSample &gps_init = _gps_buffer.get_newest();
-			bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+			const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
 
 			const baroSample &baro_init = _baro_buffer.get_newest();
 			bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
 
 			// check for inertial sensing errors in the last 10 seconds
-			bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
+			const bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
 
 			// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
 			bool reset_to_gps = !_gps_hgt_intermittent && gps_hgt_accurate && !prev_bad_vert_accel;
@@ -856,13 +852,13 @@ void Ekf::controlHeightSensorTimeouts()
 		if (_control_status.flags.gps_hgt) {
 			// check if GPS height is available
 			const gpsSample &gps_init = _gps_buffer.get_newest();
-			bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+			const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
 
 			// check the baro height source for consistency and freshness
 			const baroSample &baro_init = _baro_buffer.get_newest();
-			bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
-			float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
-			bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9,9)) * sq(_params.baro_innov_gate);
+			const bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+			const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
+			const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9,9)) * sq(_params.baro_innov_gate);
 
 			// if baro data is acceptable and GPS data is inaccurate, reset height to baro
 			bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate;
@@ -904,7 +900,7 @@ void Ekf::controlHeightSensorTimeouts()
 
 			// check if baro data is available
 			const baroSample &baro_init = _baro_buffer.get_newest();
-			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+			const bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
 
 			// reset to baro if we have no range data and baro data is available
 			bool reset_to_baro = !_rng_hgt_valid && baro_data_available;
@@ -940,11 +936,11 @@ void Ekf::controlHeightSensorTimeouts()
 		if (_control_status.flags.ev_hgt) {
 			// check if vision data is available
 			const extVisionSample &ev_init = _ext_vision_buffer.get_newest();
-			bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
+			const bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
 
 			// check if baro data is available
 			const baroSample &baro_init = _baro_buffer.get_newest();
-			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+			const bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
 
 			// reset to baro if we have no vision data and baro data is available
 			bool reset_to_baro = !ev_data_available && baro_data_available;
@@ -1209,8 +1205,8 @@ void Ekf::controlHeightFusion()
 			_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
 			// observation variance - receiver defined and parameter limited
 			// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
-			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
-			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
+			const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
+			const float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
 			gps_hgt_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
 			// innovation gate size
 			gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
@@ -1289,8 +1285,8 @@ void Ekf::controlAirDataFusion()
 	// control activation and initialisation/reset of wind states required for airspeed fusion
 
 	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
-	bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
-	bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
+	const bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
+	const bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
 
 	if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
 		_control_status.flags.wind = false;
@@ -1332,8 +1328,8 @@ void Ekf::controlBetaFusion()
 	// control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion
 
 	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
-	bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
-	bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
+	const bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
+	const bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
 
 	if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
 		_control_status.flags.wind = false;

EKF/covariance.cpp

@@ -53,8 +53,7 @@ void Ekf::initialiseCovariance()
 	_delta_angle_bias_var_accum.setZero();
 	_delta_vel_bias_var_accum.setZero();
 
-	// calculate average prediction time step in sec
-	float dt = FILTER_UPDATE_PERIOD_S;
+	const float dt = FILTER_UPDATE_PERIOD_S;
 
 	// define the initial angle uncertainty as variances for a rotation vector
 	Vector3f rot_vec_var;
@@ -128,39 +127,39 @@ void Ekf::get_vel_var(Vector3f &vel_var)
 void Ekf::predictCovariance()
 {
 	// assign intermediate state variables
-	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);
 
-	float dax = _imu_sample_delayed.delta_ang(0);
-	float day = _imu_sample_delayed.delta_ang(1);
-	float daz = _imu_sample_delayed.delta_ang(2);
+	const float dax = _imu_sample_delayed.delta_ang(0);
+	const float day = _imu_sample_delayed.delta_ang(1);
+	const float daz = _imu_sample_delayed.delta_ang(2);
 
-	float dvx = _imu_sample_delayed.delta_vel(0);
-	float dvy = _imu_sample_delayed.delta_vel(1);
-	float dvz = _imu_sample_delayed.delta_vel(2);
+	const float dvx = _imu_sample_delayed.delta_vel(0);
+	const float dvy = _imu_sample_delayed.delta_vel(1);
+	const float dvz = _imu_sample_delayed.delta_vel(2);
 
-	float dax_b = _state.delta_ang_bias(0);
-	float day_b = _state.delta_ang_bias(1);
-	float daz_b = _state.delta_ang_bias(2);
+	const float dax_b = _state.delta_ang_bias(0);
+	const float day_b = _state.delta_ang_bias(1);
+	const float daz_b = _state.delta_ang_bias(2);
 
-	float dvx_b = _state.delta_vel_bias(0);
-	float dvy_b = _state.delta_vel_bias(1);
-	float dvz_b = _state.delta_vel_bias(2);
+	const float dvx_b = _state.delta_vel_bias(0);
+	const float dvy_b = _state.delta_vel_bias(1);
+	const float dvz_b = _state.delta_vel_bias(2);
 
-	float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
-	float dt_inv = 1.0f / dt;
+	const float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
+	const float dt_inv = 1.0f / dt;
 
 	// convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
-	float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
+	const float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
 
 	// convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update
-	float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
+	const float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
 
 	// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
-	float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
-	float beta = 1.0f - alpha;
+	const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
+	const float beta = 1.0f - alpha;
 	_ang_rate_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt);
 	_accel_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_magnitude_filt);
 	_accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt;

EKF/drag_fusion.cpp

@@ -51,30 +51,30 @@ void Ekf::fuseDrag()
 	float Kfusion[24] = {}; // Kalman gain vector
 	float R_ACC = _params.drag_noise; // observation noise variance in specific force drag (m/sec**2)**2
 
-	float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)
+	const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)
 
 	// calculate inverse of ballistic coefficient
 	if (_params.bcoef_x < 1.0f || _params.bcoef_y < 1.0f) {
 		return;
 	}
 
-	float BC_inv_x = 1.0f / _params.bcoef_x;
-	float BC_inv_y = 1.0f / _params.bcoef_y;
+	const float BC_inv_x = 1.0f / _params.bcoef_x;
+	const float BC_inv_y = 1.0f / _params.bcoef_y;
 
 	// 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);
 
 	// get latest velocity in earth frame
-	float vn = _state.vel(0);
-	float ve = _state.vel(1);
-	float vd = _state.vel(2);
+	const float vn = _state.vel(0);
+	const float ve = _state.vel(1);
+	const float vd = _state.vel(2);
 
 	// get latest wind velocity in earth frame
-	float vwn = _state.wind_vel(0);
-	float vwe = _state.wind_vel(1);
+	const float vwn = _state.wind_vel(0);
+	const float vwe = _state.wind_vel(1);
 
 	// predicted specific forces
 	// calculate relative wind velocity in earth frame and rotte into body frame
@@ -82,7 +82,7 @@ void Ekf::fuseDrag()
 	rel_wind(0) = vn - vwn;
 	rel_wind(1) = ve - vwe;
 	rel_wind(2) = vd;
-	Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
+	const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
 	rel_wind = earth_to_body * rel_wind;
 
 	// perform sequential fusion of XY specific forces
@@ -90,12 +90,12 @@ void Ekf::fuseDrag()
 		// calculate observation jacobiam and Kalman gain vectors
 		if (axis_index == 0) {
 			// Estimate the airspeed from the measured drag force and ballistic coefficient
-			float mea_acc = _drag_sample_delayed.accelXY(axis_index)  - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
-			float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho));
+			const float mea_acc = _drag_sample_delayed.accelXY(axis_index)  - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
+			const float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho));
 
 			// Estimate the derivative of specific force wrt airspeed along the X axis
 			// Limit lower value to prevent arithmetic exceptions
-			float Kacc = fmaxf(1e-1f, rho * BC_inv_x * airSpd);
+			const float Kacc = fmaxf(1e-1f, rho * BC_inv_x * airSpd);
 
 			SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
 			SH_ACC[1] = vn - vwn;
@@ -157,18 +157,18 @@ void Ekf::fuseDrag()
 				drag_sign = -1.0f;
 			}
 
-			float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
+			const float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
 			_drag_innov[axis_index] = predAccel - mea_acc;
 			_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
 
 		} else if (axis_index == 1) {
 			// Estimate the airspeed from the measured drag force and ballistic coefficient
-			float mea_acc = _drag_sample_delayed.accelXY(axis_index)  - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
-			float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho));
+			const float mea_acc = _drag_sample_delayed.accelXY(axis_index)  - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
+			const float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho));
 
 			// Estimate the derivative of specific force wrt airspeed along the X axis
 			// Limit lower value to prevent arithmetic exceptions
-			float Kacc = fmaxf(1e-1f, rho * BC_inv_y * airSpd);
+			const float Kacc = fmaxf(1e-1f, rho * BC_inv_y * airSpd);
 
 			SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
 			SH_ACC[1] = vn - vwn;
@@ -232,7 +232,7 @@ void Ekf::fuseDrag()
 				drag_sign = -1.0f;
 			}
 
-			float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
+			const float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
 			_drag_innov[axis_index] = predAccel - mea_acc;
 			_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
 

EKF/ekf.cpp

@@ -250,7 +250,7 @@ void Ekf::predictState()
 {
 	// apply imu bias corrections
 	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
-	Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
+	const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
 
 	// correct delta angles for earth rotation rate
 	corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
@@ -266,13 +266,13 @@ void Ekf::predictState()
 	_state.quat_nominal.normalize();
 
 	// save the previous value of velocity so we can use trapzoidal integration
-	Vector3f vel_last = _state.vel;
+	const Vector3f vel_last = _state.vel;
 
 	// update transformation matrix from body to world frame
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	// Calculate an earth frame delta velocity
-	Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
+	const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
 
 	// calculate a filtered horizontal acceleration with a 1 sec time constant
 	// this are used for manoeuvre detection elsewhere

EKF/ekf_helper.cpp

@@ -391,25 +391,26 @@ void Ekf::alignOutputFilter()
 // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only.
 bool Ekf::realignYawGPS()
 {
-	// Need at least 5 m/s of GPS horizontal speed and ratio of velocity error to velocity < 0.15  for a reliable alignment
-	float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
+	const float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
 
+	// Need at least 5 m/s of GPS horizontal speed and
+	// ratio of velocity error to velocity < 0.15  for a reliable alignment
 	if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) {
 		// check for excessive horizontal GPS velocity innovations
-		bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps;
+		const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps;
 
 		// calculate GPS course over ground angle
-		float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
+		const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
 
 		// calculate course yaw angle
-		float ekfGOG = atan2f(_state.vel(1), _state.vel(0));
+		const float ekfGOG = atan2f(_state.vel(1), _state.vel(0));
 
 		// Check the EKF and GPS course over ground for consistency
-		float courseYawError = gpsCOG - ekfGOG;
+		const float courseYawError = gpsCOG - ekfGOG;
 
 		// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
-		bool badYawErr = fabsf(courseYawError) > 0.5f;
-		bool badMagYaw = (badYawErr && badVelInnov);
+		const bool badYawErr = fabsf(courseYawError) > 0.5f;
+		const bool badMagYaw = (badYawErr && badVelInnov);
 
 		if (badMagYaw) {
 			_num_bad_flight_yaw_events ++;
@@ -426,7 +427,7 @@ bool Ekf::realignYawGPS()
 			}
 
 			// save a copy of the quaternion state for later use in calculating the amount of reset change
-			Quatf quat_before_reset = _state.quat_nominal;
+			const Quatf quat_before_reset = _state.quat_nominal;
 
 			// update transformation matrix from body to world frame using the current state estimate
 			_R_to_earth = Dcmf(_state.quat_nominal);
@@ -455,14 +456,12 @@ bool Ekf::realignYawGPS()
 
 			// calculate new filter quaternion states using corrected yaw angle
 			_state.quat_nominal = Quatf(euler321);
+			_R_to_earth = Dcmf(_state.quat_nominal);
 			uncorrelateQuatStates();
 
 			// If heading was bad, then we also need to reset the velocity and position states
 			_velpos_reset_request = badMagYaw;
 
-			// update transformation matrix from body to world frame using the current state estimate
-			_R_to_earth = Dcmf(_state.quat_nominal);
-
 			// Use the last magnetometer measurements to reset the field states
 			_state.mag_B.zero();
 			_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
@@ -551,7 +550,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 	}
 
 	// save a copy of the quaternion state for later use in calculating the amount of reset change
-	Quatf quat_before_reset = _state.quat_nominal;
+	const Quatf quat_before_reset = _state.quat_nominal;
 	Quatf quat_after_reset = _state.quat_nominal;
 
 	// update transformation matrix from body to world frame using the current estimate
@@ -572,13 +571,13 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 		// calculate the observed yaw angle
 		if (_control_status.flags.ev_yaw) {
 			// convert the observed quaternion to a rotation matrix
-			Dcmf R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
+			const Dcmf R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
 			// calculate the yaw angle for a 312 sequence
 			euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
 
 		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle
-			Vector3f mag_earth_pred = R_to_earth * mag_init;
+			const Vector3f mag_earth_pred = R_to_earth * mag_init;
 			// the angle of the projection onto the horizontal gives the yaw angle
 			euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
 
@@ -630,13 +629,13 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 		// calculate the observed yaw angle
 		if (_control_status.flags.ev_yaw) {
 			// convert the observed quaternion to a rotation matrix
-			Dcmf R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
+			const Dcmf R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
 			// calculate the yaw angle for a 312 sequence
 			euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
 
 		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle
-			Vector3f mag_earth_pred = R_to_earth * mag_init;
+			const Vector3f mag_earth_pred = R_to_earth * mag_init;
 			// the angle of the projection onto the horizontal gives the yaw angle
 			euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
 
@@ -668,7 +667,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 	}
 
 	// set the earth magnetic field states using the updated rotation
-	Dcmf R_to_earth_after(quat_after_reset);
+	const Dcmf R_to_earth_after(quat_after_reset);
 	_state.mag_I = R_to_earth_after * mag_init;
 
 	// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
@@ -1198,21 +1197,21 @@ hagl_max : Maximum height above ground (meters). NaN when limiting is not needed
 void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max)
 {
 	// Calculate range finder limits
-	float rangefinder_hagl_min = _rng_valid_min_val;
+	const float rangefinder_hagl_min = _rng_valid_min_val;
 	// Allow use of 75% of rangefinder maximum range to allow for angular motion
-	float rangefinder_hagl_max = 0.75f * _rng_valid_max_val;
+	const float rangefinder_hagl_max = 0.75f * _rng_valid_max_val;
 
 	// Calculate optical flow limits
 	// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
-	float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f);
-	float flow_hagl_min = _flow_min_distance;
-	float flow_hagl_max = _flow_max_distance;
+	const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f);
+	const float flow_hagl_min = _flow_min_distance;
+	const float flow_hagl_max = _flow_max_distance;
 
 	// TODO : calculate visual odometry limits
 
-	bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
+	const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
 
-	bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel);
+	const bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel);
 
 	// Do not require limiting by default
 	*vxy_max = NAN;
@@ -1649,8 +1648,8 @@ void Ekf::calcExtVisRotMat()
 	if (rot_vec_norm > 1e-6f) {
 
 		// apply an input limiter to protect from spikes
-		Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
-		float input_delta_len = _input_delta_vec.norm();
+		const Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
+		const float input_delta_len = _input_delta_vec.norm();
 
 		if (input_delta_len > 0.1f) {
 			rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len);

EKF/estimator_interface.cpp

@@ -385,10 +385,9 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 			flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate);
 		}
 
-		bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel;
+		const bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel;
 
-		// check quality metric
-		bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
+		const bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
 
 		// Check data validity and write to buffers
 		// Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion()
@@ -507,7 +506,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	// set the observation buffer length to handle the minimum time of arrival between observations in combination
 	// with the worst case delay from current time to ekf fusion time
 	// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
-	uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((float)max_time_delay_ms * 0.5f));
+	const uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((float)max_time_delay_ms * 0.5f));
 	_obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
 
 	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)

EKF/gps_checks.cpp

@@ -62,8 +62,8 @@ bool Ekf::collect_gps(const gps_message &gps)
 	_gps_checks_passed = gps_is_good(gps);
 	if (!_NED_origin_initialised && _gps_checks_passed) {
 		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-		double lat = gps.lat / 1.0e7;
-		double lon = gps.lon / 1.0e7;
+		const double lat = gps.lat / 1.0e7;
+		const double lon = gps.lon / 1.0e7;
 		map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
 
 		// if we are already doing aiding, correct for the change in position since the EKF started navigationg
@@ -137,12 +137,12 @@ bool Ekf::gps_is_good(const gps_message &gps)
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
 	const float filt_time_const = 10.0f;
-	float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const);
-	float filter_coef = dt / filt_time_const;
+	const float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const);
+	const float filter_coef = dt / filt_time_const;
 
 	// The following checks are only valid when the vehicle is at rest
-	double lat = gps.lat * 1.0e-7;
-	double lon = gps.lon * 1.0e-7;
+	const double lat = gps.lat * 1.0e-7;
+	const double lon = gps.lon * 1.0e-7;
 	if (!_control_status.flags.in_air && _vehicle_at_rest) {
 		// Calculate position movement since last measurement
 		float delta_pos_n = 0.0f;

EKF/gps_yaw_fusion.cpp

@@ -48,10 +48,10 @@
 void Ekf::fuseGpsAntYaw()
 {
 	// assign intermediate state variables
-	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);
 
 	float R_YAW = 1.0f;
 	float predicted_hdg;
@@ -295,24 +295,24 @@ bool Ekf::resetGpsAntYaw()
 	if (ISFINITE(_gps_sample_delayed.yaw)) {
 
 		// define the predicted antenna array vector and rotate into earth frame
-		Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
-		Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+		const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+		const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
 
 		// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
 		if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
 			return false;
 		}
 
-		float predicted_yaw =  atan2f(ant_vec_ef(1),ant_vec_ef(0));
+		const float predicted_yaw =  atan2f(ant_vec_ef(1),ant_vec_ef(0));
 
 		// get measurement and correct for antenna array yaw offset
-		float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
+		const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
 
 		// calculate the amount the yaw needs to be rotated by
 		float yaw_delta = wrap_pi(measured_yaw - predicted_yaw);
 
 		// save a copy of the quaternion state for later use in calculating the amount of reset change
-		Quatf quat_before_reset = _state.quat_nominal;
+		const Quatf quat_before_reset = _state.quat_nominal;
 		Quatf quat_after_reset = _state.quat_nominal;
 
 		// obtain the yaw angle using the best conditioned from either a Tait-Bryan 321 or 312 sequence

EKF/mag_fusion.cpp

@@ -47,18 +47,17 @@
 void Ekf::fuseMag()
 {
 	// assign intermediate variables
-	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);
 
-	float magN = _state.mag_I(0);
-	float magE = _state.mag_I(1);
-	float magD = _state.mag_I(2);
+	const float magN = _state.mag_I(0);
+	const float magE = _state.mag_I(1);
+	const float magD = _state.mag_I(2);
 
 	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
-	float R_MAG = fmaxf(_params.mag_noise, 0.0f);
-	R_MAG = R_MAG * R_MAG;
+	const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
 
 	// intermediate variables from algebraic optimisation
 	float SH_MAG[9];
@@ -73,9 +72,9 @@ void Ekf::fuseMag()
 	SH_MAG[8] = 2.0f*magE*q3;
 
 	// rotate magnetometer earth field state into body frame
-	Dcmf R_to_body = quat_to_invrotmat(_state.quat_nominal);
+	const Dcmf R_to_body = quat_to_invrotmat(_state.quat_nominal);
 
-	Vector3f mag_I_rot = R_to_body * _state.mag_I;
+	const Vector3f mag_I_rot = R_to_body * _state.mag_I;
 
 	// compute magnetometer innovations
 	_mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0);
@@ -437,10 +436,10 @@ void Ekf::fuseMag()
 void Ekf::fuseHeading()
 {
 	// assign intermediate state variables
-	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);
 
 	float R_YAW = 1.0f;
 	float predicted_hdg;
@@ -488,7 +487,7 @@ void Ekf::fuseHeading()
 		if (_control_status.flags.mag_hdg) {
 			// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
 			euler321(2) = 0.0f;
-			Dcmf R_to_earth(euler321);
+			const Dcmf R_to_earth(euler321);
 
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle
 			if (_control_status.flags.mag_3D) {
@@ -505,8 +504,8 @@ void Ekf::fuseHeading()
 		} else if (_control_status.flags.ev_yaw) {
 			// calculate the yaw angle for a 321 sequence
 			// Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
-			float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
-			float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
+			const float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
+			const float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
 			measured_hdg = atan2f(Tbn_1_0,Tbn_0_0);
 
 		} else if (_mag_use_inhibit) {
@@ -561,8 +560,8 @@ void Ekf::fuseHeading()
 		[                               -cos(roll)*sin(pitch),           sin(roll),                                cos(pitch)*cos(roll)]
 		*/
 		float yaw = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
-		float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll)
-		float pitch = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
+		const float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll)
+		const float pitch = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
 
 		predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation
 
@@ -813,14 +812,14 @@ void Ekf::fuseHeading()
 void Ekf::fuseDeclination(float decl_sigma)
 {
 	// assign intermediate state variables
-	float magN = _state.mag_I(0);
-	float magE = _state.mag_I(1);
+	const float magN = _state.mag_I(0);
+	const float magE = _state.mag_I(1);
 
 	// minimum horizontal field strength before calculation becomes badly conditioned (T)
-	float h_field_min = 0.001f;
+	const float h_field_min = 0.001f;
 
 	// observation variance (rad**2)
-	float R_DECL = sq(decl_sigma);
+	const float R_DECL = sq(decl_sigma);
 
 	// Calculate intermediate variables
 	float t2 = magE*magE;
@@ -889,9 +888,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 	Kfusion[22] = -t4*t13*(P(22,16)*magE-P(22,17)*magN);
 	Kfusion[23] = -t4*t13*(P(23,16)*magE-P(23,17)*magN);
 
-	// calculate innovation and constrain
-	float innovation = atan2f(magE, magN) - getMagDeclination();
-	innovation = math::constrain(innovation, -0.5f, 0.5f);
+	const float innovation = math::constrain(atan2f(magE, magN) - getMagDeclination(), -0.5f, 0.5f);
 
 	// apply covariance correction via P_new = (I -K*H)*P
 	// first calculate expression for KHP
@@ -1004,7 +1001,7 @@ float Ekf::calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag
 {
 	// theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge
 	// of the earth magnetic field vector at the current location
-	float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f));
+	const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f));
 
 	// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetomer Z component
 	float mag_z_body_pred = _R_to_earth(0,2) * mag_earth_predicted(0) + _R_to_earth(1,2) * mag_earth_predicted(1) + _R_to_earth(2,2) * mag_earth_predicted(2);

EKF/optflow_fusion.cpp

@@ -50,43 +50,43 @@ void Ekf::fuseOptFlow()
 	float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f);
 
 	// 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);
 
 	// get latest velocity in earth frame
-	float vn = _state.vel(0);
-	float ve = _state.vel(1);
-	float vd = _state.vel(2);
+	const float vn = _state.vel(0);
+	const float ve = _state.vel(1);
+	const float vd = _state.vel(2);
 
 	// calculate the optical flow observation variance
-	float R_LOS = calcOptFlowMeasVar();
+	const float R_LOS = calcOptFlowMeasVar();
 
 	float H_LOS[2][24] = {}; // Optical flow observation Jacobians
 	float Kfusion[24][2] = {}; // Optical flow Kalman gains
 
 	// 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;
 
 	// height above ground of the IMU
 	float heightAboveGndEst = _terrain_vpos - _state.pos(2);
 
 	// calculate the sensor position relative to the IMU in earth frame
-	Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
@@ -96,7 +96,7 @@ void Ekf::fuseOptFlow()
 	heightAboveGndEst = math::max(heightAboveGndEst, gndclearance);
 
 	// calculate range from focal point to centre of image
-	float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view
+	const float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view
 
 	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
 	// correct for gyro bias errors in the data used to do the motion compensation
@@ -522,20 +522,17 @@ bool Ekf::calcOptFlowBodyRateComp()
 		return false;
 	}
 
-	bool use_flow_sensor_gyro =  ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2));
+	const bool use_flow_sensor_gyro =  ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2));
 
 	if (use_flow_sensor_gyro) {
 
 		// if accumulation time differences are not excessive and accumulation time is adequate
 		// compare the optical flow and and navigation rate data and calculate a bias error
 		if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) {
-			// calculate a reference angular rate
-			Vector3f reference_body_rate;
-			reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
 
-			// calculate the optical flow sensor measured body rate
-			Vector3f of_body_rate;
-			of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
+			const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of));
+
+			const Vector3f measured_body_rate(_flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt));
 
 			// calculate the bias estimate using  a combined LPF and spike filter
 			_flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
@@ -563,8 +560,8 @@ bool Ekf::calcOptFlowBodyRateComp()
 float Ekf::calcOptFlowMeasVar()
 {
 	// calculate the observation noise variance - scaling noise linearly across flow quality range
-	float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
-	float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
+	const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
+	const float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
 
 	// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
 	float weighting = (255.0f - (float)_params.flow_qual_min);
@@ -578,7 +575,7 @@ float Ekf::calcOptFlowMeasVar()
 	}
 
 	// take the weighted average of the observation noise for the best and wort flow quality
-	float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
+	const float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
 
 	return R_LOS;
 }

EKF/sideslip_fusion.cpp

@@ -53,19 +53,19 @@ void Ekf::fuseSideslip()
 	float R_BETA = _params.beta_noise;
 
 	// 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);
 
 	// get latest velocity in earth frame
-	float vn = _state.vel(0);
-	float ve = _state.vel(1);
-	float vd = _state.vel(2);
+	const float vn = _state.vel(0);
+	const float ve = _state.vel(1);
+	const float vd = _state.vel(2);
 
 	// get latest wind velocity in earth frame
-	float vwn = _state.wind_vel(0);
-	float vwe = _state.wind_vel(1);
+	const float vwn = _state.wind_vel(0);
+	const float vwe = _state.wind_vel(1);
 
 	// relative wind velocity in earth frame
 	Vector3f rel_wind;
@@ -73,7 +73,7 @@ void Ekf::fuseSideslip()
 	rel_wind(1) = ve - vwe;
 	rel_wind(2) = vd;
 
-	Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
+	const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
 
 	// rotate into body axes
 	rel_wind = earth_to_body * rel_wind;

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));

EKF/vel_pos_fusion.cpp

@@ -54,7 +54,7 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga
 	test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
 			       sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
 
-	bool innov_check_pass = (test_ratio(0) <= 1.0f);
+	const bool innov_check_pass = (test_ratio(0) <= 1.0f);
 	if (innov_check_pass)
 	{
 		_time_last_hor_vel_fuse = _time_last_imu;
@@ -76,7 +76,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate
 	innov_var(2) = P(6,6) + obs_var(2);
 	test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
 
-	bool innov_check_pass = (test_ratio(1) <= 1.0f);
+	const bool innov_check_pass = (test_ratio(1) <= 1.0f);
 	if (innov_check_pass) {
 		_time_last_ver_vel_fuse = _time_last_imu;
 		_innov_check_fail_status.flags.reject_ver_vel = false;
@@ -124,7 +124,7 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
 	innov_var(2) = P(9,9) + obs_var(2);
 	test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
 
-	bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align;
+	const bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align;
 	if (innov_check_pass) {
 		_time_last_hgt_fuse = _time_last_imu;
 		_innov_check_fail_status.flags.reject_ver_pos = false;
@@ -142,7 +142,7 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
 void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
 {
 	float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used.
-	unsigned state_index = obs_index + 4;	// we start with vx and this is the 4. state
+	const unsigned state_index = obs_index + 4;	// we start with vx and this is the 4. state
 
 	// calculate kalman gain K = PHS, where S = 1/innovation variance
 	for (int row = 0; row < _k_num_states; row++) {