zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ekf2_derivation: automatically replace pow function

main
bresch 3 years ago committed by Mathieu Bresciani
parent
11f78a3686
commit
78dd557b26
18 changed files with 98 additions and 87 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 13
      src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py
  2. 6
      src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp
  3. 6
      src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp
  4. 6
      src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp
  5. 8
      src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp
  6. 10
      src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp
  7. 8
      src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp
  8. 14
      src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp
  9. 20
      src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp
  10. 10
      src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp
  11. 14
      src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp
  12. 12
      src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp
  13. 6
      src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp
  14. 6
      src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp
  15. 6
      src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp
  16. 4
      src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp
  17. 8
      src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp
  18. 28
      src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp

13
src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py
Unescape View File

@ -17,7 +17,18 @@ class CodeGenerator:
self.file.write("// " + string + "\n") self.file.write("// " + string + "\n")
def get_ccode(self, expression): 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): def write_subexpressions(self,subexpressions):
write_string = "" write_string = ""

6
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp
Unescape View File

@ -5,7 +5,7 @@ const float HKX1 = magD*q3 + magE*q2;
const float HKX2 = 2*magN; const float HKX2 = 2*magN;
const float HKX3 = HKX2*q2 + magD*q0 - magE*q1; const float HKX3 = HKX2*q2 + magD*q0 - magE*q1;
const float HKX4 = -HKX2*q3 + magD*q1 + magE*q0; 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 HKX6 = q0*q3 + q1*q2;
const float HKX7 = q0*q2 - q1*q3; const float HKX7 = q0*q2 - q1*q3;
const float HKX8 = 2*HKX0; 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 HKY3 = magD*q3 + magN*q1;
const float HKY4 = HKY1*q3 - magD*q2 + magN*q0; const float HKY4 = HKY1*q3 - magD*q2 + magN*q0;
const float HKY5 = q0*q3 - q1*q2; 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 HKY7 = q0*q1 + q2*q3;
const float HKY8 = 2*HKY7; const float HKY8 = 2*HKY7;
const float HKY9 = 2*HKY3; 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 HKZ4 = magE*q2 + magN*q1;
const float HKZ5 = q0*q2 + q1*q3; const float HKZ5 = q0*q2 + q1*q3;
const float HKZ6 = q0*q1 - q2*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 HKZ8 = 2*HKZ0;
const float HKZ9 = 2*HKZ6; const float HKZ9 = 2*HKZ6;
const float HKZ10 = 2*HKZ2; const float HKZ10 = 2*HKZ2;

6
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp
Unescape View File

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

6
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp
Unescape View File

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

8
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp
Unescape View File

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

10
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp
Unescape View File

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

8
src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp
Unescape View File

@ -10,12 +10,12 @@ const float HK7 = 2*vd;
const float HK8 = q0*q3; const float HK8 = q0*q3;
const float HK9 = q1*q2; const float HK9 = q1*q2;
const float HK10 = 2*HK8 + 2*HK9; const float HK10 = 2*HK8 + 2*HK9;
const float HK11 = 2*powf(q3, 2) - 1; const float HK11 = 2*(q3)*(q3) - 1;
const float HK12 = HK11 + 2*powf(q2, 2); const float HK12 = HK11 + 2*(q2)*(q2);
const float HK13 = HK1*HK12 - HK10*HK4 + HK6*HK7; 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 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 HK17 = 2*HK1*(HK8 - HK9) - HK15*HK7 + HK16*HK4;
const float HK18 = HK14*HK17; const float HK18 = HK14*HK17;
const float HK19 = 2*HK14; const float HK19 = 2*HK14;

14
src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp
Unescape View File

@ -1,9 +1,9 @@
// Equations for covariance matrix prediction, without process noise! // 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 PS1 = 0.25F*daxVar;
const float PS2 = powf(q2, 2); const float PS2 = (q2)*(q2);
const float PS3 = 0.25F*dayVar; 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 PS5 = 0.25F*dazVar;
const float PS6 = 0.5F*q1; const float PS6 = 0.5F*q1;
const float PS7 = 0.5F*q2; const float PS7 = 0.5F*q2;
@ -94,7 +94,7 @@ const float PS91 = PS59 + PS82;
const float PS92 = PS69 + PS79; const float PS92 = PS69 + PS79;
const float PS93 = PS49 - 2*PS51 + PS53; 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 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 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 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; 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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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; 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;

