wip

rednose/SConscript

@@ -0,0 +1,17 @@
+Import('env', 'envCython', 'common')
+
+cc_sources = [
+  "helpers/ekf_load.cc",
+  "helpers/ekf_sym.cc",
+]
+libs = ["dl"]
+if common != "":
+  # for SWAGLOG support
+  libs += [common, 'zmq']
+
+ekf_objects = env.SharedObject(cc_sources)
+rednose = env.Library("helpers/ekf_sym", ekf_objects, LIBS=libs)
+rednose_python = envCython.Program("helpers/ekf_sym_pyx.so", ["helpers/ekf_sym_pyx.pyx", ekf_objects],
+                                   LIBS=libs + envCython["LIBS"])
+
+Export('rednose', 'rednose_python')

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); \
+}

rednose/helpers/ekf_load.cc

@@ -0,0 +1,39 @@
+#include "ekf_load.h"
+#include <dlfcn.h>
+
+std::vector<const EKF*>& ekf_get_all() {
+  static std::vector<const EKF*> vec;
+  return vec;
+}
+
+void ekf_register(const EKF* ekf) {
+  ekf_get_all().push_back(ekf);
+}
+
+const EKF* ekf_lookup(const std::string& ekf_name) {
+  for (const auto& ekfi : ekf_get_all()) {
+    if (ekf_name == ekfi->name) {
+      return ekfi;
+    }
+  }
+  return NULL;
+}
+
+void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name) {
+  if (ekf_lookup(ekf_name)) {
+    return;
+  }
+
+#ifdef __APPLE__
+  std::string dylib_ext = ".dylib";
+#else
+  std::string dylib_ext = ".so";
+#endif
+  std::string ekf_path = ekf_directory + "/lib" + ekf_name + dylib_ext;
+  void* handle = dlopen(ekf_path.c_str(), RTLD_NOW);
+  assert(handle);
+  void* (*ekf_get)() = (void*(*)())dlsym(handle, "ekf_get");
+  assert(ekf_get != NULL);
+  const EKF* ekf = (const EKF*)ekf_get();
+  ekf_register(ekf);
+}

rednose/helpers/ekf_load.h

@@ -0,0 +1,9 @@
+#include <vector>
+#include <string>
+
+#include "ekf.h"
+
+std::vector<const EKF*>& ekf_get_all();
+const EKF* ekf_lookup(const std::string& ekf_name);
+void ekf_register(const EKF* ekf);
+void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name);

rednose/helpers/ekf_sym.cc

