add kinematic example and test

.github/workflows/tests.yml

@@ -11,3 +11,5 @@ jobs:
     - uses: actions/checkout@v2
     - name: Build docker image
       run: docker build -t rednose .
+    - name: Unit Tests
+      run: docker run rednose bash -c "cd /project/rednose/examples; python -m unittest discover"

SConscript

@@ -8,6 +8,7 @@ ekf_sym = "#rednose/helpers/ekf_sym.py"
 
 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'),

examples/kinematic_kf.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+import sys
+
+import numpy as np
+import sympy as sp
+
+from rednose.helpers.ekf_sym import EKF_sym, gen_code
+
+EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
+
+
+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 KinematicKalman():
+  name = 'kinematic'
+
+  initial_x = np.array([0.5, 0.0])
+
+  # state covariance
+  initial_P_diag = np.array([1.0**2, 1.0**2])
+
+  # process noise
+  Q = np.diag([0.1**2, 2.0**2])
+
+  @staticmethod
+  def generate_code(generated_dir):
+    name = KinematicKalman.name
+    dim_state = KinematicKalman.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):
+    self.dim_state = self.initial_x.shape[0]
+    self.dim_state_err = self.initial_P_diag.shape[0]
+
+    self.obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)}
+
+    # init filter
+    self.filter = EKF_sym(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):
+    return self.filter.state()
+
+  @property
+  def t(self):
+    return self.filter.filter_time
+
+  @property
+  def P(self):
+    return self.filter.covs()
+
+  def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
+    if covs_diag is not None:
+      P = np.diag(covs_diag)
+    elif covs is not None:
+      P = covs
+    else:
+      P = self.filter.covs()
+    self.filter.init_state(state, P, filter_time)
+
+  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
+
+  def predict_and_observe(self, t, kind, data, R=None):
+    if len(data) > 0:
+      data = np.atleast_2d(data)
+
+    if R is None:
+      R = self.get_R(kind, len(data))
+
+    self.filter.predict_and_update_batch(t, kind, data, R)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  KinematicKalman.generate_code(generated_dir)

examples/live_kf.py

@@ -1,13 +1,12 @@
 #!/usr/bin/env python3
-import os
+import sys
 import numpy as np
 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)
 
-GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
 EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
 
 
@@ -121,7 +120,7 @@ class LiveKalman():
                (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
 
   @staticmethod
-  def generate_code():
+  def generate_code(generated_dir):
     name = LiveKalman.name
     dim_state = LiveKalman.initial_x.shape[0]
     dim_state_err = LiveKalman.initial_P_diag.shape[0]
@@ -240,9 +239,9 @@ def generate_code():
                [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
                [h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
 
-    gen_code(GENERATED_DIR, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
 
-  def __init__(self):
+  def __init__(self, generated_dir):
     self.dim_state = self.initial_x.shape[0]
     self.dim_state_err = self.initial_P_diag.shape[0]
 
@@ -255,7 +254,7 @@ def __init__(self):
                       ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
 
     # init filter
-    self.filter = EKF_sym(GENERATED_DIR, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
+    self.filter = EKF_sym(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):
@@ -335,4 +334,5 @@ def predict_and_update_odo_rot(self, rot, t, kind):
 
 
 if __name__ == "__main__":
-  LiveKalman.generate_code()
+  generated_dir = sys.argv[2]
+  LiveKalman.generate_code(generated_dir)

examples/test_kinematic_kf.py

@@ -0,0 +1,82 @@
+import os
+import numpy as np
+import unittest
+
+from kinematic_kf import KinematicKalman, ObservationKind, States
+
+GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
+
+class TestKinematic(unittest.TestCase):
+  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)
+    vs = np.sin(ts * 5)
+
+    x = 0.0
+    xs = []
+
+    xs_meas = []
+
+    xs_kf = []
+    vs_kf = []
+
+    xs_kf_std = []
+    vs_kf_std = []
+
+    for t, v in zip(ts, vs):
+      xs.append(x)
+
+      # Update kf
+      meas = np.random.normal(x, 0.1)
+      xs_meas.append(meas)
+      kf.predict_and_observe(t, ObservationKind.POSITION, [meas])
+
+      # Retrieve kf values
+      state = kf.x
+      xs_kf.append(float(state[States.POSITION]))
+      vs_kf.append(float(state[States.VELOCITY]))
+      std = np.sqrt(kf.P)
+      xs_kf_std.append(float(std[States.POSITION, States.POSITION]))
+      vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY]))
+
+      # Update simulation
+      x += v * dt
+
+    xs, xs_meas, xs_kf, vs_kf, xs_kf_std, vs_kf_std = [np.asarray(a) for a in (xs, xs_meas, xs_kf, vs_kf, xs_kf_std, vs_kf_std)]
+
+    self.assertAlmostEqual(xs_kf[-1], -0.010866289677966417)
+    self.assertAlmostEqual(xs_kf_std[-1], 0.04477103863330089)
+    self.assertAlmostEqual(vs_kf[-1], -0.8553720537261753)
+    self.assertAlmostEqual(vs_kf_std[-1], 0.6695762270974388)
+
+    if "PLOT" in os.environ:
+      import matplotlib.pyplot as plt
+      plt.figure()
+      plt.subplot(2, 1, 1)
+      plt.plot(ts, xs, 'k', label='Simulation')
+      plt.plot(ts, xs_meas, 'k.', label='Measurements')
+      plt.plot(ts, xs_kf, label='KF')
+      ax = plt.gca()
+      ax.fill_between(ts, xs_kf - xs_kf_std, xs_kf + xs_kf_std, alpha=.2, color='C0')
+
+      plt.xlabel("Time [s]")
+      plt.ylabel("Position [m]")
+      plt.legend()
+
+      plt.subplot(2, 1, 2)
+      plt.plot(ts, vs, 'k', label='Simulation')
+      plt.plot(ts, vs_kf, label='KF')
+
+      ax = plt.gca()
+      ax.fill_between(ts, vs_kf - vs_kf_std, vs_kf + vs_kf_std, alpha=.2, color='C0')
+
+      plt.xlabel("Time [s]")
+      plt.ylabel("Velocity [m/s]")
+      plt.legend()
+
+      plt.show()