20
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp
Unescape View File

@ -8,7 +8,7 @@ const float HK4 = Tbs(1,0)*q3;
const float HK5 = Tbs(1,2)*q1; const float HK5 = Tbs(1,2)*q1;
const float HK6 = -HK5; const float HK6 = -HK5;
const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3); 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 HK9 = 2*HK8;
const float HK10 = Tbs(1,1)*q2; const float HK10 = Tbs(1,1)*q2;
const float HK11 = Tbs(1,2)*q3; 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 HK22 = q0*q3;
const float HK23 = q1*q2; const float HK23 = q1*q2;
const float HK24 = 2*Tbs(1,1); const float HK24 = 2*Tbs(1,1);
const float HK25 = 2*powf(q2, 2); const float HK25 = 2*(q2)*(q2);
const float HK26 = 2*powf(q3, 2) - 1; const float HK26 = 2*(q3)*(q3) - 1;
const float HK27 = -HK21*(HK19 + HK20) + HK24*(HK22 - HK23) + Tbs(1,0)*(HK25 + HK26); const float HK27 = -HK21*(HK19 + HK20) + HK24*(HK22 - HK23) + Tbs(1,0)*(HK25 + HK26);
const float HK28 = 2*Tbs(1,0); const float HK28 = 2*Tbs(1,0);
const float HK29 = q0*q1; const float HK29 = q0*q1;
const float HK30 = q2*q3; 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 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 HK33 = -HK24*(HK29 + HK30) + HK28*(HK19 - HK20) + Tbs(1,2)*(HK25 + HK31 - 1);
const float HK34 = 2*HK7; 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 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 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 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); 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 HK7 = Tbs(0,2)*q1;
const float HK8 = -HK7; const float HK8 = -HK7;
const float HK9 = HK6 + HK8; const float HK9 = HK6 + HK8;
const float HK10 = 1.0F/range; const float HK10 = 1.0F/(range);
const float HK11 = 2*HK10; const float HK11 = 2*HK10;
const float HK12 = Tbs(0,2)*q0; const float HK12 = Tbs(0,2)*q0;
const float HK13 = -HK0 + 2*HK1 + HK12; const float HK13 = -HK0 + 2*HK1 + HK12;
@ -133,15 +133,15 @@ const float HK27 = q0*q3;
const float HK28 = q1*q2; const float HK28 = q1*q2;
const float HK29 = HK27 - HK28; const float HK29 = HK27 - HK28;
const float HK30 = 2*Tbs(0,1); const float HK30 = 2*Tbs(0,1);
const float HK31 = 2*powf(q2, 2); const float HK31 = 2*(q2)*(q2);
const float HK32 = 2*powf(q3, 2) - 1; const float HK32 = 2*(q3)*(q3) - 1;
const float HK33 = HK31 + HK32; const float HK33 = HK31 + HK32;
const float HK34 = HK27 + HK28; const float HK34 = HK27 + HK28;
const float HK35 = q0*q1; const float HK35 = q0*q1;
const float HK36 = q2*q3; const float HK36 = q2*q3;
const float HK37 = HK35 - HK36; const float HK37 = HK35 - HK36;
const float HK38 = 2*Tbs(0,2); 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 HK40 = HK32 + HK39;
const float HK41 = HK35 + HK36; const float HK41 = HK35 + HK36;
const float HK42 = HK24 - HK25; 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 HK50 = -2*HK19*vd + 2*HK21;
const float HK51 = -2*HK22*vn + 2*HK23; 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 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 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 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); 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);

10
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp
Unescape View File

