ekf2_derivation: automatically replace pow function

src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py

@@ -17,7 +17,18 @@ class CodeGenerator:
         self.file.write("// " + string + "\n")
 
     def get_ccode(self, expression):
-        return ccode(expression, type_aliases={real:float32})
+        custom_functions = {
+                "Pow": [
+                    (lambda b, e: e == 2, lambda b, e: f"({b})*({b})"),
+                    (lambda b, e: e == -1, lambda b, e: f"1.0F/({b})"),
+                    (lambda b, e: e == -2, lambda b, e: f"1.0F/(({b})*({b}))"),
+                    (lambda b, e: e == 0.5, lambda b, e: f"sqrtf({b})"),
+                    (lambda b, e: e == -0.5, lambda b, e: f"1.0F/sqrtf({b})"),
+                    (lambda b, e: True, "ecl::powf"),
+                    ]
+        }
+
+        return ccode(expression, type_aliases={real:float32}, user_functions=custom_functions)
 
     def write_subexpressions(self,subexpressions):
         write_string = ""

src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp

@@ -5,7 +5,7 @@ const float HKX1 = magD*q3 + magE*q2;
 const float HKX2 = 2*magN;
 const float HKX3 = HKX2*q2 + magD*q0 - magE*q1;
 const float HKX4 = -HKX2*q3 + magD*q1 + magE*q0;
-const float HKX5 = 2*powf(q2, 2) + 2*powf(q3, 2) - 1;
+const float HKX5 = 2*(q2)*(q2) + 2*(q3)*(q3) - 1;
 const float HKX6 = q0*q3 + q1*q2;
 const float HKX7 = q0*q2 - q1*q3;
 const float HKX8 = 2*HKX0;
@@ -87,7 +87,7 @@ const float HKY2 = -HKY1*q1 + magD*q0 + magN*q2;
 const float HKY3 = magD*q3 + magN*q1;
 const float HKY4 = HKY1*q3 - magD*q2 + magN*q0;
 const float HKY5 = q0*q3 - q1*q2;
-const float HKY6 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
+const float HKY6 = 2*(q1)*(q1) + 2*(q3)*(q3) - 1;
 const float HKY7 = q0*q1 + q2*q3;
 const float HKY8 = 2*HKY7;
 const float HKY9 = 2*HKY3;
@@ -169,7 +169,7 @@ const float HKZ3 = -HKZ1*q2 + magE*q3 + magN*q0;
 const float HKZ4 = magE*q2 + magN*q1;
 const float HKZ5 = q0*q2 + q1*q3;
 const float HKZ6 = q0*q1 - q2*q3;
-const float HKZ7 = 2*powf(q1, 2) + 2*powf(q2, 2) - 1;
+const float HKZ7 = 2*(q1)*(q1) + 2*(q2)*(q2) - 1;
 const float HKZ8 = 2*HKZ0;
 const float HKZ9 = 2*HKZ6;
 const float HKZ10 = 2*HKZ2;

src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp

@@ -12,8 +12,8 @@ const float HK9 = magD*q1;
 const float HK10 = magE*q0;
 const float HK11 = magN*q3;
 const float HK12 = HK10 - 2*HK11 + HK9;
-const float HK13 = 2*powf(q2, 2);
-const float HK14 = 2*powf(q3, 2) - 1;
+const float HK13 = 2*(q2)*(q2);
+const float HK14 = 2*(q3)*(q3) - 1;
 const float HK15 = HK13 + HK14;
 const float HK16 = q0*q3;
 const float HK17 = HK16 + q1*q2;
@@ -45,7 +45,7 @@ const float HK42 = magN*q0;
 const float HK43 = magE*q3;
 const float HK44 = -HK0 + HK42 + 2*HK43;
 const float HK45 = HK16 - q1*q2;
-const float HK46 = 2*powf(q1, 2);
+const float HK46 = 2*(q1)*(q1);
 const float HK47 = HK14 + HK46;
 const float HK48 = q0*q1;
 const float HK49 = HK48 + q2*q3;

src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp

@@ -11,8 +11,8 @@ const float IV8 = 2*q0*q3 - 2*q1*q2;
 const float IV9 = magN*q2;
 const float IV10 = magE*q1;
 const float IV11 = -4*IV10 + 2*IV9 + 2*magD*q0;
-const float IV12 = 2*powf(q1, 2) - 1;
-const float IV13 = IV12 + 2*powf(q3, 2);
+const float IV12 = 2*(q1)*(q1) - 1;
+const float IV13 = IV12 + 2*(q3)*(q3);
 const float IV14 = magN*q0;
 const float IV15 = magD*q2;
 const float IV16 = magE*q3;
