openpilot v0.9.6 release

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

rednose/helpers/ekf.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <iostream>
+#include <cassert>
+#include <string>
+#include <vector>
+#include <deque>
+#include <unordered_map>
+#include <map>
+#include <cmath>
+
+#include <eigen3/Eigen/Dense>
+
+typedef void (*extra_routine_t)(double *, double *);
+
+struct EKF {
+  std::string name;
+  std::vector<int> kinds;
+  std::vector<int> feature_kinds;
+
+  void (*f_fun)(double *, double, double *);
+  void (*F_fun)(double *, double, double *);
+  void (*err_fun)(double *, double *, double *);
+  void (*inv_err_fun)(double *, double *, double *);
+  void (*H_mod_fun)(double *, double *);
+  void (*predict)(double *, double *, double *, double);
+  std::unordered_map<int, void (*)(double *, double *, double *)> hs = {};
+  std::unordered_map<int, void (*)(double *, double *, double *)> Hs = {};
+  std::unordered_map<int, void (*)(double *, double *, double *, double *, double *)> updates = {};
+  std::unordered_map<int, void (*)(double *, double *, double *)> Hes = {};
+  std::unordered_map<std::string, void (*)(double)> sets = {};
+  std::unordered_map<std::string, extra_routine_t> extra_routines = {};
+};
+
+#define ekf_lib_init(ekf) \
+extern "C" void* ekf_get() { \
+  return (void*) &ekf; \
+} \
+extern void  __attribute__((weak)) ekf_register(const EKF* ptr); \
+static void __attribute__((constructor)) do_ekf_init_ ## ekf(void) { \
+  if (ekf_register) ekf_register(&ekf); \
+}