try np ekf

.github/workflows/tests.yaml

@@ -109,7 +109,7 @@ jobs:
     - name: Build openpilot
       run: scons
     - name: Run unit tests
-      timeout-minutes: ${{ contains(runner.name, 'nsc') && 2 || 20 }}
+      timeout-minutes: ${{ contains(runner.name, 'nsc') && 10 || 20 }}
       run: |
         source openpilot/selfdrive/test/setup_xvfb.sh
         # Pre-compile Python bytecode so each pytest worker doesn't need to
@@ -132,7 +132,7 @@ jobs:
     - name: Build openpilot
       run: scons
     - name: Run replay
-      timeout-minutes: ${{ contains(runner.name, 'nsc') && 2 || 20 }}
+      timeout-minutes: ${{ contains(runner.name, 'nsc') && 10 || 20 }}
       continue-on-error: ${{ github.ref == 'refs/heads/master' }}
       run: openpilot/selfdrive/test/process_replay/test_processes.py -j$(nproc)
     - name: Print diff

SConstruct

@@ -233,9 +233,6 @@ Export('messaging')
 # Build other submodules
 SConscript(['panda/SConscript'])
 
-# Build rednose library
-SConscript(['rednose/SConscript'])
-
 # Build system services
 SConscript([
   'openpilot/system/loggerd/SConscript',

openpilot/selfdrive/locationd/SConscript

@@ -1,4 +1,4 @@
-Import('env', 'rednose')
+Import('env')
 
 # build ekf models
 rednose_gen_dir = 'models/generated'

openpilot/selfdrive/locationd/locationd.py

@@ -86,7 +86,7 @@ def _validate_sensor_time(self, sensor_time: float, t: float):
 
   def _validate_timestamp(self, t: float):
     kf_t = self.kf.t
-    invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
+    invalid = kf_t is not None and not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
     if invalid:
       cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter")
     return not invalid
@@ -97,6 +97,31 @@ def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray):
       cloudlog.error("Non-finite values detected, kalman reset")
       self.reset(t)
 
+  def _predict_and_observe_camera_odometry(self, t: float, rot_device: np.ndarray, rot_device_noise: np.ndarray,
+                                           trans_device: np.ndarray, trans_device_noise: np.ndarray):
+    ekf = self.kf.filter
+    # Camera rotation and translation have the same delayed timestamp. Rewind once, apply both updates,
+    # then fast-forward once instead of replaying all newer IMU observations twice.
+    if ekf.filter_time is not None and t < ekf.filter_time:
+      if len(ekf.rewind_t) == 0 or t < ekf.rewind_t[0] or t < ekf.rewind_t[-1] - ekf.max_rewind_age:
+        ekf.logger.error(f"observation too old at {t:.3f} with filter at {ekf.filter_time:.3f}, ignoring")
+        return None, None
+      rewound = ekf.rewind(t)
+    else:
+      rewound = []
+
+    rot_R = np.array([np.diag(rot_device_noise)])
+    trans_R = np.array([np.diag(trans_device_noise)])
+    cam_odo_rot_res = ekf._predict_and_update_batch(t, ObservationKind.CAMERA_ODO_ROTATION, np.atleast_2d(rot_device),
+                                                    rot_R, [[]])
+    cam_odo_trans_res = ekf._predict_and_update_batch(t, ObservationKind.CAMERA_ODO_TRANSLATION,
+                                                      np.atleast_2d(trans_device), trans_R, [[]])
+
+    for r in rewound:
+      ekf._predict_and_update_batch(*r)
+
+    return cam_odo_rot_res, cam_odo_trans_res
+
   def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult:
     new_x, new_P = None, None
     if which == "accelerometer" and msg.which() == "acceleration":
@@ -190,8 +215,8 @@ def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> H
       rot_device_noise = rot_device_std ** 2
       trans_device_noise = trans_device_std ** 2
 
-      cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)]))
-      cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)]))
+      cam_odo_rot_res, cam_odo_trans_res = self._predict_and_observe_camera_odometry(t, rot_device, rot_device_noise,
+                                                                                     trans_device, trans_device_noise)
       self.camodo_yawrate_distribution =  np.array([rot_device[2], rot_device_std[2]])
       if cam_odo_rot_res is not None:
         _, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res
@@ -209,7 +234,8 @@ def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> H
   def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_initialized: bool):
     state, cov = self.kf.x, self.kf.P
     std = np.sqrt(np.diag(cov))
-    filter_time_valid = bool(np.isfinite(self.kf.t))
+    kf_t = self.kf.t
+    filter_time_valid = kf_t is not None and bool(np.isfinite(kf_t))
     filter_valid = filter_initialized and filter_time_valid
 
     orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION]
@@ -241,7 +267,7 @@ def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_initialized: b
     livePose.inputsOK = inputs_valid
     livePose.posenetOK = not std_spike or self.car_speed <= 5.0
     livePose.sensorsOK = sensors_valid
-    livePose.timestamp = int(np.nan_to_num(self.kf.t) * 1e9)
+    livePose.timestamp = int(np.nan_to_num(kf_t) * 1e9) if kf_t is not None else 0
 
     return msg
 

openpilot/selfdrive/locationd/models/car_kf.py

@@ -15,7 +15,7 @@
   import sympy as sp
   from rednose.helpers.ekf_sym import gen_code
 else:
-  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+  from rednose.helpers.ekf_sym import EKF_sym
 
 
 i = 0
@@ -163,8 +163,8 @@ def generate_code(generated_dir):
 
   def __init__(self, generated_dir):
     dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
-    self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
-                              dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
+    self.filter = EKF_sym(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
+                          dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
 
   def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
     self.filter.set_global("mass", mass)

openpilot/selfdrive/locationd/models/pose_kf.py

@@ -12,7 +12,7 @@
   from rednose.helpers.ekf_sym import gen_code
   from rednose.helpers.sympy_helpers import euler_rotate, rot_to_euler
 else:
-  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+  from rednose.helpers.ekf_sym import EKF_sym
 
 EARTH_G = 9.81
 
@@ -102,8 +102,8 @@ def generate_code(generated_dir):
 
   def __init__(self, generated_dir, max_rewind_age):
     dim_state, dim_state_err = PoseKalman.initial_x.shape[0], PoseKalman.initial_P.shape[0]
-    self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P,
-                              dim_state, dim_state_err, max_rewind_age=max_rewind_age)
+    self.filter = EKF_sym(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P,
+                          dim_state, dim_state_err, max_rewind_age=max_rewind_age)
 
 
 if __name__ == "__main__":

openpilot/selfdrive/locationd/test/test_locationd.py

@@ -0,0 +1,8 @@
+from openpilot.selfdrive.locationd.locationd import LocationEstimator
+
+
+def test_get_msg_uninitialized_filter_time():
+  msg = LocationEstimator(False).get_msg(sensors_valid=True, inputs_valid=True, filter_initialized=False)
+
+  assert not msg.valid
+  assert msg.livePose.timestamp == 0