@@ -21,7 +21,7 @@ const float IV18 = q0*q2 + q1*q3;
 const float IV19 = IV3 + magE*q2;
 const float IV20 = 2*IV10 - 2*IV9;
 const float IV21 = 2*IV0 - 2*IV1;
-const float IV22 = IV12 + 2*powf(q2, 2);
+const float IV22 = IV12 + 2*(q2)*(q2);
 const float IV23 = IV14 - 2*IV15 + IV16;
 const float IV24 = 4*IV5 + 2*IV6 + 2*magE*q0;
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp

@@ -7,7 +7,7 @@ const float HK3 = HK0*q2 + q3*vd;
 const float HK4 = 2*vn - 2*vwn;
 const float HK5 = -HK0*q1 + HK4*q2 + q0*vd;
 const float HK6 = HK0*q0 - HK4*q3 + q1*vd;
-const float HK7 = 2*powf(q2, 2) + 2*powf(q3, 2) - 1;
+const float HK7 = 2*(q2)*(q2) + 2*(q3)*(q3) - 1;
 const float HK8 = HK7*Kaccx;
 const float HK9 = q0*q3 + q1*q2;
 const float HK10 = HK2*HK9;
@@ -25,7 +25,7 @@ const float HK21 = -HK7;
 const float HK22 = -2*HK1;
 const float HK23 = 2*HK6;
 const float HK24 = -2*HK5;
-const float HK25 = powf(Kaccx, 2);
+const float HK25 = (Kaccx)*(Kaccx);
 const float HK26 = HK12*HK25;
 const float HK27 = -HK18;
 const float HK28 = HK12*P(6,23);
@@ -104,7 +104,7 @@ const float HK5 = HK0*q1 + q3*vd;
 const float HK6 = HK0*q0 + HK3*q3 - q2*vd;
 const float HK7 = q0*q3 - q1*q2;
 const float HK8 = HK2*HK7;
-const float HK9 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
+const float HK9 = 2*(q1)*(q1) + 2*(q3)*(q3) - 1;
 const float HK10 = HK9*Kaccy;
 const float HK11 = q0*q1 + q2*q3;
 const float HK12 = 2*HK6;
@@ -114,7 +114,7 @@ const float HK15 = 2*HK4;
 const float HK16 = 2*HK5;
 const float HK17 = 2*HK11;
 const float HK18 = HK13*P(0,22) + HK14*P(0,0) + HK15*P(0,1) + HK16*P(0,2) + HK17*P(0,6) + HK9*P(0,23);
-const float HK19 = powf(Kaccy, 2);
+const float HK19 = (Kaccy)*(Kaccy);
 const float HK20 = -HK9;
 const float HK21 = -2*HK6;
 const float HK22 = -2*HK7;

src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp

@@ -14,8 +14,8 @@ const float HK11 = 2*HK10 + HK7 - HK8;
 const float HK12 = q1*vd;
 const float HK13 = HK9*q3;
 const float HK14 = HK1*q0 + HK12 - 2*HK13;
-const float HK15 = 2*powf(q3, 2) - 1;
-const float HK16 = HK15 + 2*powf(q2, 2);
+const float HK15 = 2*(q3)*(q3) - 1;
+const float HK16 = HK15 + 2*(q2)*(q2);
 const float HK17 = HK16*Kaccx;
 const float HK18 = q0*q3;
 const float HK19 = q1*q2;
@@ -35,7 +35,7 @@ const float HK32 = -HK16;
 const float HK33 = -2*HK3;
 const float HK34 = 2*HK14;
 const float HK35 = -2*HK11;
-const float HK36 = powf(Kaccx, 2);
+const float HK36 = (Kaccx)*(Kaccx);
 const float HK37 = HK23*HK36;
 const float HK38 = -HK29;
 const float HK39 = HK23*P(6,23);
@@ -54,7 +54,7 @@ const float HK51 = HK5 + HK9*q1;
 const float HK52 = -HK0 + 2*HK2 + HK9*q0;
 const float HK53 = HK18 - HK19;
 const float HK54 = HK49*HK53;
-const float HK55 = HK15 + 2*powf(q1, 2);
+const float HK55 = HK15 + 2*(q1)*(q1);
 const float HK56 = HK55*Kaccy;
 const float HK57 = q0*q1 + q2*q3;
 const float HK58 = 2*HK52;