@@ -0,0 +1,223 @@
+#include "ekf_sym.h"
+#include "logger/logger.h"
+
+using namespace EKFS;
+using namespace Eigen;
+
+EKFSym::EKFSym(std::string name, Map<MatrixXdr> Q, Map<VectorXd> x_initial, Map<MatrixXdr> P_initial, int dim_main,
+    int dim_main_err, int N, int dim_augment, int dim_augment_err, std::vector<int> maha_test_kinds,
+    std::vector<int> quaternion_idxs, std::vector<std::string> global_vars, double max_rewind_age)
+{
+  // TODO: add logger
+  this->ekf = ekf_lookup(name);
+  assert(this->ekf);
+
+  this->msckf = N > 0;
+  this->N = N;
+  this->dim_augment = dim_augment;
+  this->dim_augment_err = dim_augment_err;
+  this->dim_main = dim_main;
+  this->dim_main_err = dim_main_err;
+
+  this->dim_x = x_initial.rows();
+  this->dim_err = P_initial.rows();
+
+  assert(dim_main + dim_augment * N == dim_x);
+  assert(dim_main_err + dim_augment_err * N == this->dim_err);
+  assert(Q.rows() == P_initial.rows() && Q.cols() == P_initial.cols());
+
+  // kinds that should get mahalanobis distance
+  // tested for outlier rejection
+  this->maha_test_kinds = maha_test_kinds;
+
+  // quaternions need normalization
+  this->quaternion_idxs = quaternion_idxs;
+
+  this->global_vars = global_vars;
+
+  // Process noise
+  this->Q = Q;
+
+  this->max_rewind_age = max_rewind_age;
+  this->init_state(x_initial, P_initial, NAN);
+}
+
+void EKFSym::init_state(Map<VectorXd> state, Map<MatrixXdr> covs, double init_filter_time) {
+  this->x = state;
+  this->P = covs;
+  this->filter_time = init_filter_time;
+  this->augment_times = VectorXd::Zero(this->N);
+  this->reset_rewind();
+}
+
+VectorXd EKFSym::state() {
+  return this->x;
+}
+
+MatrixXdr EKFSym::covs() {
+  return this->P;
+}
+
+void EKFSym::set_filter_time(double t) {
+  this->filter_time = t;
+}
+
+double EKFSym::get_filter_time() {
+  return this->filter_time;
+}
+
+void EKFSym::normalize_quaternions() {
+  for(std::size_t i = 0; i < this->quaternion_idxs.size(); ++i) {
+    this->normalize_slice(this->quaternion_idxs[i], this->quaternion_idxs[i] + 4);
+  }
+}
+
+void EKFSym::normalize_slice(int slice_start, int slice_end_ex) {
+  this->x.block(slice_start, 0, slice_end_ex - slice_start, this->x.cols()).normalize();
+}
+
+void EKFSym::set_global(std::string global_var, double val) {
+  this->ekf->sets.at(global_var)(val);
+}
+
+std::optional<Estimate> EKFSym::predict_and_update_batch(double t, int kind, std::vector<Map<VectorXd>> z_map,
+    std::vector<Map<MatrixXdr>> R_map, std::vector<std::vector<double>> extra_args, bool augment)
+{
+  // TODO handle rewinding at this level
+
+  std::deque<Observation> rewound;
+  if (!std::isnan(this->filter_time) && t < this->filter_time) {
+    if (this->rewind_t.empty() || t < this->rewind_t.front() || t < this->rewind_t.back() - this->max_rewind_age) {
+      LOGD("observation too old at %f with filter at %f, ignoring!", t, this->filter_time);
+      return std::nullopt;
+    }
+    rewound = this->rewind(t);
+  }
+
+  Observation obs;
+  obs.t = t;
+  obs.kind = kind;
+  obs.extra_args = extra_args;
+  for (Map<VectorXd> zi : z_map) {
+    obs.z.push_back(zi);
+  }
+  for (Map<MatrixXdr> Ri : R_map) {
+    obs.R.push_back(Ri);
+  }
+
+  std::optional<Estimate> res = std::make_optional(this->predict_and_update_batch(obs, augment));
+
+  // optional fast forward
+  while (!rewound.empty()) {
+    this->predict_and_update_batch(rewound.front(), false);
+    rewound.pop_front();
+  }
+
+  return res;
+}
+
+void EKFSym::reset_rewind() {
+  this->rewind_obscache.clear();
+  this->rewind_t.clear();
+  this->rewind_states.clear();
+}
+
+std::deque<Observation> EKFSym::rewind(double t) {
+  std::deque<Observation> rewound;
+
+  // rewind observations until t is after previous observation
+  while (this->rewind_t.back() > t) {
+    rewound.push_front(this->rewind_obscache.back());
+    this->rewind_t.pop_back();
+    this->rewind_states.pop_back();
+    this->rewind_obscache.pop_back();
+  }
+
+  // set the state to the time right before that
+  this->filter_time = this->rewind_t.back();
+  this->x = this->rewind_states.back().first;
+  this->P = this->rewind_states.back().second;
+
+  return rewound;
+}
+
+void EKFSym::checkpoint(Observation& obs) {
+  // push to rewinder
+  this->rewind_t.push_back(this->filter_time);
+  this->rewind_states.push_back(std::make_pair(this->x, this->P));
+  this->rewind_obscache.push_back(obs);
+
+  // only keep a certain number around
+  if (this->rewind_t.size() > REWIND_TO_KEEP) {
+    this->rewind_t.pop_front();
+    this->rewind_states.pop_front();
+    this->rewind_obscache.pop_front();
+  }
+}
+
+Estimate EKFSym::predict_and_update_batch(Observation& obs, bool augment) {
+  assert(obs.z.size() == obs.R.size());
+  assert(obs.z.size() == obs.extra_args.size());
+
+  this->predict(obs.t);
+
+  Estimate res;
+  res.t = obs.t;
+  res.kind = obs.kind;
+  res.z = obs.z;
+  res.extra_args = obs.extra_args;
+  res.xk1 = this->x;
+  res.Pk1 = this->P;
+
+  // update batch
+  std::vector<VectorXd> y;
+  for (int i = 0; i < obs.z.size(); i++) {
+    assert(obs.z[i].rows() == obs.R[i].rows());
+    assert(obs.z[i].rows() == obs.R[i].cols());
+
+    // update state
+    y.push_back(this->update(obs.kind, obs.z[i], obs.R[i], obs.extra_args[i]));
+  }
+
+  res.xk = this->x;
+  res.Pk = this->P;
+  res.y = y;
+
+  assert(!augment); // TODO
+  // if (augment) {
+  //   this->augment();
+  // }
+
+  this->checkpoint(obs);
+
+  return res;
+}
+
+void EKFSym::predict(double t) {
+  // initialize time
+  if (std::isnan(this->filter_time)) {
+    this->filter_time = t;
+  }
+
+  // predict
+  double dt = t - this->filter_time;
+  assert(dt >= 0.0);
+
+  this->ekf->predict(this->x.data(), this->P.data(), this->Q.data(), dt);
+  this->normalize_quaternions();
+  this->filter_time = t;
+}
+
+VectorXd EKFSym::update(int kind, VectorXd z, MatrixXdr R, std::vector<double> extra_args) {
+  this->ekf->updates.at(kind)(this->x.data(), this->P.data(), z.data(), R.data(), extra_args.data());
+  this->normalize_quaternions();
+
+  if (this->msckf && std::find(this->feature_track_kinds.begin(), this->feature_track_kinds.end(), kind) != this->feature_track_kinds.end()) {
+    return z.head(z.rows() - extra_args.size());
+  }
+  return z;
+}
+
+extra_routine_t EKFSym::get_extra_routine(const std::string& routine) {
+  return this->ekf->extra_routines.at(routine);
+}

