dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: remove old yaw estimator generated code

This commit is contained in:
bresch 2022-12-06 12:12:54 +01:00 committed by Daniel Agar
parent 0388c161e7
commit 86a3d2459a
2 changed files with 0 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,21 +0,0 @@
// Equations for covariance matrix prediction
const float S0 = cosf(psi);
const float S1 = (S0)*(S0);
const float S2 = sinf(psi);
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;
const float S7 = S0*S2;
const float S8 = P(0,1) + S7*dvxVar - S7*dvyVar;
const float S9 = P(1,2) + P(2,2)*S6;
_ekf_gsf[model_index].P(0,0) = P(0,0) - P(0,2)*S4 + S1*dvxVar + S3*dvyVar - S4*S5;
_ekf_gsf[model_index].P(0,1) = -P(1,2)*S4 + S5*S6 + S8;
_ekf_gsf[model_index].P(1,1) = P(1,1) + P(1,2)*S6 + S1*dvyVar + S3*dvxVar + S6*S9;
_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;

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

@ -1,62 +0,0 @@
// Intermediate variables
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 + (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;
const float t7 = P(0,0) + velObsVar;
const float t8 = P(0,0)*t4 + t1;
const float t9 = t5*velObsVar;
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 = 1.0F/((t2)*(t2));
const float t15 = t4*velObsVar + t8;
const float t16 = t14*t15;
const float t17 = t14*(t13 + t7*t8);
const float t18 = t10*t14;
const float t19 = P(0,1)*t12;
const float t20 = -t10*t4 + t13;
const float t21 = t14*t20;
const float t22 = P(1,1)*t7 + t1 + t7*velObsVar;
const float t23 = t14*t8;
const float t24 = t14*t22;
const float t25 = P(0,1)*t11;
const float t26 = t12*t4 + t25;
const float t27 = t14*t26;
const float t28 = P(0,1)*velObsVar;
const float t29 = t11*t7 + t19;
const float t30 = t14*t29;
// Equations for NE velocity innovation variance's determinante inverse
_ekf_gsf[model_index].S_det_inverse = t3;
// Equations for NE velocity innovation variance inverse
_ekf_gsf[model_index].S_inverse(0,0) = t3*t4;
_ekf_gsf[model_index].S_inverse(0,1) = t6;
_ekf_gsf[model_index].S_inverse(1,1) = t3*t7;
// Equations for NE velocity Kalman gain
K(0,0) = t3*t8;
K(1,0) = t9;
K(2,0) = -t11*t3;
K(0,1) = t9;
K(1,1) = -t10*t3;
K(2,1) = -t12*t3;
// Equations for covariance matrix update
_ekf_gsf[model_index].P(0,0) = P(0,0) - t13*t16 - t17*t8;
_ekf_gsf[model_index].P(0,1) = P(0,1)*(t15*t18 - t17*velObsVar + 1);
_ekf_gsf[model_index].P(1,1) = P(1,1) - t13*t24 + t18*t20;
_ekf_gsf[model_index].P(0,2) = P(0,2) + t11*t17 + t16*t19;
_ekf_gsf[model_index].P(1,2) = P(1,2) + t12*t21 + t24*t25;
_ekf_gsf[model_index].P(2,2) = P(2,2) - t11*t30 - t12*t27;