@@ -64,7 +64,7 @@ const float HK61 = 2*HK50;
 const float HK62 = 2*HK51;
 const float HK63 = 2*HK57;
 const float HK64 = HK55*P(0,23) + HK59*P(0,22) + HK60*P(0,0) + HK61*P(0,1) + HK62*P(0,2) + HK63*P(0,6);
-const float HK65 = powf(Kaccy, 2);
+const float HK65 = (Kaccy)*(Kaccy);
 const float HK66 = -HK55;
 const float HK67 = -2*HK52;
 const float HK68 = -2*HK53;

src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp

@@ -10,12 +10,12 @@ const float HK7 = 2*vd;
 const float HK8 = q0*q3;
 const float HK9 = q1*q2;
 const float HK10 = 2*HK8 + 2*HK9;
-const float HK11 = 2*powf(q3, 2) - 1;
-const float HK12 = HK11 + 2*powf(q2, 2);
+const float HK11 = 2*(q3)*(q3) - 1;
+const float HK12 = HK11 + 2*(q2)*(q2);
 const float HK13 = HK1*HK12 - HK10*HK4 + HK6*HK7;
-const float HK14 = 1.0F/HK13;
+const float HK14 = 1.0F/(HK13);
 const float HK15 = q0*q1 + q2*q3;
-const float HK16 = HK11 + 2*powf(q1, 2);
+const float HK16 = HK11 + 2*(q1)*(q1);
 const float HK17 = 2*HK1*(HK8 - HK9) - HK15*HK7 + HK16*HK4;
 const float HK18 = HK14*HK17;
 const float HK19 = 2*HK14;

src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp

@@ -1,9 +1,9 @@
 // Equations for covariance matrix prediction, without process noise!
-const float PS0 = powf(q1, 2);
+const float PS0 = (q1)*(q1);
 const float PS1 = 0.25F*daxVar;
-const float PS2 = powf(q2, 2);
+const float PS2 = (q2)*(q2);
 const float PS3 = 0.25F*dayVar;
-const float PS4 = powf(q3, 2);
+const float PS4 = (q3)*(q3);
 const float PS5 = 0.25F*dazVar;
 const float PS6 = 0.5F*q1;
 const float PS7 = 0.5F*q2;
@@ -94,7 +94,7 @@ const float PS91 = PS59 + PS82;
 const float PS92 = PS69 + PS79;
 const float PS93 = PS49 - 2*PS51 + PS53;
 const float PS94 = P(0,6) - P(1,6)*PS11 - P(2,6)*PS12 - P(3,6)*PS13 + P(6,10)*PS6 + P(6,11)*PS7 + P(6,12)*PS9;
-const float PS95 = powf(q0, 2);
+const float PS95 = (q0)*(q0);
 const float PS96 = -P(10,11)*PS34;
 const float PS97 = P(0,11)*PS11 + P(1,11) + P(11,11)*PS9 + P(2,11)*PS13 - P(3,11)*PS12 - PS19 + PS96;
 const float PS98 = P(0,2)*PS13;
@@ -238,20 +238,20 @@ nextP(0,4) = PS43*PS44 - PS45*PS47 - PS54*PS55 + PS56*PS58 + PS61*PS62 + PS66*PS
 nextP(1,4) = PS113*PS43 - PS115*PS45 - PS116*PS54 + PS118*PS56 + PS119*PS61 + PS120*PS66 + PS121*PS71 + PS122;
 nextP(2,4) = PS138*PS43 - PS140*PS45 - PS141*PS54 + PS143*PS56 + PS144*PS61 + PS145*PS66 + PS146*PS71 + PS147;
 nextP(3,4) = PS158*PS43 - PS160*PS45 - PS161*PS54 + PS163*PS56 + PS164*PS61 + PS165*PS66 + PS166*PS71 + PS167;
-nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*powf(PS56, 2) + PS185*powf(PS45, 2) + PS186 + powf(PS43, 2)*dvxVar;
+nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*(PS56)*(PS56) + PS185*(PS45)*(PS45) + PS186 + (PS43)*(PS43)*dvxVar;
 nextP(0,5) = PS47*PS81 + PS55*PS85 + PS57*PS75 - PS62*PS80 - PS67*PS78 + PS72*PS83 - PS76*PS77 + PS86;
 nextP(1,5) = PS115*PS81 + PS116*PS85 + PS117*PS75 - PS119*PS80 - PS120*PS78 + PS121*PS83 - PS123*PS76 + PS124;
 nextP(2,5) = PS140*PS81 + PS141*PS85 + PS142*PS75 - PS144*PS80 - PS145*PS78 + PS146*PS83 - PS148*PS76 + PS149;
 nextP(3,5) = PS160*PS81 + PS161*PS85 + PS162*PS75 - PS164*PS80 - PS165*PS78 + PS166*PS83 - PS168*PS76 + PS169;
 nextP(4,5) = PS172*PS195 + PS178*PS190 + PS180*PS75 - PS185*PS45*PS81 - PS187*PS76 - PS188*PS78 - PS189*PS80 + PS191*PS83 + PS192*PS85 - PS193*PS194 + PS196;
