diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index 7782037644..6f41e86d90 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -79,7 +79,7 @@ void EKFGSF_yaw::update(const imuSample& imu_sample, if (!bad_update) { float total_weight = 0.0f; // calculate weighting for each model assuming a normal distribution - const float min_weight = 1E-5f; + const float min_weight = 1e-5f; uint8_t n_weight_clips = 0; for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { _model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index); diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index c909443dca..b25e348863 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -890,12 +890,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) for (int i = 4; i <= 6; i++) { // NED velocity states - P(i,i) = math::constrain(P(i,i), 1E-6f, P_lim[1]); + P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[1]); } for (int i = 7; i <= 9; i++) { // NED position states - P(i,i) = math::constrain(P(i,i), 1E-6f, P_lim[2]); + P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[2]); } for (int i = 10; i <= 12; i++) { diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index dc4212e021..add82b9dd5 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -436,10 +436,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) const float SA2 = SA0*q0 + SA1*q1; const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); float SA4, SA5_inv; - if (sq(SA3) > 1E-6f) { + if (sq(SA3) > 1e-6f) { SA4 = 1.0F/sq(SA3); SA5_inv = sq(SA2)*SA4 + 1; - canUseA = fabsf(SA5_inv) > 1E-6f; + canUseA = fabsf(SA5_inv) > 1e-6f; } bool canUseB = false; @@ -448,10 +448,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) const float SB2 = SB0*q3 + SB1*q2; const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); float SB3, SB5_inv; - if (sq(SB2) > 1E-6f) { + if (sq(SB2) > 1e-6f) { SB3 = 1.0F/sq(SB2); SB5_inv = SB3*sq(SB4) + 1; - canUseB = fabsf(SB5_inv) > 1E-6f; + canUseB = fabsf(SB5_inv) > 1e-6f; } Vector4f H_YAW; @@ -516,10 +516,10 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) const float SA2 = SA0*q0 - SA1*q1; const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3); float SA4, SA5_inv; - if (sq(SA3) > 1E-6f) { + if (sq(SA3) > 1e-6f) { SA4 = 1.0F/sq(SA3); SA5_inv = sq(SA2)*SA4 + 1; - canUseA = fabsf(SA5_inv) > 1E-6f; + canUseA = fabsf(SA5_inv) > 1e-6f; } bool canUseB = false; @@ -528,10 +528,10 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) const float SB2 = -SB0*q3 + SB1*q2; const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3); float SB3, SB5_inv; - if (sq(SB2) > 1E-6f) { + if (sq(SB2) > 1e-6f) { SB3 = 1.0F/sq(SB2); SB5_inv = SB3*sq(SB4) + 1; - canUseB = fabsf(SB5_inv) > 1E-6f; + canUseB = fabsf(SB5_inv) > 1e-6f; } Vector4f H_YAW; diff --git a/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp b/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp index 5058e1c0dd..10136b2698 100644 --- a/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp +++ b/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp @@ -210,7 +210,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Magnetomer X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Magnetomer X axis Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -235,7 +235,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Magnetomer X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Magnetomer X axis Kfusion max diff fraction = %e\n",max_diff_fraction); @@ -387,7 +387,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Magnetomer Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Magnetomer Y axis Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -412,7 +412,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Magnetomer Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Magnetomer Y axis Kfusion max diff fraction = %e\n",max_diff_fraction); @@ -567,7 +567,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Magnetomer Z axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Magnetomer Z axis Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -592,7 +592,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Magnetomer Z axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Magnetomer Z axis Kfusion max diff fraction = %e\n",max_diff_fraction); diff --git a/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp b/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp index b05bf2d79f..523bd90654 100644 --- a/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp +++ b/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp @@ -237,7 +237,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Specific Force X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Specific Force X axis Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -263,7 +263,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Specific Force X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Specific Force X axis Kfusion max diff fraction = %e\n",max_diff_fraction); @@ -439,7 +439,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Specific Force Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Specific Force Y axis Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -464,7 +464,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Specific Force Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Specific Force Y axis Kfusion max diff fraction = %e\n",max_diff_fraction); diff --git a/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp b/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp index 80d97ac791..50940ba03b 100644 --- a/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp +++ b/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp @@ -298,7 +298,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Sideslip Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Sideslip Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -323,7 +323,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Sideslip Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Sideslip Kfusion max diff fraction = %e\n",max_diff_fraction); diff --git a/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp b/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp index 77104c9e9f..227a4a5422 100644 --- a/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp +++ b/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp @@ -364,7 +364,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Optical Flow X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Optical Flow X axis Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -389,7 +389,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Optical Flow X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Optical Flow X axis Kfusion max diff fraction = %e\n",max_diff_fraction); @@ -603,7 +603,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Optical Flow Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Optical Flow Y axis Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -628,7 +628,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Optical Flow Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Optical Flow Y axis Kfusion max diff fraction = %e\n",max_diff_fraction); diff --git a/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp b/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp index 6a2f089d98..11c4a3d547 100644 --- a/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp +++ b/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp @@ -231,7 +231,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: GPS yaw Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: GPS yaw Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -256,7 +256,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: GPS yaw Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: GPS yaw Kfusion max diff fraction = %e\n",max_diff_fraction); diff --git a/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp b/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp index 29d76e0e7a..6972e1ad1f 100644 --- a/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp +++ b/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp @@ -192,7 +192,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Mag Declination Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Mag Declination Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -217,7 +217,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Mag Declination Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Mag Declination Kfusion max diff fraction = %e\n",max_diff_fraction); diff --git a/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp b/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp index 7f9e1e50c9..5cc31b84fe 100644 --- a/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp +++ b/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp @@ -189,7 +189,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -217,7 +217,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction); diff --git a/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp b/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp index 4e168dc6ef..4be87f85d9 100644 --- a/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp +++ b/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp @@ -110,7 +110,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: 321 yaw option A Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: 321 yaw option A Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -165,7 +165,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: 321 yaw option B Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: 321 yaw option B Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -250,7 +250,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: 312 yaw option A Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: 312 yaw option A Hfusion max diff fraction = %e\n",max_diff_fraction); @@ -304,7 +304,7 @@ int main() } } - if (max_diff_fraction > 1E-5f) { + if (max_diff_fraction > 1e-5f) { printf("Fail: 312 yaw option B Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); } else { printf("Pass: 312 yaw option B Hfusion max diff fraction = %e\n",max_diff_fraction);