openpilot v0.9.6 release

date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6

rednose/templates/ekf_c.c

@@ -0,0 +1,123 @@
+#include <eigen3/Eigen/Dense>
+#include <iostream>
+
+typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
+typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
+typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;
+
+void predict(double *in_x, double *in_P, double *in_Q, double dt) {
+  typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> RRM;
+
+  double nx[DIM] = {0};
+  double in_F[EDIM*EDIM] = {0};
+
+  // functions from sympy
+  f_fun(in_x, dt, nx);
+  F_fun(in_x, dt, in_F);
+
+
+  EEM F(in_F);
+  EEM P(in_P);
+  EEM Q(in_Q);
+
+  RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
+  P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
+  P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
+  P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();
+
+  P = P + dt*Q;
+
+  // copy out state
+  memcpy(in_x, nx, DIM * sizeof(double));
+  memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+}
+
+// note: extra_args dim only correct when null space projecting
+// otherwise 1
+template <int ZDIM, int EADIM, bool MAHA_TEST>
+void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
+  typedef Eigen::Matrix<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
+  typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
+  typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
+  //typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
+  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
+  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXM;
+
+  double in_hx[ZDIM] = {0};
+  double in_H[ZDIM * DIM] = {0};
+  double in_H_mod[EDIM * DIM] = {0};
+  double delta_x[EDIM] = {0};
+  double x_new[DIM] = {0};
+
+
+  // state x, P
+  Eigen::Matrix<double, ZDIM, 1> z(in_z);
+  EEM P(in_P);
+  ZZM pre_R(in_R);
+
+  // functions from sympy
+  h_fun(in_x, in_ea, in_hx);
+  H_fun(in_x, in_ea, in_H);
+  ZDM pre_H(in_H);
+
+  // get y (y = z - hx)
+  Eigen::Matrix<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
+  X1M y; XXM H; XXM R;
+  if (Hea_fun){
+    typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> ZAM;
+    double in_Hea[ZDIM * EADIM] = {0};
+    Hea_fun(in_x, in_ea, in_Hea);
+    ZAM Hea(in_Hea);
+    XXM A = Hea.transpose().fullPivLu().kernel();
+
+
+    y = A.transpose() * pre_y;
+    H = A.transpose() * pre_H;
+    R = A.transpose() * pre_R * A;
+  } else {
+    y = pre_y;
+    H = pre_H;
+    R = pre_R;
+  }
+  // get modified H
+  H_mod_fun(in_x, in_H_mod);
+  DEM H_mod(in_H_mod);
+  XEM H_err = H * H_mod;
+
+  // Do mahalobis distance test
+  if (MAHA_TEST){
+    XXM a = (H_err * P * H_err.transpose() + R).inverse();
+    double maha_dist = y.transpose() * a * y;
+    if (maha_dist > MAHA_THRESHOLD){
+      R = 1.0e16 * R;
+    }
+  }
+
+  // Outlier resilient weighting
+  double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
+
+  // kalman gains and I_KH
+  XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
+  XEM KT = S.fullPivLu().solve(H_err * P.transpose());
+  //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
+  //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
+  //std::cout << "Here is the matrix rot:\n" << K << std::endl;
+  EEM I_KH = Eigen::Matrix<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);
+
+  // update state by injecting dx
+  Eigen::Matrix<double, EDIM, 1> dx(delta_x);
+  dx  = (KT.transpose() * y);
+  memcpy(delta_x, dx.data(), EDIM * sizeof(double));
+  err_fun(in_x, delta_x, x_new);
+  Eigen::Matrix<double, DIM, 1> x(x_new);
+
+  // update cov
+  P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);
+
+  // copy out state
+  memcpy(in_x, x.data(), DIM * sizeof(double));
+  memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+  memcpy(in_z, y.data(), y.rows() * sizeof(double));
+}
+
+