-nextP(5,5) = PS185*powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*powf(PS76, 2) + PS213 + powf(PS75, 2)*dvyVar;
+nextP(5,5) = PS185*(PS81)*(PS81) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*(PS76)*(PS76) + PS213 + (PS75)*(PS75)*dvyVar;
 nextP(0,6) = PS46*PS87 + PS55*PS91 - PS58*PS88 + PS62*PS93 + PS67*PS92 - PS72*PS89 + PS77*PS90 + PS94;
 nextP(1,6) = PS114*PS87 + PS116*PS91 - PS118*PS88 + PS119*PS93 + PS120*PS92 - PS121*PS89 + PS123*PS90 + PS125;
 nextP(2,6) = PS139*PS87 + PS141*PS91 - PS143*PS88 + PS144*PS93 + PS145*PS92 - PS146*PS89 + PS148*PS90 + PS150;
 nextP(3,6) = PS159*PS87 + PS161*PS91 - PS163*PS88 + PS164*PS93 + PS165*PS92 - PS166*PS89 + PS168*PS90 + PS170;
 nextP(4,6) = -PS171*PS198 + PS178*PS87 - PS180*PS197 - PS184*PS56*PS88 + PS187*PS90 + PS188*PS92 + PS189*PS93 - PS191*PS89 + PS192*PS91 + PS194*PS199 + PS200;
 nextP(5,6) = PS190*PS198 - PS195*PS197 - PS197*PS205 + PS199*PS206 + PS207*PS216 + PS208*PS217 + PS209*PS87 - PS210*PS214 + PS211*PS215 - PS212*PS76*PS90 + PS218;
-nextP(6,6) = PS184*powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + powf(PS87, 2)*dvzVar;
+nextP(6,6) = PS184*(PS88)*(PS88) - PS197*PS220 + PS199*PS221 + PS212*(PS90)*(PS90) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + (PS87)*(PS87)*dvzVar;
 nextP(0,7) = P(0,7) - P(1,7)*PS11 - P(2,7)*PS12 - P(3,7)*PS13 + P(7,10)*PS6 + P(7,11)*PS7 + P(7,12)*PS9 + PS73*dt;
 nextP(1,7) = P(0,7)*PS11 + P(1,7) + P(2,7)*PS13 - P(3,7)*PS12 - P(7,10)*PS34 + P(7,11)*PS9 - P(7,12)*PS7 + PS122*dt;
 nextP(2,7) = P(0,7)*PS12 - P(1,7)*PS13 + P(2,7) + P(3,7)*PS11 - P(7,10)*PS9 - P(7,11)*PS34 + P(7,12)*PS6 + PS147*dt;

src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp

@@ -8,7 +8,7 @@ const float HK4 = Tbs(1,0)*q3;
 const float HK5 = Tbs(1,2)*q1;
 const float HK6 = -HK5;
 const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3);
-const float HK8 = 1.0F/range;
+const float HK8 = 1.0F/(range);
 const float HK9 = 2*HK8;
 const float HK10 = Tbs(1,1)*q2;
 const float HK11 = Tbs(1,2)*q3;
@@ -25,18 +25,18 @@ const float HK21 = 2*Tbs(1,2);
 const float HK22 = q0*q3;
 const float HK23 = q1*q2;
 const float HK24 = 2*Tbs(1,1);
-const float HK25 = 2*powf(q2, 2);
-const float HK26 = 2*powf(q3, 2) - 1;
+const float HK25 = 2*(q2)*(q2);
+const float HK26 = 2*(q3)*(q3) - 1;
 const float HK27 = -HK21*(HK19 + HK20) + HK24*(HK22 - HK23) + Tbs(1,0)*(HK25 + HK26);
 const float HK28 = 2*Tbs(1,0);
 const float HK29 = q0*q1;
 const float HK30 = q2*q3;
-const float HK31 = 2*powf(q1, 2);
+const float HK31 = 2*(q1)*(q1);
 const float HK32 = HK21*(HK29 - HK30) - HK28*(HK22 + HK23) + Tbs(1,1)*(HK26 + HK31);
 const float HK33 = -HK24*(HK29 + HK30) + HK28*(HK19 - HK20) + Tbs(1,2)*(HK25 + HK31 - 1);
 const float HK34 = 2*HK7;
 const float HK35 = 2*HK14*P(0,1) + 2*HK17*P(0,2) + 2*HK18*P(0,3) - HK27*P(0,4) - HK32*P(0,5) - HK33*P(0,6) - HK34*P(0,0);