rednose/helpers/ekf_sym.h

@@ -0,0 +1,113 @@
+#pragma once
+
+#include <iostream>
+#include <cassert>
+#include <string>
+#include <vector>
+#include <deque>
+#include <unordered_map>
+#include <map>
+#include <cmath>
+#include <optional>
+
+#include <eigen3/Eigen/Dense>
+
+#include "ekf.h"
+#include "ekf_load.h"
+
+#define REWIND_TO_KEEP 512
+
+namespace EKFS {
+
+typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXdr;
+
+typedef struct Observation {
+  double t;
+  int kind;
+  std::vector<Eigen::VectorXd> z;
+  std::vector<MatrixXdr> R;
+  std::vector<std::vector<double>> extra_args;
+} Observation;
+
+typedef struct Estimate {
+  Eigen::VectorXd xk1;
+  Eigen::VectorXd xk;
+  MatrixXdr Pk1;
+  MatrixXdr Pk;
+  double t;
+  int kind;
+  std::vector<Eigen::VectorXd> y;
+  std::vector<Eigen::VectorXd> z;
+  std::vector<std::vector<double>> extra_args;
+} Estimate;
+
+class EKFSym {
+public:
+  EKFSym(std::string name, Eigen::Map<MatrixXdr> Q, Eigen::Map<Eigen::VectorXd> x_initial,
+      Eigen::Map<MatrixXdr> P_initial, int dim_main, int dim_main_err, int N = 0, int dim_augment = 0,
+      int dim_augment_err = 0, std::vector<int> maha_test_kinds = std::vector<int>(),
+      std::vector<int> quaternion_idxs = std::vector<int>(),
+      std::vector<std::string> global_vars = std::vector<std::string>(), double max_rewind_age = 1.0);
+  void init_state(Eigen::Map<Eigen::VectorXd> state, Eigen::Map<MatrixXdr> covs, double filter_time);
+
+  Eigen::VectorXd state();
+  MatrixXdr covs();
+  void set_filter_time(double t);
+  double get_filter_time();
+  void normalize_quaternions();
+  void normalize_slice(int slice_start, int slice_end_ex);
+  void set_global(std::string global_var, double val);
+  void reset_rewind();
+
+  void predict(double t);
+  std::optional<Estimate> predict_and_update_batch(double t, int kind, std::vector<Eigen::Map<Eigen::VectorXd>> z,
+      std::vector<Eigen::Map<MatrixXdr>> R, std::vector<std::vector<double>> extra_args = {{}}, bool augment = false);
+
+  extra_routine_t get_extra_routine(const std::string& routine);
+
+private:
+  std::deque<Observation> rewind(double t);
+  void checkpoint(Observation& obs);
+
+  Estimate predict_and_update_batch(Observation& obs, bool augment);
+  Eigen::VectorXd update(int kind, Eigen::VectorXd z, MatrixXdr R, std::vector<double> extra_args);
+
+  // stuct with linked sympy generated functions
+  const EKF *ekf = NULL;
+
+  Eigen::VectorXd x;  // state
+  MatrixXdr P;  // covs
+
+  bool msckf;
+  int N;
+  int dim_augment;
+  int dim_augment_err;
+  int dim_main;
+  int dim_main_err;
+
+  // state
+  int dim_x;
+  int dim_err;
+
+  double filter_time;
+
+  std::vector<int> maha_test_kinds;
+  std::vector<int> quaternion_idxs;
+
+  std::vector<std::string> global_vars;
+
+  // process noise
+  MatrixXdr Q;
+
+  // rewind stuff
+  double max_rewind_age;
+  std::deque<double> rewind_t;
+  std::deque<std::pair<Eigen::VectorXd, MatrixXdr>> rewind_states;
+  std::deque<Observation> rewind_obscache;
+
+  Eigen::VectorXd augment_times;
+
+  std::vector<int> feature_track_kinds;
+};
+
+}

