diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp index c02fa64763..afe1abcba4 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/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); diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index e6df5135f1..6bec030e7e 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/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) * diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index e1dbec9c4b..a87a9cf91f 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 6435faa69b..27b4f37b19 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b14552ddf6..e4f491fed1 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 397ea3a44e..738caf40d5 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 317744da17..6250272967 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/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); diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index f2ffd2b0d5..f7e944a606 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 8179413de0..69dae18ca9 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/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);