-const float HK36 = powf(range, -2);
+const float HK36 = 1.0F/((range)*(range));
 const float HK37 = 2*HK14*P(1,6) + 2*HK17*P(2,6) + 2*HK18*P(3,6) - HK27*P(4,6) - HK32*P(5,6) - HK33*P(6,6) - HK34*P(0,6);
 const float HK38 = 2*HK14*P(1,5) + 2*HK17*P(2,5) + 2*HK18*P(3,5) - HK27*P(4,5) - HK32*P(5,5) - HK33*P(5,6) - HK34*P(0,5);
 const float HK39 = 2*HK14*P(1,4) + 2*HK17*P(2,4) + 2*HK18*P(3,4) - HK27*P(4,4) - HK32*P(4,5) - HK33*P(4,6) - HK34*P(0,4);
@@ -112,7 +112,7 @@ const float HK6 = Tbs(0,0)*q3;
 const float HK7 = Tbs(0,2)*q1;
 const float HK8 = -HK7;
 const float HK9 = HK6 + HK8;
-const float HK10 = 1.0F/range;
+const float HK10 = 1.0F/(range);
 const float HK11 = 2*HK10;
 const float HK12 = Tbs(0,2)*q0;
 const float HK13 = -HK0 + 2*HK1 + HK12;
@@ -133,15 +133,15 @@ const float HK27 = q0*q3;
 const float HK28 = q1*q2;
 const float HK29 = HK27 - HK28;
 const float HK30 = 2*Tbs(0,1);
-const float HK31 = 2*powf(q2, 2);
-const float HK32 = 2*powf(q3, 2) - 1;
+const float HK31 = 2*(q2)*(q2);
+const float HK32 = 2*(q3)*(q3) - 1;
 const float HK33 = HK31 + HK32;
 const float HK34 = HK27 + HK28;
 const float HK35 = q0*q1;
 const float HK36 = q2*q3;
 const float HK37 = HK35 - HK36;
 const float HK38 = 2*Tbs(0,2);
-const float HK39 = 2*powf(q1, 2);
+const float HK39 = 2*(q1)*(q1);
 const float HK40 = HK32 + HK39;
 const float HK41 = HK35 + HK36;
 const float HK42 = HK24 - HK25;
@@ -155,7 +155,7 @@ const float HK49 = -2*HK13*ve + 2*HK17;
 const float HK50 = -2*HK19*vd + 2*HK21;
 const float HK51 = -2*HK22*vn + 2*HK23;
 const float HK52 = HK45*P(0,0) + HK46*P(0,4) + HK47*P(0,5) + HK48*P(0,6) + HK49*P(0,1) + HK50*P(0,2) + HK51*P(0,3);
-const float HK53 = powf(range, -2);
+const float HK53 = 1.0F/((range)*(range));
 const float HK54 = HK45*P(0,6) + HK46*P(4,6) + HK47*P(5,6) + HK48*P(6,6) + HK49*P(1,6) + HK50*P(2,6) + HK51*P(3,6);
 const float HK55 = HK45*P(0,5) + HK46*P(4,5) + HK47*P(5,5) + HK48*P(5,6) + HK49*P(1,5) + HK50*P(2,5) + HK51*P(3,5);
 const float HK56 = HK45*P(0,4) + HK46*P(4,4) + HK47*P(4,5) + HK48*P(4,6) + HK49*P(1,4) + HK50*P(2,4) + HK51*P(3,4);

src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp

@@ -7,7 +7,7 @@ const float HK4 = Tbs(1,0)*q3;
 const float HK5 = Tbs(1,2)*q1;
 const float HK6 = -HK5;
 const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3);
-const float HK8 = 1.0F/range;
+const float HK8 = 1.0F/(range);
 const float HK9 = 2*HK8;
 const float HK10 = Tbs(1,1)*q2;
 const float HK11 = Tbs(1,2)*q3;
@@ -26,8 +26,8 @@ const float HK23 = q0*q3;
 const float HK24 = q1*q2;
 const float HK25 = HK23 - HK24;
 const float HK26 = 2*Tbs(1,1);
