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

EKF: Use ecl::powf in comparison test

Browse Source
This commit is contained in:
Paul Riseborough
2020-08-13 19:51:33 +10:00
committed by Paul Riseborough
parent 274523bd98
commit 1ad042da23
2 changed files with 30 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp
+6 -9
View File
@@ -2,6 +2,7 @@
#include <stdio.h> #include <stdio.h>
#include <cstdlib> #include <cstdlib>
#include "../../../../../matrix/matrix/math.hpp" #include "../../../../../matrix/matrix/math.hpp"
#include "util.h"
typedef matrix::Vector<float, 24> Vector24f; typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f; typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
@@ -9,10 +10,6 @@ typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
template<int ... Idxs> template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
float sq(float in) {
return in * in;
}
int main() int main()
{ {
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations // 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 HK15 = q1*q2;
const float HK16 = HK14 - HK15; const float HK16 = HK14 - HK15;
const float HK17 = 2*Tbs(1,1); const float HK17 = 2*Tbs(1,1);
const float HK18 = powf(q1, 2); const float HK18 = ecl::powf(q1, 2);
const float HK19 = powf(q2, 2); const float HK19 = ecl::powf(q2, 2);
const float HK20 = -HK19; const float HK20 = -HK19;
const float HK21 = powf(q0, 2); const float HK21 = ecl::powf(q0, 2);
const float HK22 = powf(q3, 2); const float HK22 = ecl::powf(q3, 2);
const float HK23 = HK21 - HK22; const float HK23 = HK21 - HK22;
const float HK24 = HK18 + HK20 + HK23; const float HK24 = HK18 + HK20 + HK23;
const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0); const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
@@ -107,7 +104,7 @@ int main()
const float HK40 = 2*HK8; const float HK40 = 2*HK8;
const float HK41 = 2*HK9; 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 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 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 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); 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);
EKF/python/ekf_derivation/generated/util.h
+24
View File
@@ -0,0 +1,24 @@
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::SquareMatrix<float, 24> 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;
}
}