EKF_sym class rewritten to c++

.gitignore

@@ -2,12 +2,19 @@ generated/
 .sconsign.dblite
 *.swp
 
+# Cython intermediates
+*_pyx.cpp
+*_pyx.h
+*_pyx_api.h
+*.os
+
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]
 *$py.class
 
 # C extensions
+*.o
 *.so
 
 # Distribution / packaging

Dockerfile

@@ -8,9 +8,9 @@ ENV PATH="/root/.pyenv/bin:/root/.pyenv/shims:${PATH}"
 RUN pyenv install 3.7.3
 RUN pyenv global 3.7.3
 RUN pyenv rehash
-RUN pip3 install scons==3.1.1 pre-commit==2.4.0 pylint==2.5.2
+RUN pip3 install scons==3.1.1 pre-commit==2.4.0 pylint==2.5.2 Cython==0.29.22
 
-WORKDIR /project/rednose
+WORKDIR /project
 
 ENV PYTHONPATH=/project
 

SConscript

@@ -1,32 +1,46 @@
-Import('env', 'arch')
+Import('env', 'envCython', 'arch')
 
 templates = Glob('#rednose/templates/*')
 
 # TODO: get dependencies based on installation
 sympy_helpers = "#rednose/helpers/sympy_helpers.py"
 ekf_sym = "#rednose/helpers/ekf_sym.py"
+ekf_sym_pyx = "#rednose/helpers/ekf_sym_pyx.pyx"
+ekf_sym_cc = "#rednose/helpers/ekf_sym.cc"
+common_ekf = "#rednose/helpers/common_ekf.cc"
 
 to_build = {
-    'live': ('examples/live_kf.py', 'examples/generated'),
-    'kinematic': ('examples/kinematic_kf.py', 'examples/generated'),
-    'pos_computer_4': ('rednose/helpers/lst_sq_computer.py', 'examples/generated'),
-    'pos_computer_5': ('rednose/helpers/lst_sq_computer.py', 'examples/generated'),
-    'feature_handler_5': ('rednose/helpers/feature_handler.py', 'examples/generated'),
+    'live': ('examples/live_kf.py', 'examples/generated', True),
+    'kinematic': ('examples/kinematic_kf.py', 'examples/generated', True),
+    'compare': ('examples/test_compare.py', 'examples/generated', True),
+    'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', 'examples/generated', False),
+    'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', 'examples/generated', False),
+    'feature_handler_5': ('#rednose/helpers/feature_handler.py', 'examples/generated', False),
 }
 
-
 found = {}
 
-for target, (command, generated_folder) in to_build.items():
+for target, (command, generated_folder, combined_lib) in to_build.items():
     if File(command).exists():
-        found[target] = (command, generated_folder)
+        found[target] = (command, generated_folder, combined_lib)
 
-for target, (command, generated_folder) in found.items():
+lib_target = [common_ekf]
+for target, (command, generated_folder, combined_lib) in found.items():
     target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h'])
     command_file = File(command)
 
     env.Command(target_files,
                 [templates, command_file, sympy_helpers, ekf_sym],
                 command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath())
 
-    env.SharedLibrary(f'{generated_folder}/' + target, target_files[0])
+    if combined_lib:
+        lib_target.append(target_files[0])
+    else:
+        env.SharedLibrary(f'{generated_folder}/' + target, target_files[0])
+
+libkf = env.SharedLibrary(f'{generated_folder}/libkf', lib_target)
+
+lenv = envCython.Clone()
+lenv["LINKFLAGS"] += [libkf[0].get_labspath()]
+ekf_sym_so = lenv.Program('#rednose/helpers/ekf_sym_pyx.so', [ekf_sym_pyx, ekf_sym_cc, common_ekf])
+lenv.Depends(ekf_sym_so, libkf)

SConstruct

@@ -1,8 +1,20 @@
 import os
 import subprocess
