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

EKF: use ecl::powf for integer exponents

Browse Source
This commit is contained in:
Paul Riseborough
2020-08-13 18:21:17 +10:00
committed by Paul Riseborough
parent 0aa2f17562
commit f89c52e840
2 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/gps_yaw_fusion.cpp
+5 -5
View File
@@ -79,9 +79,9 @@ void Ekf::fuseGpsYaw()
const float HK2 = q1*q2;
const float HK3 = 2*HK0*(HK1 - HK2);
const float HK4 = cosf(_gps_yaw_offset);
const float HK5 = powf(q1, 2);
const float HK6 = powf(q2, 2);
const float HK7 = powf(q0, 2) - powf(q3, 2);
const float HK5 = ecl::powf(q1, 2);
const float HK6 = ecl::powf(q2, 2);
const float HK7 = ecl::powf(q0, 2) - ecl::powf(q3, 2);
const float HK8 = HK4*(HK5 - HK6 + HK7);
const float HK9 = HK3 - HK8;
if (fabsf(HK9) < 1e-3f) {
@@ -94,7 +94,7 @@ void Ekf::fuseGpsYaw()
const float HK14 = HK10*HK13;
const float HK15 = HK0*q0 + HK4*q3;
const float HK16 = HK10*(HK14*(HK11 - HK12) + HK15);
const float HK17 = powf(HK13, 2)/powf(HK9, 2) + 1;
const float HK17 = ecl::powf(HK13, 2)/ecl::powf(HK9, 2) + 1;
if (fabsf(HK17) < 1e-3f) {
return;
}
@@ -114,7 +114,7 @@ void Ekf::fuseGpsYaw()
const float HK26 = HK10*(-HK11 + HK12 + HK14*HK15);
const float HK27 = -HK16*P(0,0) - HK24*P(0,1) - HK25*P(0,2) + HK26*P(0,3);
const float HK28 = -HK16*P(0,1) - HK24*P(1,1) - HK25*P(1,2) + HK26*P(1,3);
const float HK29 = 4/powf(HK17, 2);
const float HK29 = 4/ecl::powf(HK17, 2);
const float HK30 = -HK16*P(0,2) - HK24*P(1,2) - HK25*P(2,2) + HK26*P(2,3);
const float HK31 = -HK16*P(0,3) - HK24*P(1,3) - HK25*P(2,3) + HK26*P(3,3);
// const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
EKF/mag_fusion.cpp
+10 -10
View File
@@ -68,7 +68,7 @@ void Ekf::fuseMag()
const float HKX3 = magD*q0;
const float HKX4 = magN*q2;
const float HKX5 = magD*q1 + magE*q0 - magN*q3;
const float HKX6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
const float HKX6 = ecl::powf(q0, 2) + ecl::powf(q1, 2) - ecl::powf(q2, 2) - ecl::powf(q3, 2);
const float HKX7 = q0*q3 + q1*q2;
const float HKX8 = q1*q3;
const float HKX9 = q0*q2;
@@ -114,9 +114,9 @@ void Ekf::fuseMag()
const float IV5 = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
const float IV6 = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
const float IV7 = -2*magD*q2 + 2*magE*q3 + 2*magN*q0;
const float IV8 = powf(q2, 2);
const float IV9 = powf(q3, 2);
const float IV10 = powf(q0, 2) - powf(q1, 2);
const float IV8 = ecl::powf(q2, 2);
const float IV9 = ecl::powf(q3, 2);
const float IV10 = ecl::powf(q0, 2) - ecl::powf(q1, 2);
const float IV11 = IV10 + IV8 - IV9;
const float IV12 = IV7*P(2,3);
const float IV13 = IV5*P(0,1);
@@ -250,7 +250,7 @@ void Ekf::fuseMag()
const float HKY5 = magN*q0;
const float HKY6 = q1*q2;
const float HKY7 = q0*q3;
const float HKY8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
const float HKY8 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2);
const float HKY9 = q0*q1 + q2*q3;
const float HKY10 = 2*HKY9;
const float HKY11 = -2*HKY6 + 2*HKY7;
@@ -331,7 +331,7 @@ void Ekf::fuseMag()
const float HKZ6 = q0*q2 + q1*q3;
const float HKZ7 = q2*q3;
const float HKZ8 = q0*q1;
const float HKZ9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
const float HKZ9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) - ecl::powf(q2, 2) + ecl::powf(q3, 2);
const float HKZ10 = 2*HKZ6;
const float HKZ11 = -2*HKZ7 + 2*HKZ8;
const float HKZ12 = 2*HKZ5;
@@ -886,16 +886,16 @@ void Ekf::fuseDeclination(float decl_sigma)
// calculation is badly conditioned close to +-90 deg declination
return;
}
const float HK0 = powf(magN, -2);
const float HK1 = HK0*powf(magE, 2) + 1.0F;
const float HK0 = ecl::powf(magN, -2);
const float HK1 = HK0*ecl::powf(magE, 2) + 1.0F;
const float HK2 = 1.0F/HK1;
const float HK3 = 1.0F/magN;
const float HK4 = HK2*HK3;
const float HK5 = HK3*magE;
const float HK6 = HK5*P(16,17) - P(17,17);
const float HK7 = powf(HK1, -2);
const float HK7 = ecl::powf(HK1, -2);
const float HK8 = HK5*P(16,16) - P(16,17);
const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/powf(magN, 3) + R_DECL;
const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/ecl::powf(magN, 3) + R_DECL;
float HK9;
if (innovation_variance > R_DECL) {
HK9 = HK4/innovation_variance;