Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

Commit

Permalink
EKF: Code format fix - replace 1E-x with 1e-x
Browse files Browse the repository at this point in the history
  • Loading branch information
@priseborough
priseborough committed Oct 6, 2020
1 parent e82d0af commit f666ebb
Show file tree
Hide file tree
Showing 11 changed files with 37 additions and 37 deletions.
2 changes: 1 addition & 1 deletion EKF/EKFGSF_yaw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions EKF/covariance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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++) {
Expand Down
16 changes: 8 additions & 8 deletions EKF/mag_fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down
12 changes: 6 additions & 6 deletions EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down
8 changes: 4 additions & 4 deletions EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand Down
8 changes: 4 additions & 4 deletions EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
8 changes: 4 additions & 4 deletions EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit f666ebb

Please sign in to comment.