From f89c52e8405acdb4527c0c27567187ce54305e70 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 13 Aug 2020 18:21:17 +1000 Subject: [PATCH] EKF: use ecl::powf for integer exponents --- EKF/gps_yaw_fusion.cpp | 10 +++++----- EKF/mag_fusion.cpp | 20 ++++++++++---------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 68865aa6dc..ed43709d29 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -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); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 30238d7d11..5c85a66f5c 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -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;