+import sysconfig
+import numpy as np
 
 arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
 
+python_path = sysconfig.get_paths()['include']
+cpppath = [
+  '#',
+  '#rednose',
+  '#rednose/examples/generated',
+  '/usr/lib/include',
+  python_path,
+  np.get_include(),
+]
+
 env = Environment(
   ENV=os.environ,
   CC='clang',
@@ -17,10 +29,26 @@ env = Environment(
     "-Werror=return-type",
     "-Werror=format-extra-args",
   ],
+  LIBPATH=["#rednose/examples/generated"],
   CFLAGS="-std=gnu11",
   CXXFLAGS="-std=c++1z",
+  CPPPATH=cpppath,
+  tools=["default", "cython"],
 )
 
+# Cython build enviroment
+envCython = env.Clone()
+envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-deprecated-declarations"]
+
+envCython["LIBS"] = []
+if arch == "Darwin":
+  envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"]
+elif arch == "aarch64":
+  envCython["LINKFLAGS"] = ["-shared"]
+  envCython["LIBS"] = [os.path.basename(python_path)]
+else:
+  envCython["LINKFLAGS"] = ["-pthread", "-shared"]
+
+Export('env', 'envCython', 'arch')
 
-Export('env', 'arch')
-SConscript(['SConscript'])
+SConscript(['SConscript'])

examples/kinematic_kf.py

@@ -4,8 +4,12 @@
 import numpy as np
 import sympy as sp
 
-from rednose import KalmanFilter
-from rednose.helpers.ekf_sym import gen_code
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__ == '__main__':  # generating sympy code
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym
 
 
 class ObservationKind():
@@ -64,6 +68,14 @@ def generate_code(generated_dir):
 
     gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
 
+  def __init__(self, generated_dir):
+    dim_state = self.initial_x.shape[0]
+    dim_state_err = self.initial_P_diag.shape[0]
+
+    # init filter
+    self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
+
+
 if __name__ == "__main__":
   generated_dir = sys.argv[2]
   KinematicKalman.generate_code(generated_dir)

examples/live_kf.py

@@ -4,8 +4,13 @@
 import sympy as sp
 
 from rednose.helpers import KalmanError
-from rednose.helpers.ekf_sym import EKF_sym, gen_code
-from rednose.helpers.sympy_helpers import (euler_rotate, quat_matrix_r, quat_rotate)
+
+if __name__ == '__main__':  # Generating sympy
+  import sympy as sp
+  from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym  # pylint: disable=no-name-in-module
 
 EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
 

