diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py b/src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py index 632abb46ef..29a5df1064 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py +++ b/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 = "" diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp index 22df4e4099..f0d2e80115 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp index 1067bc58bc..3a58a69a81 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp index 80e8a7b663..fc5f5fc12f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp index d24599191a..79453f7c49 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp index 0f1bc49743..fc888c4e87 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp index 9d6cc96839..613e9df66f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp index 18b563ddd4..ded6b2e950 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp index 6bfa6d9117..6177740d4e 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp +++ b/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); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp index d7bc88e4ef..28b67b6168 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp +++ b/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); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp index 90513f0b40..c19f5894b5 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp +++ b/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); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp index 56c59ee421..7d5f07696f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp index 5a5bf5e64d..d97e969f66 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp index e8d75238f8..14cc2b39f3 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp index 2af64c5267..d88011752d 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp index 0fb98bc13f..bb8bc6672f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp index e6f20cc9fd..60086b4274 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp +++ b/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); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp index 3bb8d350e1..711028c645 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp +++ b/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;