fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 21:44:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Code format fix - replace 1E-x with 1e-x

Browse Source
This commit is contained in:
Paul Riseborough 2020-10-06 19:29:19 +11:00 committed by Paul Riseborough
parent e82d0af6d2
commit f666ebb995
11 changed files with 37 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/EKFGSF_yaw.cpp
View File

@ -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);

4
EKF/covariance.cpp
View File

@ -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++) {

16
EKF/mag_fusion.cpp
View File

@ -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;

12
EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp
View File

@ -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);

8
EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp
View File

@ -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);

4
EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp
View File

@ -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);

8
EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp
View File

@ -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);

4
EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp
View File

@ -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);

4
EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp
View File

@ -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);

4
EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp
View File

@ -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);

8
EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp
View File

@ -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);