From 1f386c1bb8d8a294e1b556573f1de9e76d20d227 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Sat, 15 Aug 2020 08:37:14 +1000
Subject: [PATCH] EKF: Add yaw estimate SymPy generated code files

---
 ...imator_covariance_prediction_generated.cpp | 21 ++++++
 ...estimator_measurement_update_generated.cpp | 68 +++++++++++++++++++
 2 files changed, 89 insertions(+)
 create mode 100644 EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp
 create mode 100644 EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp

diff --git a/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp b/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp
new file mode 100644
index 0000000000..0fb98bc13f
--- /dev/null
+++ b/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp
@@ -0,0 +1,21 @@
+// Equations for covariance matrix prediction
+const float S0 = cosf(psi);
+const float S1 = powf(S0, 2);
+const float S2 = sinf(psi);
+const float S3 = powf(S2, 2);
+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;
+
+
diff --git a/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp b/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp
new file mode 100644
index 0000000000..07a1db0e72
--- /dev/null
+++ b/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp
@@ -0,0 +1,68 @@
+// Intermediate variables
+const float t0 = powf(P(0,1), 2);
+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 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;
+const float t11 = t1 + t10;
+const float t12 = P(0,1)*P(1,2);
+const float t13 = P(0,2)*t4;
+const float t14 = P(0,1)*P(0,2);
+const float t15 = P(1,2)*t7;
+const float t16 = t0*velObsVar;
+const float t17 = powf(t2, -2);
+const float t18 = t4*velObsVar + t8;
+const float t19 = t17*t18;
+const float t20 = t17*(t16 + t7*t8);
+const float t21 = t0 - t10;
+const float t22 = t17*t21;
+const float t23 = t14 - t15;
+const float t24 = P(0,1)*t23;
+const float t25 = t12 - t13;
+const float t26 = t16 - t21*t4;
+const float t27 = t17*t26;
+const float t28 = t11 + t7*velObsVar;
+const float t29 = t17*t8;
+const float t30 = t17*t28;
+const float t31 = P(0,1)*t25;
+const float t32 = t23*t4 + t31;
+const float t33 = t17*t32;
+const float t34 = P(0,1)*velObsVar;
+const float t35 = t24 + t25*t7;
+const float t36 = t17*t35;
+
+
+// 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) = t3*(-t12 + t13);
+K(0,1) = t9;
+K(1,1) = t11*t3;
+K(2,1) = t3*(-t14 + t15);
+
+
+// Equations for covariance matrix update
+_ekf_gsf[model_index].P(0,0) = P(0,0) - t16*t19 - t20*t8;
+_ekf_gsf[model_index].P(0,1) = P(0,1)*(t18*t22 - t20*velObsVar + 1);
+_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;
+
+