examples/test_compare.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+import os
+import sys
+import sympy as sp
+import numpy as np
+import unittest
+
+if __name__ == '__main__':  # generating sympy code
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym
+  from rednose.helpers.ekf_sym import EKF_sym as EKF_sym2
+
+
+GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
+
+
+class ObservationKind:
+  UNKNOWN = 0
+  NO_OBSERVATION = 1
+  POSITION = 1
+
+  names = [
+    'Unknown',
+    'No observation',
+    'Position'
+  ]
+
+  @classmethod
+  def to_string(cls, kind):
+    return cls.names[kind]
+
+
+class States:
+  POSITION = slice(0, 1)
+  VELOCITY = slice(1, 2)
+
+
+class CompareFilter:
+  name = "compare"
+
+  initial_x = np.array([0.5, 0.0])
+  initial_P_diag = np.array([1.0**2, 1.0**2])
+  Q = np.diag([0.1**2, 2.0**2])
+  obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)}
+
+  @staticmethod
+  def generate_code(generated_dir):
+    name = CompareFilter.name
+    dim_state = CompareFilter.initial_x.shape[0]
+
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+
+    position = state[States.POSITION, :][0,:]
+    velocity = state[States.VELOCITY, :][0,:]
+
+    dt = sp.Symbol('dt')
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.POSITION.start, 0] = velocity
+    f_sym = state + dt * state_dot
+
+    obs_eqs = [
+      [sp.Matrix([position]), ObservationKind.POSITION, None],
+    ]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
+
+  def __init__(self, generated_dir):
+    dim_state = self.initial_x.shape[0]
+    dim_state_err = self.initial_P_diag.shape[0]
+
+    # init filter
+    self.filter_py = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
+    self.filter_pyx = EKF_sym2(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
+
+  def get_R(self, kind, n):
+    obs_noise = self.obs_noise[kind]
+    dim = obs_noise.shape[0]
+    R = np.zeros((n, dim, dim))
+    for i in range(n):
+      R[i, :, :] = obs_noise
+    return R
+
+
+class TestCompare(unittest.TestCase):
+  def test_compare(self):
+    np.random.seed(0)
+
+    kf = CompareFilter(GENERATED_DIR)
+
+    # Simple simulation
+    dt = 0.01
+    ts = np.arange(0, 5, step=dt)
+    xs = np.empty(ts.shape)
+
+    # Simulate
+    x = 0.0
+    for i, v in enumerate(np.sin(ts * 5)):
+      xs[i] = x
+      x += v * dt
+
+    # insert late observation
+    switch = (20, 40)
+    ts[switch[0]], ts[switch[1]] = ts[switch[1]], ts[switch[0]]
+    xs[switch[0]], xs[switch[1]] = xs[switch[1]], xs[switch[0]]
+
+    for t, x in zip(ts, xs):
+      # get measurement
+      meas = np.random.normal(x, 0.1)
+      z = np.array([[meas]])
+      R = kf.get_R(ObservationKind.POSITION, 1)
+
+      # Update kf
+      kf.filter_py.predict_and_update_batch(t, ObservationKind.POSITION, z, R)
+      kf.filter_pyx.predict_and_update_batch(t, ObservationKind.POSITION, z, R)
+
+      self.assertAlmostEqual(kf.filter_py.get_filter_time(), kf.filter_pyx.get_filter_time())
+      self.assertTrue(np.allclose(kf.filter_py.state(), kf.filter_pyx.state()))
+      self.assertTrue(np.allclose(kf.filter_py.covs(), kf.filter_pyx.covs()))
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  CompareFilter.generate_code(generated_dir)

rednose/helpers/__init__.py

@@ -13,18 +13,23 @@ def write_code(folder, name, code, header):
   open(os.path.join(folder, f"{name}.h"), 'w').write(header)
 
 
-def load_code(folder, name):
+def load_code(folder, name, lib_name=None):
+  if lib_name is None:
+    lib_name = name
   shared_ext = "dylib" if platform.system() == "Darwin" else "so"
-  shared_fn = os.path.join(folder, f"lib{name}.{shared_ext}")
+  shared_fn = os.path.join(folder, f"lib{lib_name}.{shared_ext}")
   header_fn = os.path.join(folder, f"{name}.h")
 
   with open(header_fn) as f:
     header = f.read()
 
+  # is the only thing that can be parsed by cffi
+  header = "\n".join([line for line in header.split("\n") if line.startswith("void ")])
+
   ffi = FFI()
   ffi.cdef(header)
   return (ffi, ffi.dlopen(shared_fn))
 
 
 class KalmanError(Exception):
-  pass
+  pass

rednose/helpers/common_ekf.cc

@@ -0,0 +1,19 @@
+#include "common_ekf.h"
+
+std::vector<const EKF*>& get_ekfs() {
+  static std::vector<const EKF*> vec;
+  return vec;
+}
+
+void ekf_register(const EKF* ekf) {
+  get_ekfs().push_back(ekf);
+}
+
+const EKF* ekf_lookup(const std::string& ekf_name) {
+  for (const auto& ekfi : get_ekfs()) {
+    if (ekf_name == ekfi->name) {
+      return ekfi;
+    }
+  }
+  return NULL;
+}