fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 16:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Whitespace cleanup.

Browse Source
This commit is contained in:
mcsauder
2021-10-01 11:06:15 -06:00
committed by Daniel Agar
parent 4cf8eb8226
commit 21163d859e
76 changed files with 300 additions and 367 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/__init__.py
-1
View File
@@ -5,4 +5,3 @@ Created on Sat Mar 14 13:02:26 2020
@author: roman
"""
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp
+19 -19
View File
@@ -14,7 +14,7 @@ int main()
{
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
float Hfusion[24];
float Hfusion[24];
Vector24f H_MAG;
Vector24f Kfusion;
float mag_innov_var;
@@ -30,11 +30,11 @@ int main()
q2 /= length;
q3 /= length;
const float magN = 2.0f * ((float)rand() - 0.5f);
const float magE = 2.0f * ((float)rand() - 0.5f);
const float magD = 2.0f * ((float)rand() - 0.5f);
const float magN = 2.0f * ((float)rand() - 0.5f);
const float magE = 2.0f * ((float)rand() - 0.5f);
const float magD = 2.0f * ((float)rand() - 0.5f);
const float R_MAG = sq(0.05f);
const float R_MAG = sq(0.05f);
const bool update_all_states = true;
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
@@ -50,8 +50,8 @@ int main()
}
// common expressions used by sympy generated equations
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainss
const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainss
const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
const float HKX1 = magD*q3 + magE*q2 + magN*q1;
const float HKX2 = magE*q1;
const float HKX3 = magD*q0;
@@ -78,16 +78,16 @@ int main()
const float HKX24 = 1.0F/(HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG);
// common expressions used by matlab generated equations
float SH_MAG[9];
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
SH_MAG[3] = sq(q3);
SH_MAG[4] = sq(q2);
SH_MAG[5] = sq(q1);
SH_MAG[6] = sq(q0);
SH_MAG[7] = 2.0f*magN*q0;
SH_MAG[8] = 2.0f*magE*q3;
float SH_MAG[9];
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
SH_MAG[3] = sq(q3);
SH_MAG[4] = sq(q2);
SH_MAG[5] = sq(q1);
SH_MAG[6] = sq(q0);
SH_MAG[7] = 2.0f*magN*q0;
SH_MAG[8] = 2.0f*magE*q3;
// Compare X axis equations
{
@@ -139,8 +139,8 @@ int main()
}
// repeat calculation using matlab generated equations
// X axis innovation variance
mag_innov_var = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
// X axis innovation variance
mag_innov_var = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
// Calculate X axis observation jacobians
H_MAG.setZero();
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp
-2
View File
@@ -340,5 +340,3 @@ Kfusion(20) = HKZ70*(-HKZ13*P(16,20) - HKZ15*P(3,20) + HKZ17*P(0,20) + HKZ19*P(1
Kfusion(21) = HKZ69*HKZ70;
Kfusion(22) = HKZ70*(-HKZ13*P(16,22) - HKZ15*P(3,22) + HKZ17*P(0,22) + HKZ19*P(17,22) + HKZ21*P(18,22) - HKZ23*P(2,22) + HKZ25*P(1,22) - P(21,22));
Kfusion(23) = HKZ70*(-HKZ13*P(16,23) - HKZ15*P(3,23) + HKZ17*P(0,23) + HKZ19*P(17,23) + HKZ21*P(18,23) - HKZ23*P(2,23) + HKZ25*P(1,23) - P(21,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp
-2
View File
@@ -45,5 +45,3 @@ Hfusion.at<2>() = IV18*P(16,21) + IV18*(IV18*P(16,16) + IV19*P(3,16) - IV20*P(0,
// Kalman gains - axis 2
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp
-2
View File
@@ -191,5 +191,3 @@ Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20)
Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) + HK9*P(21,23) - HK9*P(5,21));
Kfusion(22) = -HK22*HK32;
Kfusion(23) = -HK27*HK32;
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp
-2
View File
@@ -198,5 +198,3 @@ Kfusion(23) = -HK79*HK84;
// Kalman gains - axis 2
src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp
-2
View File
@@ -109,5 +109,3 @@ Kfusion(20) = HK55*(-HK24*P(0,20) - HK34*P(2,20) - HK35*P(3,20) - HK40*P(5,20) -
Kfusion(21) = HK55*(-HK24*P(0,21) - HK34*P(2,21) - HK35*P(3,21) - HK40*P(5,21) - HK41*P(6,21) + HK42*P(21,22) - HK43*P(21,23) - HK44*P(4,21) - HK45*P(1,21));
Kfusion(22) = HK48*HK55;
Kfusion(23) = HK49*HK55;
src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp
-2
View File
@@ -524,5 +524,3 @@ nextP(20,23) = P(20,23);
nextP(21,23) = P(21,23);
nextP(22,23) = P(22,23);
nextP(23,23) = P(23,23);
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp
-2
View File
@@ -226,5 +226,3 @@ Kfusion(20) = -HK66*(HK47*P(0,20) + HK50*P(4,20) + HK52*P(5,20) + HK53*P(6,20) +
Kfusion(21) = -HK66*(HK47*P(0,21) + HK50*P(4,21) + HK52*P(5,21) + HK53*P(6,21) + HK54*P(1,21) + HK55*P(2,21) + HK56*P(3,21));
Kfusion(22) = -HK66*(HK47*P(0,22) + HK50*P(4,22) + HK52*P(5,22) + HK53*P(6,22) + HK54*P(1,22) + HK55*P(2,22) + HK56*P(3,22));
Kfusion(23) = -HK66*(HK47*P(0,23) + HK50*P(4,23) + HK52*P(5,23) + HK53*P(6,23) + HK54*P(1,23) + HK55*P(2,23) + HK56*P(3,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp
-2
View File
@@ -218,5 +218,3 @@ Kfusion(20) = -HK110*(HK100*P(2,20) + HK101*P(3,20) + HK92*P(0,20) + HK95*P(4,20
Kfusion(21) = -HK110*(HK100*P(2,21) + HK101*P(3,21) + HK92*P(0,21) + HK95*P(4,21) + HK97*P(5,21) + HK98*P(6,21) + HK99*P(1,21));
Kfusion(22) = -HK110*(HK100*P(2,22) + HK101*P(3,22) + HK92*P(0,22) + HK95*P(4,22) + HK97*P(5,22) + HK98*P(6,22) + HK99*P(1,22));
Kfusion(23) = -HK110*(HK100*P(2,23) + HK101*P(3,23) + HK92*P(0,23) + HK95*P(4,23) + HK97*P(5,23) + HK98*P(6,23) + HK99*P(1,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp
-2
View File
@@ -80,5 +80,3 @@ Kfusion(20) = HK26*(HK12*P(0,20) - HK18*P(1,20) - HK19*P(2,20) + HK20*P(3,20));
Kfusion(21) = HK26*(HK12*P(0,21) - HK18*P(1,21) - HK19*P(2,21) + HK20*P(3,21));
Kfusion(22) = HK26*(HK12*P(0,22) - HK18*P(1,22) - HK19*P(2,22) + HK20*P(3,22));
Kfusion(23) = HK26*(HK12*P(0,23) - HK18*P(1,23) - HK19*P(2,23) + HK20*P(3,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp
-2
View File
@@ -63,5 +63,3 @@ Kfusion(20) = -HK9*(HK5*P(16,20) - P(17,20));
Kfusion(21) = -HK9*(HK5*P(16,21) - P(17,21));
Kfusion(22) = -HK9*(HK5*P(16,22) - P(17,22));
Kfusion(23) = -HK9*(HK5*P(16,23) - P(17,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp
+19 -19
View File
@@ -23,18 +23,18 @@ int main()
Vector24f Hfusion_matlab;
Vector24f Kfusion_matlab;
const float R_TAS = sq(1.5f);
const float R_TAS = sq(1.5f);
const bool update_wind_only = false;
// get latest velocity in earth frame
const float vn = 9.0f;
const float ve = 12.0f;
const float vd = -1.5f;
// get latest velocity in earth frame
const float vn = 9.0f;
const float ve = 12.0f;
const float vd = -1.5f;
// get latest wind velocity in earth frame
const float vwn = -4.0f;
const float vwe = 3.0f;
// get latest wind velocity in earth frame
const float vwn = -4.0f;
const float vwe = 3.0f;
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
@@ -77,7 +77,7 @@ int main()
const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
// Observation Jacobians
SparseVector24f<4,5,6,22,23> Hfusion;
SparseVector24f<4,5,6,22,23> Hfusion;
Hfusion.at<4>() = HK4;
Hfusion.at<5>() = HK5;
Hfusion.at<6>() = HK3*vd;
@@ -189,11 +189,11 @@ int main()
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction);
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction);
}
// find largest Kalman gain difference as a fraction of the matlab value
max_diff_fraction = 0.0f;
@@ -217,11 +217,11 @@ int main()
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);
}
return 0;
}
src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp
-2
View File
@@ -70,5 +70,3 @@ Kfusion(20) = HK16*(-HK0*P(20,22) + HK0*P(4,20) - HK1*P(20,23) + HK1*P(5,20) + P
Kfusion(21) = HK16*(-HK0*P(21,22) + HK0*P(4,21) - HK1*P(21,23) + HK1*P(5,21) + P(6,21)*vd);
Kfusion(22) = HK15*HK16;
Kfusion(23) = HK14*HK16;
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp
-2
View File
@@ -244,5 +244,3 @@ Kfusion(20) = -HK27*(-HK13*P(3,20) - HK14*P(4,20) + HK15*P(0,20) + HK16*P(5,20)
Kfusion(21) = -HK27*(-HK13*P(3,21) - HK14*P(4,21) + HK15*P(0,21) + HK16*P(5,21) - HK17*P(2,21) + HK18*P(6,21) + HK19*P(1,21));
Kfusion(22) = -HK27*(-HK13*P(3,22) - HK14*P(4,22) + HK15*P(0,22) + HK16*P(5,22) - HK17*P(2,22) + HK18*P(6,22) + HK19*P(1,22));
Kfusion(23) = -HK27*(-HK13*P(3,23) - HK14*P(4,23) + HK15*P(0,23) + HK16*P(5,23) - HK17*P(2,23) + HK18*P(6,23) + HK19*P(1,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp
-2
View File
@@ -247,5 +247,3 @@ Kfusion(20) = -HK85*(-HK71*P(3,20) - HK72*P(4,20) + HK73*P(0,20) + HK74*P(5,20)
Kfusion(21) = -HK85*(-HK71*P(3,21) - HK72*P(4,21) + HK73*P(0,21) + HK74*P(5,21) - HK75*P(2,21) + HK76*P(6,21) + HK77*P(1,21));
Kfusion(22) = -HK85*(-HK71*P(3,22) - HK72*P(4,22) + HK73*P(0,22) + HK74*P(5,22) - HK75*P(2,22) + HK76*P(6,22) + HK77*P(1,22));
Kfusion(23) = -HK85*(-HK71*P(3,23) - HK72*P(4,23) + HK73*P(0,23) + HK74*P(5,23) - HK75*P(2,23) + HK76*P(6,23) + HK77*P(1,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp
-2
View File
@@ -17,5 +17,3 @@ _ekf_gsf[model_index].P(1,1) = P(1,1) + P(1,2)*S6 + S1*dvyVar + S3*dvxVar + S6*S
_ekf_gsf[model_index].P(0,2) = S5;
_ekf_gsf[model_index].P(1,2) = S9;
_ekf_gsf[model_index].P(2,2) = P(2,2) + dazVar;
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp
-2
View File
@@ -64,5 +64,3 @@ _ekf_gsf[model_index].P(1,1) = P(1,1) - t16*t30 + t22*t26;
_ekf_gsf[model_index].P(0,2) = P(0,2) + t19*t24 + t20*t25;
_ekf_gsf[model_index].P(1,2) = P(1,2) + t23*t27 + t30*t31;
_ekf_gsf[model_index].P(2,2) = P(2,2) - t23*t33 - t25*t36;
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp
-2
View File
@@ -148,5 +148,3 @@ H_YAW.at<20>() = 0;
H_YAW.at<21>() = 0;
H_YAW.at<22>() = 0;
H_YAW.at<23>() = 0;
src/modules/ekf2/EKF/python/terrain_flow_derivation/derive_terrain_flow.py
-1
View File
@@ -95,4 +95,3 @@ H_y_simple = cse(H_y, symbols('t0:30'))
write_simplified(H_x_simple, "flow_x_observation.txt", "Hx")
write_simplified(H_y_simple, "flow_y_observation.txt", "Hy")
src/modules/ekf2/EKF/python/wind_cov_init/derivation.py
+2 -2
View File
@@ -29,7 +29,7 @@ R_tas_init = 1.4**2
#########################################################################################################################
# define symbols: true airspeed, sidslip angle,
# define symbols: true airspeed, sidslip angle,
V, beta, yaw, groundspeed_body_x, groundspeed_body_y = symbols('V beta yaw vx_body vy_body')
R_tas, R_beta, R_yaw = symbols('R_tas R_beta R_yaw')
@@ -64,4 +64,4 @@ P_wind_earth_numeric = P_wind_earth_numeric.subs([(groundspeed_body_x, groundspe
print('P[22][22] = ' + str(P_wind_earth_numeric[0,0]))
print('P[22][23] = ' + str(P_wind_earth_numeric[0,1]))
print('P[23][22] = ' + str(P_wind_earth_numeric[1,0]))
print('P[23][23] = ' + str(P_wind_earth_numeric[1,1]))
print('P[23][23] = ' + str(P_wind_earth_numeric[1,1]))