@ -7,7 +7,7 @@ const float HK4 = Tbs(1,0)*q3;
const float HK5 = Tbs(1,2)*q1; const float HK5 = Tbs(1,2)*q1;
const float HK6 = -HK5; const float HK6 = -HK5;
const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3); 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 HK9 = 2*HK8;
const float HK10 = Tbs(1,1)*q2; const float HK10 = Tbs(1,1)*q2;
const float HK11 = Tbs(1,2)*q3; const float HK11 = Tbs(1,2)*q3;
@ -26,8 +26,8 @@ const float HK23 = q0*q3;
const float HK24 = q1*q2; const float HK24 = q1*q2;
const float HK25 = HK23 - HK24; const float HK25 = HK23 - HK24;
const float HK26 = 2*Tbs(1,1); const float HK26 = 2*Tbs(1,1);
const float HK27 = 2*powf(q2, 2); const float HK27 = 2*(q2)*(q2);
const float HK28 = 2*powf(q3, 2) - 1; const float HK28 = 2*(q3)*(q3) - 1;
const float HK29 = HK27 + HK28; const float HK29 = HK27 + HK28;
const float HK30 = -HK21*HK22 + HK25*HK26 + HK29*Tbs(1,0); const float HK30 = -HK21*HK22 + HK25*HK26 + HK29*Tbs(1,0);
const float HK31 = HK23 + HK24; const float HK31 = HK23 + HK24;
@ -35,7 +35,7 @@ const float HK32 = 2*Tbs(1,0);
const float HK33 = q0*q1; const float HK33 = q0*q1;
const float HK34 = q2*q3; const float HK34 = q2*q3;
const float HK35 = HK33 - HK34; 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 HK37 = HK28 + HK36;
const float HK38 = HK22*HK35 - HK31*HK32 + HK37*Tbs(1,1); const float HK38 = HK22*HK35 - HK31*HK32 + HK37*Tbs(1,1);
const float HK39 = HK33 + HK34; 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 HK42 = -HK26*HK39 + HK32*HK40 + HK41*Tbs(1,2);
const float HK43 = 2*HK7; 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 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 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 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); 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);

14
src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp
Unescape View File

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

12
src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp
Unescape View File

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

6
src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp
Unescape View File

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

6
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp
Unescape View File

@ -4,7 +4,7 @@ const float HK1 = q2*ve + q3*vd;
const float HK2 = 2*vn; const float HK2 = 2*vn;
const float HK3 = HK2*q2 + q0*vd - q1*ve; const float HK3 = HK2*q2 + q0*vd - q1*ve;
const float HK4 = -HK2*q3 + q0*ve + q1*vd; 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 HK6 = q0*q3 + q1*q2;
const float HK7 = q0*q2 - q1*q3; const float HK7 = q0*q2 - q1*q3;
const float HK8 = 2*HK1; 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 HK3 = q1*vn + q3*vd;
const float HK4 = HK1*q3 + q0*vn - q2*vd; const float HK4 = HK1*q3 + q0*vn - q2*vd;
const float HK5 = q0*q3 - q1*q2; 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 HK7 = q0*q1 + q2*q3;
const float HK8 = 2*HK3; const float HK8 = 2*HK3;
const float HK9 = 2*HK7; 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 HK4 = q1*vn + q2*ve;
const float HK5 = q0*q2 + q1*q3; const float HK5 = q0*q2 + q1*q3;
const float HK6 = q0*q1 - q2*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 HK8 = 2*HK4;
const float HK9 = 2*HK5; const float HK9 = 2*HK5;
const float HK10 = 2*HK0; const float HK10 = 2*HK0;

6
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp
Unescape View File

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

4
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp
Unescape View File

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

8
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp
Unescape View File

@ -1,8 +1,8 @@
// Intermediate variables // 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 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 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 t3 = 1.0F/(t2);
const float t4 = P(1,1) + velObsVar; const float t4 = P(1,1) + velObsVar;
const float t5 = P(0,1)*t3; const float t5 = P(0,1)*t3;
const float t6 = -t5; 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 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 t12 = P(0,1)*P(0,2) - P(1,2)*t7;
const float t13 = t0*velObsVar; 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 t15 = t4*velObsVar + t8;
const float t16 = t14*t15; const float t16 = t14*t15;
const float t17 = t14*(t13 + t7*t8); const float t17 = t14*(t13 + t7*t8);

28
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp
Unescape View File

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

Write Preview
Loading…
Cancel
Save