-const float HK27 = 2*powf(q2, 2);
-const float HK28 = 2*powf(q3, 2) - 1;
+const float HK27 = 2*(q2)*(q2);
+const float HK28 = 2*(q3)*(q3) - 1;
 const float HK29 = HK27 + HK28;
 const float HK30 = -HK21*HK22 + HK25*HK26 + HK29*Tbs(1,0);
 const float HK31 = HK23 + HK24;
@@ -35,7 +35,7 @@ const float HK32 = 2*Tbs(1,0);
 const float HK33 = q0*q1;
 const float HK34 = q2*q3;
 const float HK35 = HK33 - HK34;
-const float HK36 = 2*powf(q1, 2);
+const float HK36 = 2*(q1)*(q1);
 const float HK37 = HK28 + HK36;
 const float HK38 = HK22*HK35 - HK31*HK32 + HK37*Tbs(1,1);
 const float HK39 = HK33 + HK34;
@@ -44,7 +44,7 @@ const float HK41 = HK27 + HK36 - 1;
 const float HK42 = -HK26*HK39 + HK32*HK40 + HK41*Tbs(1,2);
 const float HK43 = 2*HK7;
 const float HK44 = 2*HK14*P(0,1) + 2*HK17*P(0,2) + 2*HK18*P(0,3) - HK30*P(0,4) - HK38*P(0,5) - HK42*P(0,6) - HK43*P(0,0);
-const float HK45 = powf(range, -2);
+const float HK45 = 1.0F/((range)*(range));
 const float HK46 = 2*HK14*P(1,6) + 2*HK17*P(2,6) + 2*HK18*P(3,6) - HK30*P(4,6) - HK38*P(5,6) - HK42*P(6,6) - HK43*P(0,6);
 const float HK47 = 2*HK14*P(1,5) + 2*HK17*P(2,5) + 2*HK18*P(3,5) - HK30*P(4,5) - HK38*P(5,5) - HK42*P(5,6) - HK43*P(0,5);
 const float HK48 = 2*HK14*P(1,4) + 2*HK17*P(2,4) + 2*HK18*P(3,4) - HK30*P(4,4) - HK38*P(4,5) - HK42*P(4,6) - HK43*P(0,4);

src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp

@@ -4,14 +4,14 @@ const float HK1 = sinf(ant_yaw);
 const float HK2 = q0*q3;
 const float HK3 = q1*q2;
 const float HK4 = 2*HK0;
-const float HK5 = 2*powf(q3, 2) - 1;
-const float HK6 = -HK1*(HK5 + 2*powf(q1, 2)) + HK4*(HK2 + HK3);
+const float HK5 = 2*(q3)*(q3) - 1;
+const float HK6 = -HK1*(HK5 + 2*(q1)*(q1)) + HK4*(HK2 + HK3);
 const float HK7 = 2*HK1;
-const float HK8 = HK0*(HK5 + 2*powf(q2, 2)) + HK7*(HK2 - HK3);
-const float HK9 = 1.0F/HK8;
+const float HK8 = HK0*(HK5 + 2*(q2)*(q2)) + HK7*(HK2 - HK3);
+const float HK9 = 1.0F/(HK8);
 const float HK10 = q3*(-HK0 + HK1*HK6*HK9);
-const float HK11 = powf(HK8, -2);
-const float HK12 = HK11*powf(HK6, 2) + 1;
+const float HK11 = 1.0F/((HK8)*(HK8));
+const float HK12 = HK11*(HK6)*(HK6) + 1;
 const float HK13 = 2*HK9/HK12;
 const float HK14 = HK0*q2;
 const float HK15 = HK1*q1;
@@ -20,7 +20,7 @@ const float HK17 = HK1*HK16*q2 + HK14 - 2*HK15;
 const float HK18 = HK0*q1 + HK16*(-2*HK14 + HK15);
 const float HK19 = -HK0*q0 + HK16*(HK1*q0 + HK4*q3) + HK7*q3;
 const float HK20 = HK10*P(0,0) - HK17*P(0,1) - HK18*P(0,2) + HK19*P(0,3);
-const float HK21 = 4*HK11/powf(HK12, 2);
+const float HK21 = 4*HK11/(HK12)*(HK12);
 const float HK22 = HK10*P(0,1) - HK17*P(1,1) - HK18*P(1,2) + HK19*P(1,3);
 const float HK23 = HK10*P(0,2) - HK17*P(1,2) - HK18*P(2,2) + HK19*P(2,3);
 const float HK24 = HK10*P(0,3) - HK17*P(1,3) - HK18*P(2,3) + HK19*P(3,3);

src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp

@@ -1,14 +1,14 @@
 // Sub Expressions
