ekf2: remove const references

src/modules/ekf2/EKF/EKFGSF_yaw.cpp

@@ -264,13 +264,13 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 	// predict covariance - equations generated using EKF/python/gsf_ekf_yaw_estimator/main.py
 
 	// Local short variable name copies required for readability
-	const float &P00 = _ekf_gsf[model_index].P(0,0);
-	const float &P01 = _ekf_gsf[model_index].P(0,1);
-	const float &P02 = _ekf_gsf[model_index].P(0,2);
-	const float &P11 = _ekf_gsf[model_index].P(1,1);
-	const float &P12 = _ekf_gsf[model_index].P(1,2);
-	const float &P22 = _ekf_gsf[model_index].P(2,2);
-	const float &psi = _ekf_gsf[model_index].X(2);
+	const float P00 = _ekf_gsf[model_index].P(0,0);
+	const float P01 = _ekf_gsf[model_index].P(0,1);
+	const float P02 = _ekf_gsf[model_index].P(0,2);
+	const float P11 = _ekf_gsf[model_index].P(1,1);
+	const float P12 = _ekf_gsf[model_index].P(1,2);
+	const float P22 = _ekf_gsf[model_index].P(2,2);
+	const float psi = _ekf_gsf[model_index].X(2);
 
 	// Use fixed values for delta velocity and delta angle process noise variances
 	const float dvxVar = sq(_accel_noise * _delta_vel_dt); // variance of forward delta velocity - (m/s)^2
@@ -320,12 +320,12 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
 	_ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - _vel_NE(1);
 
 	// Use temporary variables for covariance elements to reduce verbosity of auto-code expressions
-	const float &P00 = _ekf_gsf[model_index].P(0,0);
-	const float &P01 = _ekf_gsf[model_index].P(0,1);
-	const float &P02 = _ekf_gsf[model_index].P(0,2);
-	const float &P11 = _ekf_gsf[model_index].P(1,1);
-	const float &P12 = _ekf_gsf[model_index].P(1,2);
-	const float &P22 = _ekf_gsf[model_index].P(2,2);
+	const float P00 = _ekf_gsf[model_index].P(0,0);
+	const float P01 = _ekf_gsf[model_index].P(0,1);
+	const float P02 = _ekf_gsf[model_index].P(0,2);
+	const float P11 = _ekf_gsf[model_index].P(1,1);
+	const float P12 = _ekf_gsf[model_index].P(1,2);
+	const float P22 = _ekf_gsf[model_index].P(2,2);
 
 	// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
 	const float t0 = ecl::powf(P01, 2);

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -47,11 +47,11 @@
 
 void Ekf::fuseAirspeed()
 {
-	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
+	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
 
 	// Variance for true airspeed measurement - (m/sec)^2
 	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *

src/modules/ekf2/EKF/covariance.cpp

@@ -100,26 +100,26 @@ void Ekf::initialiseCovariance()
 void Ekf::predictCovariance()
 {
 	// assign intermediate state variables
-	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);
+	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);
 
-	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);
+	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);
 
-	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);
+	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);
 
-	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);
+	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);
 
-	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);
+	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);
 
 	// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
 	const float dt = _dt_ekf_avg;

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -66,19 +66,19 @@ void Ekf::fuseDrag()
 	}
 
 	// get latest estimated orientation
-	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);
+	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
-	const float &vn = _state.vel(0);
-	const float &ve = _state.vel(1);
-	const 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
-	const float &vwn = _state.wind_vel(0);
-	const 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 rotate into body frame

src/modules/ekf2/EKF/ekf.h

@@ -636,27 +636,27 @@ private:
 
 	void resetVelocityToGps();
 
-	inline void resetHorizontalVelocityToOpticalFlow();
+	void resetHorizontalVelocityToOpticalFlow();
 
-	inline void resetVelocityToVision();
+	void resetVelocityToVision();
 
-	inline void resetHorizontalVelocityToZero();
+	void resetHorizontalVelocityToZero();
 
-	inline void resetVelocityTo(const Vector3f &vel);
+	void resetVelocityTo(const Vector3f &vel);
 
-	inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
+	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
 
-	inline void resetVerticalVelocityTo(float new_vert_vel);
+	void resetVerticalVelocityTo(float new_vert_vel);
 
 	void resetHorizontalPosition();
 
 	void resetHorizontalPositionToGps();
 
-	inline void resetHorizontalPositionToVision();
+	void resetHorizontalPositionToVision();
 
-	inline void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
+	void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
 
-	inline void resetVerticalPositionTo(const float &new_vert_pos);
+	void resetVerticalPositionTo(const float new_vert_pos);
 
 	void resetHeight();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -229,7 +229,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
 	_state_reset_status.posNE_counter++;
 }
 
-void Ekf::resetVerticalPositionTo(const float &new_vert_pos)
+void Ekf::resetVerticalPositionTo(const float new_vert_pos)
 {
 	const float old_vert_pos = _state.pos(2);
 	_state.pos(2) = new_vert_pos;

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -48,10 +48,10 @@
 void Ekf::fuseGpsYaw()
 {
 	// assign intermediate state variables
-	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);
+	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 observed yaw angle of antenna array, converting a from body to antenna yaw measurement
 	const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -48,14 +48,14 @@
 void Ekf::fuseMag(const Vector3f &mag)
 {
 	// assign intermediate variables
-	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);
+	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);
 
-	const float &magN = _state.mag_I(0);
-	const float &magE = _state.mag_I(1);
-	const 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
 	const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
@@ -420,10 +420,10 @@ void Ekf::fuseMag(const Vector3f &mag)
 void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 {
 	// assign intermediate state variables
-	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);
+	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);
 
 	const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
 	const float measurement = wrap_pi(yaw);
@@ -776,8 +776,8 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var)
 void Ekf::fuseDeclination(float decl_sigma)
 {
 	// assign intermediate state variables
-	const float &magN = _state.mag_I(0);
-	const float &magE = _state.mag_I(1);
+	const float magN = _state.mag_I(0);
+	const float magE = _state.mag_I(1);
 
 	// minimum North field strength before calculation becomes badly conditioned (T)
 	constexpr float N_field_min = 0.001f;

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -48,19 +48,19 @@
 void Ekf::fuseSideslip()
 {
 	// get latest estimated orientation
-	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);
+	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
-	const float &vn = _state.vel(0);
-	const float &ve = _state.vel(1);
-	const 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
-	const float &vwn = _state.wind_vel(0);
-	const float &vwe = _state.wind_vel(1);
+	const float vwn = _state.wind_vel(0);
+	const float vwe = _state.wind_vel(1);
 
 	// calculate relative wind velocity in earth frame and rotate into body frame
 	const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);