Move ekf_sym cython to cc

examples/kinematic_kf.py

@@ -9,7 +9,7 @@
 if __name__ == '__main__':  # generating sympy code
   from rednose.helpers.ekf_sym import gen_code
 else:
-  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module
+  from rednose.helpers import EKFSym
 
 
 class ObservationKind():
@@ -36,13 +36,13 @@ class States():
 class KinematicKalman(KalmanFilter):
   name = 'kinematic'
 
-  initial_x = np.array([0.5, 0.0])
+  initial_x: np.ndarray = np.array([0.5, 0.0])
 
   # state covariance
-  initial_P_diag = np.array([1.0**2, 1.0**2])
+  initial_P_diag: np.ndarray = np.array([1.0**2, 1.0**2])
 
   # process noise
-  Q = np.diag([0.1**2, 2.0**2])
+  Q: np.ndarray = np.diag([0.1**2, 2.0**2])
 
   obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)}
 
@@ -73,7 +73,7 @@ def __init__(self, generated_dir):
     dim_state_err = self.initial_P_diag.shape[0]
 
     # init filter
-    self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
+    self.filter = EKFSym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
 
 
 if __name__ == "__main__":

examples/live_kf.py

@@ -9,7 +9,7 @@
   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_pyx  # pylint: disable=no-name-in-module
+  from rednose.helpers import EKFSym
 
 EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
 
@@ -258,7 +258,7 @@ def __init__(self, generated_dir):
                       ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
 
     # init filter
-    self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
+    self.filter = EKFSym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
 
   @property
   def x(self):

examples/test_compare.py

@@ -8,7 +8,7 @@
 if __name__ == '__main__':  # generating sympy code
   from rednose.helpers.ekf_sym import gen_code
 else:
-  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module
+  from rednose.helpers import EKFSym
   from rednose.helpers.ekf_sym import EKF_sym as EKF_sym2
 
 
@@ -71,7 +71,7 @@ def __init__(self, generated_dir):
     dim_state_err = self.initial_P_diag.shape[0]
 
     # init filter
-    self.filter_py = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
+    self.filter_py = EKFSym(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):

examples/test_kinematic_kf.py

@@ -7,11 +7,12 @@
 GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
 
 class TestKinematic:
+  def setup_method(self):
+    self.kf = KinematicKalman(GENERATED_DIR)
+
   def test_kinematic_kf(self):
     np.random.seed(0)
 
-    kf = KinematicKalman(GENERATED_DIR)
-
     # Simple simulation
     dt = 0.01
     ts = np.arange(0, 5, step=dt)
@@ -34,13 +35,13 @@ def test_kinematic_kf(self):
       # Update kf
       meas = np.random.normal(x, 0.1)
       xs_meas.append(meas)
-      kf.predict_and_observe(t, ObservationKind.POSITION, [meas])
+      self.kf.predict_and_observe(t, ObservationKind.POSITION, [meas])
 
       # Retrieve kf values
-      state = kf.x
+      state = self.kf.x
       xs_kf.append(float(state[States.POSITION].item()))
       vs_kf.append(float(state[States.VELOCITY].item()))
-      std = np.sqrt(kf.P)
+      std = np.sqrt(self.kf.P)
       xs_kf_std.append(float(std[States.POSITION, States.POSITION].item()))
       vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY].item()))
 
@@ -80,3 +81,47 @@ def test_kinematic_kf(self):
       plt.legend()
 
       plt.show()
+
+  def test_init_state(self):
+    init_x = self.kf.x
+
+    dim_state_err = self.kf.initial_P_diag.shape[0]
+
+    new_x = np.copy(init_x)
+    new_x[States.POSITION] = 100.0
+    new_x[States.VELOCITY] = 5.0
+
+    new_P = np.eye(dim_state_err) * 0.5
+
+    self.kf.init_state(new_x, covs=new_P, filter_time=1.0)
+
+    assert np.allclose(self.kf.x, new_x)
+    assert np.allclose(self.kf.P, new_P)
+    assert self.kf.t == 1.0
+
+  def test_set_filter_time(self):
+    assert np.isnan(self.kf.t)
+
+    self.kf.filter.set_filter_time(10.5)
+    assert self.kf.t == 10.5
+
+  def test_predict(self):
+    dim_state = self.kf.initial_x.shape[0]
+
+    x0 = np.zeros(dim_state)
+    x0[States.VELOCITY] = 10.0
+    self.kf.init_state(x0, filter_time=0.0)
+
+    t0 = self.kf.t
+    dt = 0.1
+
+    self.kf.filter.predict(t0 + dt)
+
+    assert self.kf.t == pytest.approx(t0 + dt)
+    assert self.kf.x[States.POSITION].item() == pytest.approx(1.0)
+
+  def test_rewind(self):
+    try:
+      self.kf.filter.reset_rewind()
+    except Exception as e:
+      pytest.fail(f"reset_rewind raised exception: {e}")

rednose/SConscript

@@ -11,7 +11,7 @@ if common != "":
 
 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"])
+rednose_python = envCython.SharedLibrary("helpers/_ekf_sym_module.so", ["helpers/ekf_sym_module.cc", ekf_objects],
+                                   LIBS=libs + envCython["LIBS"], SHLIBPREFIX="", CPPPATH=envCython["CPPPATH"] + [Dir('.').abspath])
 
 Export('rednose', 'rednose_python')

rednose/helpers/__init__.py

@@ -33,3 +33,5 @@ def load_code(folder, name):
 
 class KalmanError(Exception):
   pass
+
+from ._ekf_sym_module import EKFSym  # noqa: F401