-const float HK0 = powf(magN, -2);
-const float HK1 = HK0*powf(magE, 2) + 1;
-const float HK2 = 1.0F/HK1;
-const float HK3 = 1.0F/magN;
+const float HK0 = 1.0F/((magN)*(magN));
+const float HK1 = HK0*(magE)*(magE) + 1;
+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 = 1.0F/((HK1)*(HK1));
 const float HK8 = HK5*P(16,16) - P(16,17);
-const float HK9 = HK4/(-HK0*HK6*HK7 + HK7*HK8*magE/powf(magN, 3) + R_DECL);
+const float HK9 = HK4/(-HK0*HK6*HK7 + HK7*HK8*magE/ecl::powf(magN, 3) + R_DECL);
 
 
 // Observation Jacobians

src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp

@@ -1,11 +1,11 @@
 // Sub Expressions
 const float HK0 = vn - vwn;
 const float HK1 = ve - vwe;
-const float HK2 = powf(HK0, 2) + powf(HK1, 2) + powf(vd, 2);
-const float HK3 = powf(HK2, -1.0F/2.0F);
+const float HK2 = (HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd);
+const float HK3 = 1.0F/sqrtf(HK2);
 const float HK4 = HK0*HK3;
 const float HK5 = HK1*HK3;
-const float HK6 = 1.0F/HK2;
+const float HK6 = 1.0F/(HK2);
 const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd;
 const float HK8 = HK1*P(5,23);
 const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd;

src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp

@@ -4,7 +4,7 @@ const float HK1 = q2*ve + q3*vd;
 const float HK2 = 2*vn;
 const float HK3 = HK2*q2 + q0*vd - q1*ve;
 const float HK4 = -HK2*q3 + q0*ve + q1*vd;
-const float HK5 = 2*powf(q2, 2) + 2*powf(q3, 2) - 1;
+const float HK5 = 2*(q2)*(q2) + 2*(q3)*(q3) - 1;
 const float HK6 = q0*q3 + q1*q2;
 const float HK7 = q0*q2 - q1*q3;
 const float HK8 = 2*HK1;
@@ -82,7 +82,7 @@ const float HK2 = -HK1*q1 + q0*vd + q2*vn;
 const float HK3 = q1*vn + q3*vd;
 const float HK4 = HK1*q3 + q0*vn - q2*vd;
 const float HK5 = q0*q3 - q1*q2;
-const float HK6 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
+const float HK6 = 2*(q1)*(q1) + 2*(q3)*(q3) - 1;
 const float HK7 = q0*q1 + q2*q3;
 const float HK8 = 2*HK3;
 const float HK9 = 2*HK7;
@@ -160,7 +160,7 @@ const float HK3 = -HK1*q2 + q0*vn + q3*ve;
 const float HK4 = q1*vn + q2*ve;
 const float HK5 = q0*q2 + q1*q3;
 const float HK6 = q0*q1 - q2*q3;
-const float HK7 = 2*powf(q1, 2) + 2*powf(q2, 2) - 1;
+const float HK7 = 2*(q1)*(q1) + 2*(q2)*(q2) - 1;
 const float HK8 = 2*HK4;
 const float HK9 = 2*HK5;
 const float HK10 = 2*HK0;

src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp

@@ -12,8 +12,8 @@ const float HK9 = q0*ve;
 const float HK10 = q1*vd;
 const float HK11 = q3*vn;
 const float HK12 = HK10 - 2*HK11 + HK9;
-const float HK13 = 2*powf(q2, 2);
-const float HK14 = 2*powf(q3, 2) - 1;
+const float HK13 = 2*(q2)*(q2);
+const float HK14 = 2*(q3)*(q3) - 1;
 const float HK15 = HK13 + HK14;
 const float HK16 = q0*q3;
 const float HK17 = HK16 + q1*q2;
@@ -42,7 +42,7 @@ const float HK39 = q0*vn;
 const float HK40 = q3*ve;
 const float HK41 = -HK0 + HK39 + 2*HK40;
 const float HK42 = HK16 - q1*q2;
-const float HK43 = 2*powf(q1, 2);
+const float HK43 = 2*(q1)*(q1);
 const float HK44 = HK14 + HK43;
 const float HK45 = q0*q1;
 const float HK46 = HK45 + q2*q3;

src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp

@@ -1,8 +1,8 @@
 // Equations for covariance matrix prediction
 const float S0 = cosf(psi);
-const float S1 = powf(S0, 2);
+const float S1 = (S0)*(S0);
 const float S2 = sinf(psi);