rednose/helpers/ekf_sym.py

@@ -464,7 +464,7 @@ class EKF_sym():
     # rewind
     if self.filter_time is not None and t < self.filter_time:
       if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] - self.max_rewind_age:
-        self.logger.error("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time))
+        self.logger.error(f"observation too old at {t:.3f} with filter at {self.filter_time:.3f}, ignoring")
         return None
       rewound = self.rewind(t)
     else:

rednose/helpers/kalmanfilter.py

@@ -1,4 +1,4 @@
-from typing import Any, Dict
+from typing import Any
 
 import numpy as np
 
@@ -8,10 +8,10 @@ class KalmanFilter:
   initial_x = np.zeros((0, 0))
   initial_P_diag = np.zeros((0, 0))
   Q = np.zeros((0, 0))
-  obs_noise: Dict[int, Any] = {}
+  obs_noise: dict[int, Any] = {}
 
   # Should be initialized when initializating a KalmanFilter implementation
-  filter = None # noqa: A003
+  filter = None
 
   @property
   def x(self):

rednose/logger/logger.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#ifdef SWAGLOG
+#include SWAGLOG
+#else
+
+#define CLOUDLOG_DEBUG 10
+#define CLOUDLOG_INFO 20
+#define CLOUDLOG_WARNING 30
+#define CLOUDLOG_ERROR 40
+#define CLOUDLOG_CRITICAL 50
+
+#define cloudlog(lvl, fmt, ...) printf(fmt "\n", ## __VA_ARGS__)
+
+#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
+#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
+#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
+#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
+
+#endif