From 1ad042da232ef3b92c43c992277d13d62562db01 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 13 Aug 2020 19:51:33 +1000 Subject: [PATCH] EKF: Use ecl::powf in comparison test --- .../flow_fusion_generated_compare.cpp | 15 +++++------- EKF/python/ekf_derivation/generated/util.h | 24 +++++++++++++++++++ 2 files changed, 30 insertions(+), 9 deletions(-) create mode 100644 EKF/python/ekf_derivation/generated/util.h 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 d9726b51b1..77104c9e9f 100644 --- a/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp +++ b/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp @@ -2,6 +2,7 @@ #include #include #include "../../../../../matrix/matrix/math.hpp" +#include "util.h" typedef matrix::Vector Vector24f; typedef matrix::SquareMatrix SquareMatrix24f; @@ -9,10 +10,6 @@ typedef matrix::SquareMatrix SquareMatrix24f; template using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; -float sq(float in) { - return in * in; -} - int main() { // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations @@ -82,11 +79,11 @@ int main() const float HK15 = q1*q2; const float HK16 = HK14 - HK15; const float HK17 = 2*Tbs(1,1); - const float HK18 = powf(q1, 2); - const float HK19 = powf(q2, 2); + const float HK18 = ecl::powf(q1, 2); + const float HK19 = ecl::powf(q2, 2); const float HK20 = -HK19; - const float HK21 = powf(q0, 2); - const float HK22 = powf(q3, 2); + const float HK21 = ecl::powf(q0, 2); + const float HK22 = ecl::powf(q3, 2); const float HK23 = HK21 - HK22; const float HK24 = HK18 + HK20 + HK23; const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0); @@ -107,7 +104,7 @@ int main() const float HK40 = 2*HK8; const float HK41 = 2*HK9; const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3); - const float HK43 = powf(range, -2); + const float HK43 = ecl::powf(range, -2); const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6); const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5); const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4); diff --git a/EKF/python/ekf_derivation/generated/util.h b/EKF/python/ekf_derivation/generated/util.h new file mode 100644 index 0000000000..36ba9f4ffc --- /dev/null +++ b/EKF/python/ekf_derivation/generated/util.h @@ -0,0 +1,24 @@ +#include "../../../../../matrix/matrix/math.hpp" + +typedef matrix::SquareMatrix SquareMatrix24f; + +inline float sq(float in) { + return in * in; +} + +namespace ecl{ + inline float powf(float x, int exp) + { + float ret; + if (exp > 0) { + ret = x; + for (int count = 1; count < exp; count++) { + ret *= x; + } + return ret; + } else if (exp < 0) { + return 1.0f / ecl::powf(x, -exp); + } + return 1.0f; + } +}