-const float S3 = powf(S2, 2);
+const float S3 = (S2)*(S2);
 const float S4 = S0*dvy + S2*dvx;
 const float S5 = P(0,2) - P(2,2)*S4;
 const float S6 = S0*dvx - S2*dvy;

src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp

@@ -1,8 +1,8 @@
 // Intermediate variables
-const float t0 = powf(P(0,1), 2);
+const float t0 = (P(0,1))*(P(0,1));
 const float t1 = -t0;
-const float t2 = P(0,0)*P(1,1) + P(0,0)*velObsVar + P(1,1)*velObsVar + t1 + powf(velObsVar, 2);
-const float t3 = 1.0F/t2;
+const float t2 = P(0,0)*P(1,1) + P(0,0)*velObsVar + P(1,1)*velObsVar + t1 + (velObsVar)*(velObsVar);
+const float t3 = 1.0F/(t2);
 const float t4 = P(1,1) + velObsVar;
 const float t5 = P(0,1)*t3;
 const float t6 = -t5;
@@ -13,7 +13,7 @@ const float t10 = -P(1,1)*t7 + t0;
 const float t11 = P(0,1)*P(1,2) - P(0,2)*t4;
 const float t12 = P(0,1)*P(0,2) - P(1,2)*t7;
 const float t13 = t0*velObsVar;
-const float t14 = powf(t2, -2);
+const float t14 = 1.0F/((t2)*(t2));
 const float t15 = t4*velObsVar + t8;
 const float t16 = t14*t15;
 const float t17 = t14*(t13 + t7*t8);

src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp

@@ -2,10 +2,10 @@
 const float SA0 = 2*q0;
 const float SA1 = 2*q1;
 const float SA2 = SA0*q3 + SA1*q2;
-const float SA3 = -2*powf(q2, 2) - 2*powf(q3, 2) + 1;
-const float SA4 = powf(SA3, -2);
-const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
-const float SA6 = 1.0F/SA3;
+const float SA3 = -2*(q2)*(q2) - 2*(q3)*(q3) + 1;
+const float SA4 = 1.0F/((SA3)*(SA3));
+const float SA5 = 1.0F/((SA2)*(SA2)*SA4 + 1);
+const float SA6 = 1.0F/(SA3);
 const float SA7 = 2*SA5*SA6;
 const float SA8 = 4*SA2*SA4;
 
@@ -40,9 +40,9 @@ H_YAW.at<23>() = 0;
 const float SB0 = 2*q0;
 const float SB1 = 2*q1;
 const float SB2 = SB0*q3 + SB1*q2;
-const float SB3 = powf(SB2, -2);
-const float SB4 = -2*powf(q2, 2) - 2*powf(q3, 2) + 1;
-const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
+const float SB3 = 1.0F/((SB2)*(SB2));
+const float SB4 = -2*(q2)*(q2) - 2*(q3)*(q3) + 1;
+const float SB5 = 1.0F/(SB3*(SB4)*(SB4) + 1);
 const float SB6 = SB3*SB4;
 const float SB7 = 2*SB5*SB6;
 const float SB8 = 4/SB2;
@@ -78,10 +78,10 @@ H_YAW.at<23>() = 0;
 const float SA0 = 2*q0;
 const float SA1 = 2*q2;
 const float SA2 = SA0*q3 - SA1*q1;
-const float SA3 = -2*powf(q1, 2) - 2*powf(q3, 2) + 1;
-const float SA4 = powf(SA3, -2);
-const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
-const float SA6 = 1.0F/SA3;
+const float SA3 = -2*(q1)*(q1) - 2*(q3)*(q3) + 1;
+const float SA4 = 1.0F/((SA3)*(SA3));
+const float SA5 = 1.0F/((SA2)*(SA2)*SA4 + 1);
+const float SA6 = 1.0F/(SA3);
 const float SA7 = 2*SA5*SA6;
 const float SA8 = 4*SA2*SA4;
 
@@ -115,9 +115,9 @@ H_YAW.at<23>() = 0;
 // calculate 312 yaw observation matrix - option B
 const float SB0 = 2*q0;
 const float SB1 = -SB0*q3 + 2*q1*q2;
-const float SB2 = powf(SB1, -2);
-const float SB3 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
-const float SB4 = 1.0F/(SB2*powf(SB3, 2) + 1);
+const float SB2 = 1.0F/((SB1)*(SB1));
+const float SB3 = 2*(q1)*(q1) + 2*(q3)*(q3) - 1;
+const float SB4 = 1.0F/(SB2*(SB3)*(SB3) + 1);
 const float SB5 = SB2*SB3;
 const float SB6 = 2*SB4*SB5;
 const float SB7 = 4/SB1;