diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp index bbb0473c0b..e4467a225f 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp @@ -261,13 +261,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 @@ -317,12 +317,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 d93033693c..4b7a38797b 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 f1b7478bd9..9aa4238fda 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 &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 &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 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 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 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 47cb2d7bbb..8a89803f75 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -66,19 +66,19 @@ void Ekf::fuseDrag(const dragSample &drag_sample) } // 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/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 5922d3211e..7501082f6c 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)); @@ -424,10 +424,10 @@ void Ekf::fuseMag(const Vector3f &mag) bool 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); @@ -779,8 +779,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 830efa3872..25a610a4ce 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);