build

2026-07-18 15:52:09 +08:00 · 2026-07-15 11:25:27 -05:00
parent d4cfd34291
commit a6ffaaa781
84 changed files with 1387 additions and 1358 deletions
@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-77db3e50-DEBUG";
+const uint8_t gitversion[19] = "DEV-d4cfd342-DEBUG";
@@ -1 +1 @@
-DEV-77db3e50-DEBUG
+DEV-d4cfd342-DEBUG
@@ -62,17 +62,36 @@ def load_existing_rows(manifest_path: Path) -> dict[str, dict[str, str]]:
    return {row["record_key"]: row for row in reader if row.get("record_key")}


-def read_frame_at(video_path: Path, target_time_s: float):
+def read_frames_at(video_path: Path, target_times_s: list[float]):
  capture = cv2.VideoCapture(str(video_path))
  fps = capture.get(cv2.CAP_PROP_FPS) or 20.0
-  frame_index = max(int(round(target_time_s * fps)), 0)
-  capture.set(cv2.CAP_PROP_POS_FRAMES, frame_index)
-  ok, frame_bgr = capture.read()
+  targets = sorted((max(int(round(target_time_s * fps)), 0), target_time_s) for target_time_s in target_times_s)
+  results = {}
+  frame_index = 0
+  last_frame_index = -1
+  last_frame_bgr = None
+
+  for target_frame_index, target_time_s in targets:
+    if target_frame_index == last_frame_index and last_frame_bgr is not None:
+      results[target_time_s] = (last_frame_index / fps, last_frame_bgr.copy())
+      continue
+
+    while frame_index < target_frame_index:
+      if not capture.grab():
+        capture.release()
+        return results
+      frame_index += 1
+
+    ok, frame_bgr = capture.read()
+    if not ok or frame_bgr is None:
+      break
+    last_frame_index = frame_index
+    last_frame_bgr = frame_bgr
+    results[target_time_s] = (frame_index / fps, frame_bgr)
+    frame_index += 1
+
  capture.release()
-  if not ok or frame_bgr is None:
-    return None
-  actual_time_s = frame_index / fps
-  return actual_time_s, frame_bgr
+  return results


 def sample_offsets(leadin_start_s: float, segment_offset_s: float, sample_every: float, max_samples: int):
@@ -104,7 +123,7 @@ def extract_window_frames(row: dict, clip_root: Path, sample_every: float, max_s
  leadin_start_s = float(row["leadinStartS"])
  spans_previous_segment = bool(row.get("spansPreviousSegment"))

-  sampled = []
+  requests = []
  previous_clip = clip_root / f"{route}--{segment - 1}" / "fcamera.hevc"
  current_clip = clip_root / f"{route}--{segment}" / "fcamera.hevc"

@@ -119,10 +138,18 @@ def extract_window_frames(row: dict, clip_root: Path, sample_every: float, max_s
    if not source_video.is_file():
      continue

-    frame_info = read_frame_at(source_video, source_time_s)
+    requests.append((relative_offset_s, source_video, source_time_s))
+
+  decoded_by_video = {}
+  for source_video in {request[1] for request in requests}:
+    target_times_s = [request[2] for request in requests if request[1] == source_video]
+    decoded_by_video[source_video] = read_frames_at(source_video, target_times_s)
+
+  sampled = []
+  for relative_offset_s, source_video, source_time_s in requests:
+    frame_info = decoded_by_video[source_video].get(source_time_s)
    if frame_info is None:
      continue
-
    actual_time_s, frame_bgr = frame_info
    sampled.append({
      "relative_offset_s": relative_offset_s,
@@ -101,6 +101,8 @@ def load_route_bookmarks(clip_root: Path, log_id: str) -> list[dict]:
      if event_type == "userBookmark":
        deduped[-1]["event_type"] = event_type
        deduped[-1]["route_time_s"] = route_time_s
+        deduped[-1]["segment"] = max(int(route_time_s // 60.0), 0)
+        deduped[-1]["segment_offset_s"] = route_time_s - deduped[-1]["segment"] * 60.0
      continue

    segment = max(int(route_time_s // 60.0), 0)
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
 *                                                                            *
 *                         This file is part of 'ekf'                         *
 ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_9086440322595224646) {
-   out_9086440322595224646[0] = delta_x[0] + nom_x[0];
-   out_9086440322595224646[1] = delta_x[1] + nom_x[1];
-   out_9086440322595224646[2] = delta_x[2] + nom_x[2];
-   out_9086440322595224646[3] = delta_x[3] + nom_x[3];
-   out_9086440322595224646[4] = delta_x[4] + nom_x[4];
-   out_9086440322595224646[5] = delta_x[5] + nom_x[5];
-   out_9086440322595224646[6] = delta_x[6] + nom_x[6];
-   out_9086440322595224646[7] = delta_x[7] + nom_x[7];
-   out_9086440322595224646[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_7550976477549507356) {
+   out_7550976477549507356[0] = delta_x[0] + nom_x[0];
+   out_7550976477549507356[1] = delta_x[1] + nom_x[1];
+   out_7550976477549507356[2] = delta_x[2] + nom_x[2];
+   out_7550976477549507356[3] = delta_x[3] + nom_x[3];
+   out_7550976477549507356[4] = delta_x[4] + nom_x[4];
+   out_7550976477549507356[5] = delta_x[5] + nom_x[5];
+   out_7550976477549507356[6] = delta_x[6] + nom_x[6];
+   out_7550976477549507356[7] = delta_x[7] + nom_x[7];
+   out_7550976477549507356[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_7805211099664095660) {
-   out_7805211099664095660[0] = -nom_x[0] + true_x[0];
-   out_7805211099664095660[1] = -nom_x[1] + true_x[1];
-   out_7805211099664095660[2] = -nom_x[2] + true_x[2];
-   out_7805211099664095660[3] = -nom_x[3] + true_x[3];
-   out_7805211099664095660[4] = -nom_x[4] + true_x[4];
-   out_7805211099664095660[5] = -nom_x[5] + true_x[5];
-   out_7805211099664095660[6] = -nom_x[6] + true_x[6];
-   out_7805211099664095660[7] = -nom_x[7] + true_x[7];
-   out_7805211099664095660[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_3781853638498010) {
+   out_3781853638498010[0] = -nom_x[0] + true_x[0];
+   out_3781853638498010[1] = -nom_x[1] + true_x[1];
+   out_3781853638498010[2] = -nom_x[2] + true_x[2];
+   out_3781853638498010[3] = -nom_x[3] + true_x[3];
+   out_3781853638498010[4] = -nom_x[4] + true_x[4];
+   out_3781853638498010[5] = -nom_x[5] + true_x[5];
+   out_3781853638498010[6] = -nom_x[6] + true_x[6];
+   out_3781853638498010[7] = -nom_x[7] + true_x[7];
+   out_3781853638498010[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_2822454019109077104) {
-   out_2822454019109077104[0] = 1.0;
-   out_2822454019109077104[1] = 0.0;
-   out_2822454019109077104[2] = 0.0;
-   out_2822454019109077104[3] = 0.0;
-   out_2822454019109077104[4] = 0.0;
-   out_2822454019109077104[5] = 0.0;
-   out_2822454019109077104[6] = 0.0;
-   out_2822454019109077104[7] = 0.0;
-   out_2822454019109077104[8] = 0.0;
-   out_2822454019109077104[9] = 0.0;
-   out_2822454019109077104[10] = 1.0;
-   out_2822454019109077104[11] = 0.0;
-   out_2822454019109077104[12] = 0.0;
-   out_2822454019109077104[13] = 0.0;
-   out_2822454019109077104[14] = 0.0;
-   out_2822454019109077104[15] = 0.0;
-   out_2822454019109077104[16] = 0.0;
-   out_2822454019109077104[17] = 0.0;
-   out_2822454019109077104[18] = 0.0;
-   out_2822454019109077104[19] = 0.0;
-   out_2822454019109077104[20] = 1.0;
-   out_2822454019109077104[21] = 0.0;
-   out_2822454019109077104[22] = 0.0;
-   out_2822454019109077104[23] = 0.0;
-   out_2822454019109077104[24] = 0.0;
-   out_2822454019109077104[25] = 0.0;
-   out_2822454019109077104[26] = 0.0;
-   out_2822454019109077104[27] = 0.0;
-   out_2822454019109077104[28] = 0.0;
-   out_2822454019109077104[29] = 0.0;
-   out_2822454019109077104[30] = 1.0;
-   out_2822454019109077104[31] = 0.0;
-   out_2822454019109077104[32] = 0.0;
-   out_2822454019109077104[33] = 0.0;
-   out_2822454019109077104[34] = 0.0;
-   out_2822454019109077104[35] = 0.0;
-   out_2822454019109077104[36] = 0.0;
-   out_2822454019109077104[37] = 0.0;
-   out_2822454019109077104[38] = 0.0;
-   out_2822454019109077104[39] = 0.0;
-   out_2822454019109077104[40] = 1.0;
-   out_2822454019109077104[41] = 0.0;
-   out_2822454019109077104[42] = 0.0;
-   out_2822454019109077104[43] = 0.0;
-   out_2822454019109077104[44] = 0.0;
-   out_2822454019109077104[45] = 0.0;
-   out_2822454019109077104[46] = 0.0;
-   out_2822454019109077104[47] = 0.0;
-   out_2822454019109077104[48] = 0.0;
-   out_2822454019109077104[49] = 0.0;
-   out_2822454019109077104[50] = 1.0;
-   out_2822454019109077104[51] = 0.0;
-   out_2822454019109077104[52] = 0.0;
-   out_2822454019109077104[53] = 0.0;
-   out_2822454019109077104[54] = 0.0;
-   out_2822454019109077104[55] = 0.0;
-   out_2822454019109077104[56] = 0.0;
-   out_2822454019109077104[57] = 0.0;
-   out_2822454019109077104[58] = 0.0;
-   out_2822454019109077104[59] = 0.0;
-   out_2822454019109077104[60] = 1.0;
-   out_2822454019109077104[61] = 0.0;
-   out_2822454019109077104[62] = 0.0;
-   out_2822454019109077104[63] = 0.0;
-   out_2822454019109077104[64] = 0.0;
-   out_2822454019109077104[65] = 0.0;
-   out_2822454019109077104[66] = 0.0;
-   out_2822454019109077104[67] = 0.0;
-   out_2822454019109077104[68] = 0.0;
-   out_2822454019109077104[69] = 0.0;
-   out_2822454019109077104[70] = 1.0;
-   out_2822454019109077104[71] = 0.0;
-   out_2822454019109077104[72] = 0.0;
-   out_2822454019109077104[73] = 0.0;
-   out_2822454019109077104[74] = 0.0;
-   out_2822454019109077104[75] = 0.0;
-   out_2822454019109077104[76] = 0.0;
-   out_2822454019109077104[77] = 0.0;
-   out_2822454019109077104[78] = 0.0;
-   out_2822454019109077104[79] = 0.0;
-   out_2822454019109077104[80] = 1.0;
+void H_mod_fun(double *state, double *out_6037661802737362721) {
+   out_6037661802737362721[0] = 1.0;
+   out_6037661802737362721[1] = 0.0;
+   out_6037661802737362721[2] = 0.0;
+   out_6037661802737362721[3] = 0.0;
+   out_6037661802737362721[4] = 0.0;
+   out_6037661802737362721[5] = 0.0;
+   out_6037661802737362721[6] = 0.0;
+   out_6037661802737362721[7] = 0.0;
+   out_6037661802737362721[8] = 0.0;
+   out_6037661802737362721[9] = 0.0;
+   out_6037661802737362721[10] = 1.0;
+   out_6037661802737362721[11] = 0.0;
+   out_6037661802737362721[12] = 0.0;
+   out_6037661802737362721[13] = 0.0;
+   out_6037661802737362721[14] = 0.0;
+   out_6037661802737362721[15] = 0.0;
+   out_6037661802737362721[16] = 0.0;
+   out_6037661802737362721[17] = 0.0;
+   out_6037661802737362721[18] = 0.0;
+   out_6037661802737362721[19] = 0.0;
+   out_6037661802737362721[20] = 1.0;
+   out_6037661802737362721[21] = 0.0;
+   out_6037661802737362721[22] = 0.0;
+   out_6037661802737362721[23] = 0.0;
+   out_6037661802737362721[24] = 0.0;
+   out_6037661802737362721[25] = 0.0;
+   out_6037661802737362721[26] = 0.0;
+   out_6037661802737362721[27] = 0.0;
+   out_6037661802737362721[28] = 0.0;
+   out_6037661802737362721[29] = 0.0;
+   out_6037661802737362721[30] = 1.0;
+   out_6037661802737362721[31] = 0.0;
+   out_6037661802737362721[32] = 0.0;
+   out_6037661802737362721[33] = 0.0;
+   out_6037661802737362721[34] = 0.0;
+   out_6037661802737362721[35] = 0.0;
+   out_6037661802737362721[36] = 0.0;
+   out_6037661802737362721[37] = 0.0;
+   out_6037661802737362721[38] = 0.0;
+   out_6037661802737362721[39] = 0.0;
+   out_6037661802737362721[40] = 1.0;
+   out_6037661802737362721[41] = 0.0;
+   out_6037661802737362721[42] = 0.0;
+   out_6037661802737362721[43] = 0.0;
+   out_6037661802737362721[44] = 0.0;
+   out_6037661802737362721[45] = 0.0;
+   out_6037661802737362721[46] = 0.0;
+   out_6037661802737362721[47] = 0.0;
+   out_6037661802737362721[48] = 0.0;
+   out_6037661802737362721[49] = 0.0;
+   out_6037661802737362721[50] = 1.0;
+   out_6037661802737362721[51] = 0.0;
+   out_6037661802737362721[52] = 0.0;
+   out_6037661802737362721[53] = 0.0;
+   out_6037661802737362721[54] = 0.0;
+   out_6037661802737362721[55] = 0.0;
+   out_6037661802737362721[56] = 0.0;
+   out_6037661802737362721[57] = 0.0;
+   out_6037661802737362721[58] = 0.0;
+   out_6037661802737362721[59] = 0.0;
+   out_6037661802737362721[60] = 1.0;
+   out_6037661802737362721[61] = 0.0;
+   out_6037661802737362721[62] = 0.0;
+   out_6037661802737362721[63] = 0.0;
+   out_6037661802737362721[64] = 0.0;
+   out_6037661802737362721[65] = 0.0;
+   out_6037661802737362721[66] = 0.0;
+   out_6037661802737362721[67] = 0.0;
+   out_6037661802737362721[68] = 0.0;
+   out_6037661802737362721[69] = 0.0;
+   out_6037661802737362721[70] = 1.0;
+   out_6037661802737362721[71] = 0.0;
+   out_6037661802737362721[72] = 0.0;
+   out_6037661802737362721[73] = 0.0;
+   out_6037661802737362721[74] = 0.0;
+   out_6037661802737362721[75] = 0.0;
+   out_6037661802737362721[76] = 0.0;
+   out_6037661802737362721[77] = 0.0;
+   out_6037661802737362721[78] = 0.0;
+   out_6037661802737362721[79] = 0.0;
+   out_6037661802737362721[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_1333075080878653623) {
-   out_1333075080878653623[0] = state[0];
-   out_1333075080878653623[1] = state[1];
-   out_1333075080878653623[2] = state[2];
-   out_1333075080878653623[3] = state[3];
-   out_1333075080878653623[4] = state[4];
-   out_1333075080878653623[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
-   out_1333075080878653623[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
-   out_1333075080878653623[7] = state[7];
-   out_1333075080878653623[8] = state[8];
+void f_fun(double *state, double dt, double *out_8701591355184024691) {
+   out_8701591355184024691[0] = state[0];
+   out_8701591355184024691[1] = state[1];
+   out_8701591355184024691[2] = state[2];
+   out_8701591355184024691[3] = state[3];
+   out_8701591355184024691[4] = state[4];
+   out_8701591355184024691[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
+   out_8701591355184024691[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
+   out_8701591355184024691[7] = state[7];
+   out_8701591355184024691[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_8557949136162757558) {
-   out_8557949136162757558[0] = 1;
-   out_8557949136162757558[1] = 0;
-   out_8557949136162757558[2] = 0;
-   out_8557949136162757558[3] = 0;
-   out_8557949136162757558[4] = 0;
-   out_8557949136162757558[5] = 0;
-   out_8557949136162757558[6] = 0;
-   out_8557949136162757558[7] = 0;
-   out_8557949136162757558[8] = 0;
-   out_8557949136162757558[9] = 0;
-   out_8557949136162757558[10] = 1;
-   out_8557949136162757558[11] = 0;
-   out_8557949136162757558[12] = 0;
-   out_8557949136162757558[13] = 0;
-   out_8557949136162757558[14] = 0;
-   out_8557949136162757558[15] = 0;
-   out_8557949136162757558[16] = 0;
-   out_8557949136162757558[17] = 0;
-   out_8557949136162757558[18] = 0;
-   out_8557949136162757558[19] = 0;
-   out_8557949136162757558[20] = 1;
-   out_8557949136162757558[21] = 0;
-   out_8557949136162757558[22] = 0;
-   out_8557949136162757558[23] = 0;
-   out_8557949136162757558[24] = 0;
-   out_8557949136162757558[25] = 0;
-   out_8557949136162757558[26] = 0;
-   out_8557949136162757558[27] = 0;
-   out_8557949136162757558[28] = 0;
-   out_8557949136162757558[29] = 0;
-   out_8557949136162757558[30] = 1;
-   out_8557949136162757558[31] = 0;
-   out_8557949136162757558[32] = 0;
-   out_8557949136162757558[33] = 0;
-   out_8557949136162757558[34] = 0;
-   out_8557949136162757558[35] = 0;
-   out_8557949136162757558[36] = 0;
-   out_8557949136162757558[37] = 0;
-   out_8557949136162757558[38] = 0;
-   out_8557949136162757558[39] = 0;
-   out_8557949136162757558[40] = 1;
-   out_8557949136162757558[41] = 0;
-   out_8557949136162757558[42] = 0;
-   out_8557949136162757558[43] = 0;
-   out_8557949136162757558[44] = 0;
-   out_8557949136162757558[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
-   out_8557949136162757558[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_8557949136162757558[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_8557949136162757558[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_8557949136162757558[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
-   out_8557949136162757558[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_8557949136162757558[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_8557949136162757558[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_8557949136162757558[53] = -9.8100000000000005*dt;
-   out_8557949136162757558[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
-   out_8557949136162757558[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_8557949136162757558[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_8557949136162757558[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_8557949136162757558[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
-   out_8557949136162757558[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_8557949136162757558[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
-   out_8557949136162757558[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_8557949136162757558[62] = 0;
-   out_8557949136162757558[63] = 0;
-   out_8557949136162757558[64] = 0;
-   out_8557949136162757558[65] = 0;
-   out_8557949136162757558[66] = 0;
-   out_8557949136162757558[67] = 0;
-   out_8557949136162757558[68] = 0;
-   out_8557949136162757558[69] = 0;
-   out_8557949136162757558[70] = 1;
-   out_8557949136162757558[71] = 0;
-   out_8557949136162757558[72] = 0;
-   out_8557949136162757558[73] = 0;
-   out_8557949136162757558[74] = 0;
-   out_8557949136162757558[75] = 0;
-   out_8557949136162757558[76] = 0;
-   out_8557949136162757558[77] = 0;
-   out_8557949136162757558[78] = 0;
-   out_8557949136162757558[79] = 0;
-   out_8557949136162757558[80] = 1;
+void F_fun(double *state, double dt, double *out_4180724957458131151) {
+   out_4180724957458131151[0] = 1;
+   out_4180724957458131151[1] = 0;
+   out_4180724957458131151[2] = 0;
+   out_4180724957458131151[3] = 0;
+   out_4180724957458131151[4] = 0;
+   out_4180724957458131151[5] = 0;
+   out_4180724957458131151[6] = 0;
+   out_4180724957458131151[7] = 0;
+   out_4180724957458131151[8] = 0;
+   out_4180724957458131151[9] = 0;
+   out_4180724957458131151[10] = 1;
+   out_4180724957458131151[11] = 0;
+   out_4180724957458131151[12] = 0;
+   out_4180724957458131151[13] = 0;
+   out_4180724957458131151[14] = 0;
+   out_4180724957458131151[15] = 0;
+   out_4180724957458131151[16] = 0;
+   out_4180724957458131151[17] = 0;
+   out_4180724957458131151[18] = 0;
+   out_4180724957458131151[19] = 0;
+   out_4180724957458131151[20] = 1;
+   out_4180724957458131151[21] = 0;
+   out_4180724957458131151[22] = 0;
+   out_4180724957458131151[23] = 0;
+   out_4180724957458131151[24] = 0;
+   out_4180724957458131151[25] = 0;
+   out_4180724957458131151[26] = 0;
+   out_4180724957458131151[27] = 0;
+   out_4180724957458131151[28] = 0;
+   out_4180724957458131151[29] = 0;
+   out_4180724957458131151[30] = 1;
+   out_4180724957458131151[31] = 0;
+   out_4180724957458131151[32] = 0;
+   out_4180724957458131151[33] = 0;
+   out_4180724957458131151[34] = 0;
+   out_4180724957458131151[35] = 0;
+   out_4180724957458131151[36] = 0;
+   out_4180724957458131151[37] = 0;
+   out_4180724957458131151[38] = 0;
+   out_4180724957458131151[39] = 0;
+   out_4180724957458131151[40] = 1;
+   out_4180724957458131151[41] = 0;
+   out_4180724957458131151[42] = 0;
+   out_4180724957458131151[43] = 0;
+   out_4180724957458131151[44] = 0;
+   out_4180724957458131151[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
+   out_4180724957458131151[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_4180724957458131151[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_4180724957458131151[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_4180724957458131151[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
+   out_4180724957458131151[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_4180724957458131151[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_4180724957458131151[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_4180724957458131151[53] = -9.8100000000000005*dt;
+   out_4180724957458131151[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
+   out_4180724957458131151[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_4180724957458131151[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_4180724957458131151[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_4180724957458131151[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
+   out_4180724957458131151[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_4180724957458131151[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
+   out_4180724957458131151[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_4180724957458131151[62] = 0;
+   out_4180724957458131151[63] = 0;
+   out_4180724957458131151[64] = 0;
+   out_4180724957458131151[65] = 0;
+   out_4180724957458131151[66] = 0;
+   out_4180724957458131151[67] = 0;
+   out_4180724957458131151[68] = 0;
+   out_4180724957458131151[69] = 0;
+   out_4180724957458131151[70] = 1;
+   out_4180724957458131151[71] = 0;
+   out_4180724957458131151[72] = 0;
+   out_4180724957458131151[73] = 0;
+   out_4180724957458131151[74] = 0;
+   out_4180724957458131151[75] = 0;
+   out_4180724957458131151[76] = 0;
+   out_4180724957458131151[77] = 0;
+   out_4180724957458131151[78] = 0;
+   out_4180724957458131151[79] = 0;
+   out_4180724957458131151[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_5666050874031952366) {
-   out_5666050874031952366[0] = state[6];
+void h_25(double *state, double *unused, double *out_76571637109634956) {
+   out_76571637109634956[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_3274454347497297512) {
-   out_3274454347497297512[0] = 0;
-   out_3274454347497297512[1] = 0;
-   out_3274454347497297512[2] = 0;
-   out_3274454347497297512[3] = 0;
-   out_3274454347497297512[4] = 0;
-   out_3274454347497297512[5] = 0;
-   out_3274454347497297512[6] = 1;
-   out_3274454347497297512[7] = 0;
-   out_3274454347497297512[8] = 0;
+void H_25(double *state, double *unused, double *out_7505390445787524055) {
+   out_7505390445787524055[0] = 0;
+   out_7505390445787524055[1] = 0;
+   out_7505390445787524055[2] = 0;
+   out_7505390445787524055[3] = 0;
+   out_7505390445787524055[4] = 0;
+   out_7505390445787524055[5] = 0;
+   out_7505390445787524055[6] = 1;
+   out_7505390445787524055[7] = 0;
+   out_7505390445787524055[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_7890691293906506169) {
-   out_7890691293906506169[0] = state[4];
-   out_7890691293906506169[1] = state[5];
+void h_24(double *state, double *unused, double *out_6249911379334495626) {
+   out_6249911379334495626[0] = state[4];
+   out_6249911379334495626[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_5447103946502797078) {
-   out_5447103946502797078[0] = 0;
-   out_5447103946502797078[1] = 0;
-   out_5447103946502797078[2] = 0;
-   out_5447103946502797078[3] = 0;
-   out_5447103946502797078[4] = 1;
-   out_5447103946502797078[5] = 0;
-   out_5447103946502797078[6] = 0;
-   out_5447103946502797078[7] = 0;
-   out_5447103946502797078[8] = 0;
-   out_5447103946502797078[9] = 0;
-   out_5447103946502797078[10] = 0;
-   out_5447103946502797078[11] = 0;
-   out_5447103946502797078[12] = 0;
-   out_5447103946502797078[13] = 0;
-   out_5447103946502797078[14] = 1;
-   out_5447103946502797078[15] = 0;
-   out_5447103946502797078[16] = 0;
-   out_5447103946502797078[17] = 0;
+void H_24(double *state, double *unused, double *out_2231896162874511461) {
+   out_2231896162874511461[0] = 0;
+   out_2231896162874511461[1] = 0;
+   out_2231896162874511461[2] = 0;
+   out_2231896162874511461[3] = 0;
+   out_2231896162874511461[4] = 1;
+   out_2231896162874511461[5] = 0;
+   out_2231896162874511461[6] = 0;
+   out_2231896162874511461[7] = 0;
+   out_2231896162874511461[8] = 0;
+   out_2231896162874511461[9] = 0;
+   out_2231896162874511461[10] = 0;
+   out_2231896162874511461[11] = 0;
+   out_2231896162874511461[12] = 0;
+   out_2231896162874511461[13] = 0;
+   out_2231896162874511461[14] = 1;
+   out_2231896162874511461[15] = 0;
+   out_2231896162874511461[16] = 0;
+   out_2231896162874511461[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_5390856811747446477) {
-   out_5390856811747446477[0] = state[4];
+void h_30(double *state, double *unused, double *out_198622425174870933) {
+   out_198622425174870933[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_3642235993994319243) {
-   out_3642235993994319243[0] = 0;
-   out_3642235993994319243[1] = 0;
-   out_3642235993994319243[2] = 0;
-   out_3642235993994319243[3] = 0;
-   out_3642235993994319243[4] = 1;
-   out_3642235993994319243[5] = 0;
-   out_3642235993994319243[6] = 0;
-   out_3642235993994319243[7] = 0;
-   out_3642235993994319243[8] = 0;
+void H_30(double *state, double *unused, double *out_6413657297794419363) {
+   out_6413657297794419363[0] = 0;
+   out_6413657297794419363[1] = 0;
+   out_6413657297794419363[2] = 0;
+   out_6413657297794419363[3] = 0;
+   out_6413657297794419363[4] = 1;
+   out_6413657297794419363[5] = 0;
+   out_6413657297794419363[6] = 0;
+   out_6413657297794419363[7] = 0;
+   out_6413657297794419363[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_4304130163390982150) {
-   out_4304130163390982150[0] = state[7];
+void h_26(double *state, double *unused, double *out_5679663062383844155) {
+   out_5679663062383844155[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_7015957666371353736) {
-   out_7015957666371353736[0] = 0;
-   out_7015957666371353736[1] = 0;
-   out_7015957666371353736[2] = 0;
-   out_7015957666371353736[3] = 0;
-   out_7015957666371353736[4] = 0;
-   out_7015957666371353736[5] = 0;
-   out_7015957666371353736[6] = 0;
-   out_7015957666371353736[7] = 1;
-   out_7015957666371353736[8] = 0;
+void H_26(double *state, double *unused, double *out_7199850309047971337) {
+   out_7199850309047971337[0] = 0;
+   out_7199850309047971337[1] = 0;
+   out_7199850309047971337[2] = 0;
+   out_7199850309047971337[3] = 0;
+   out_7199850309047971337[4] = 0;
+   out_7199850309047971337[5] = 0;
+   out_7199850309047971337[6] = 0;
+   out_7199850309047971337[7] = 1;
+   out_7199850309047971337[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_5777930495884533471) {
-   out_5777930495884533471[0] = state[3];
+void h_27(double *state, double *unused, double *out_4925101630551210689) {
+   out_4925101630551210689[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_1467472682193894332) {
-   out_1467472682193894332[0] = 0;
-   out_1467472682193894332[1] = 0;
-   out_1467472682193894332[2] = 0;
-   out_1467472682193894332[3] = 1;
-   out_1467472682193894332[4] = 0;
-   out_1467472682193894332[5] = 0;
-   out_1467472682193894332[6] = 0;
-   out_1467472682193894332[7] = 0;
-   out_1467472682193894332[8] = 0;
+void H_27(double *state, double *unused, double *out_8637251368978362580) {
+   out_8637251368978362580[0] = 0;
+   out_8637251368978362580[1] = 0;
+   out_8637251368978362580[2] = 0;
+   out_8637251368978362580[3] = 1;
+   out_8637251368978362580[4] = 0;
+   out_8637251368978362580[5] = 0;
+   out_8637251368978362580[6] = 0;
+   out_8637251368978362580[7] = 0;
+   out_8637251368978362580[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_4787540927070968960) {
-   out_4787540927070968960[0] = state[1];
+void h_29(double *state, double *unused, double *out_5200295692835716578) {
+   out_5200295692835716578[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_245890044675656701) {
-   out_245890044675656701[0] = 0;
-   out_245890044675656701[1] = 1;
-   out_245890044675656701[2] = 0;
-   out_245890044675656701[3] = 0;
-   out_245890044675656701[4] = 0;
-   out_245890044675656701[5] = 0;
-   out_245890044675656701[6] = 0;
-   out_245890044675656701[7] = 0;
-   out_245890044675656701[8] = 0;
+void H_29(double *state, double *unused, double *out_6923888642108811547) {
+   out_6923888642108811547[0] = 0;
+   out_6923888642108811547[1] = 1;
+   out_6923888642108811547[2] = 0;
+   out_6923888642108811547[3] = 0;
+   out_6923888642108811547[4] = 0;
+   out_6923888642108811547[5] = 0;
+   out_6923888642108811547[6] = 0;
+   out_6923888642108811547[7] = 0;
+   out_6923888642108811547[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_6006244842257137284) {
-   out_6006244842257137284[0] = state[0];
+void h_28(double *state, double *unused, double *out_8899456413702232613) {
+   out_8899456413702232613[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_5328289061745187275) {
-   out_5328289061745187275[0] = 1;
-   out_5328289061745187275[1] = 0;
-   out_5328289061745187275[2] = 0;
-   out_5328289061745187275[3] = 0;
-   out_5328289061745187275[4] = 0;
-   out_5328289061745187275[5] = 0;
-   out_5328289061745187275[6] = 0;
-   out_5328289061745187275[7] = 0;
-   out_5328289061745187275[8] = 0;
+void H_28(double *state, double *unused, double *out_8887518913674137798) {
+   out_8887518913674137798[0] = 1;
+   out_8887518913674137798[1] = 0;
+   out_8887518913674137798[2] = 0;
+   out_8887518913674137798[3] = 0;
+   out_8887518913674137798[4] = 0;
+   out_8887518913674137798[5] = 0;
+   out_8887518913674137798[6] = 0;
+   out_8887518913674137798[7] = 0;
+   out_8887518913674137798[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_5508864205680823221) {
-   out_5508864205680823221[0] = state[8];
+void h_31(double *state, double *unused, double *out_3714021165363657306) {
+   out_3714021165363657306[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_3243808385620337084) {
-   out_3243808385620337084[0] = 0;
-   out_3243808385620337084[1] = 0;
-   out_3243808385620337084[2] = 0;
-   out_3243808385620337084[3] = 0;
-   out_3243808385620337084[4] = 0;
-   out_3243808385620337084[5] = 0;
-   out_3243808385620337084[6] = 0;
-   out_3243808385620337084[7] = 0;
-   out_3243808385620337084[8] = 1;
+void H_31(double *state, double *unused, double *out_7474744483910563627) {
+   out_7474744483910563627[0] = 0;
+   out_7474744483910563627[1] = 0;
+   out_7474744483910563627[2] = 0;
+   out_7474744483910563627[3] = 0;
+   out_7474744483910563627[4] = 0;
+   out_7474744483910563627[5] = 0;
+   out_7474744483910563627[6] = 0;
+   out_7474744483910563627[7] = 0;
+   out_7474744483910563627[8] = 1;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
  update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
 }
-void car_err_fun(double *nom_x, double *delta_x, double *out_9086440322595224646) {
-  err_fun(nom_x, delta_x, out_9086440322595224646);
+void car_err_fun(double *nom_x, double *delta_x, double *out_7550976477549507356) {
+  err_fun(nom_x, delta_x, out_7550976477549507356);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_7805211099664095660) {
-  inv_err_fun(nom_x, true_x, out_7805211099664095660);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_3781853638498010) {
+  inv_err_fun(nom_x, true_x, out_3781853638498010);
 }
-void car_H_mod_fun(double *state, double *out_2822454019109077104) {
-  H_mod_fun(state, out_2822454019109077104);
+void car_H_mod_fun(double *state, double *out_6037661802737362721) {
+  H_mod_fun(state, out_6037661802737362721);
 }
-void car_f_fun(double *state, double dt, double *out_1333075080878653623) {
-  f_fun(state,  dt, out_1333075080878653623);
+void car_f_fun(double *state, double dt, double *out_8701591355184024691) {
+  f_fun(state,  dt, out_8701591355184024691);
 }
-void car_F_fun(double *state, double dt, double *out_8557949136162757558) {
-  F_fun(state,  dt, out_8557949136162757558);
+void car_F_fun(double *state, double dt, double *out_4180724957458131151) {
+  F_fun(state,  dt, out_4180724957458131151);
 }
-void car_h_25(double *state, double *unused, double *out_5666050874031952366) {
-  h_25(state, unused, out_5666050874031952366);
+void car_h_25(double *state, double *unused, double *out_76571637109634956) {
+  h_25(state, unused, out_76571637109634956);
 }
-void car_H_25(double *state, double *unused, double *out_3274454347497297512) {
-  H_25(state, unused, out_3274454347497297512);
+void car_H_25(double *state, double *unused, double *out_7505390445787524055) {
+  H_25(state, unused, out_7505390445787524055);
 }
-void car_h_24(double *state, double *unused, double *out_7890691293906506169) {
-  h_24(state, unused, out_7890691293906506169);
+void car_h_24(double *state, double *unused, double *out_6249911379334495626) {
+  h_24(state, unused, out_6249911379334495626);
 }
-void car_H_24(double *state, double *unused, double *out_5447103946502797078) {
-  H_24(state, unused, out_5447103946502797078);
+void car_H_24(double *state, double *unused, double *out_2231896162874511461) {
+  H_24(state, unused, out_2231896162874511461);
 }
-void car_h_30(double *state, double *unused, double *out_5390856811747446477) {
-  h_30(state, unused, out_5390856811747446477);
+void car_h_30(double *state, double *unused, double *out_198622425174870933) {
+  h_30(state, unused, out_198622425174870933);
 }
-void car_H_30(double *state, double *unused, double *out_3642235993994319243) {
-  H_30(state, unused, out_3642235993994319243);
+void car_H_30(double *state, double *unused, double *out_6413657297794419363) {
+  H_30(state, unused, out_6413657297794419363);
 }
-void car_h_26(double *state, double *unused, double *out_4304130163390982150) {
-  h_26(state, unused, out_4304130163390982150);
+void car_h_26(double *state, double *unused, double *out_5679663062383844155) {
+  h_26(state, unused, out_5679663062383844155);
 }
-void car_H_26(double *state, double *unused, double *out_7015957666371353736) {
-  H_26(state, unused, out_7015957666371353736);
+void car_H_26(double *state, double *unused, double *out_7199850309047971337) {
+  H_26(state, unused, out_7199850309047971337);
 }
-void car_h_27(double *state, double *unused, double *out_5777930495884533471) {
-  h_27(state, unused, out_5777930495884533471);
+void car_h_27(double *state, double *unused, double *out_4925101630551210689) {
+  h_27(state, unused, out_4925101630551210689);
 }
-void car_H_27(double *state, double *unused, double *out_1467472682193894332) {
-  H_27(state, unused, out_1467472682193894332);
+void car_H_27(double *state, double *unused, double *out_8637251368978362580) {
+  H_27(state, unused, out_8637251368978362580);
 }
-void car_h_29(double *state, double *unused, double *out_4787540927070968960) {
-  h_29(state, unused, out_4787540927070968960);
+void car_h_29(double *state, double *unused, double *out_5200295692835716578) {
+  h_29(state, unused, out_5200295692835716578);
 }
-void car_H_29(double *state, double *unused, double *out_245890044675656701) {
-  H_29(state, unused, out_245890044675656701);
+void car_H_29(double *state, double *unused, double *out_6923888642108811547) {
+  H_29(state, unused, out_6923888642108811547);
 }
-void car_h_28(double *state, double *unused, double *out_6006244842257137284) {
-  h_28(state, unused, out_6006244842257137284);
+void car_h_28(double *state, double *unused, double *out_8899456413702232613) {
+  h_28(state, unused, out_8899456413702232613);
 }
-void car_H_28(double *state, double *unused, double *out_5328289061745187275) {
-  H_28(state, unused, out_5328289061745187275);
+void car_H_28(double *state, double *unused, double *out_8887518913674137798) {
+  H_28(state, unused, out_8887518913674137798);
 }
-void car_h_31(double *state, double *unused, double *out_5508864205680823221) {
-  h_31(state, unused, out_5508864205680823221);
+void car_h_31(double *state, double *unused, double *out_3714021165363657306) {
+  h_31(state, unused, out_3714021165363657306);
 }
-void car_H_31(double *state, double *unused, double *out_3243808385620337084) {
-  H_31(state, unused, out_3243808385620337084);
+void car_H_31(double *state, double *unused, double *out_7474744483910563627) {
+  H_31(state, unused, out_7474744483910563627);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
  predict(in_x, in_P, in_Q, dt);
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_9086440322595224646);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_7805211099664095660);
-void car_H_mod_fun(double *state, double *out_2822454019109077104);
-void car_f_fun(double *state, double dt, double *out_1333075080878653623);
-void car_F_fun(double *state, double dt, double *out_8557949136162757558);
-void car_h_25(double *state, double *unused, double *out_5666050874031952366);
-void car_H_25(double *state, double *unused, double *out_3274454347497297512);
-void car_h_24(double *state, double *unused, double *out_7890691293906506169);
-void car_H_24(double *state, double *unused, double *out_5447103946502797078);
-void car_h_30(double *state, double *unused, double *out_5390856811747446477);
-void car_H_30(double *state, double *unused, double *out_3642235993994319243);
-void car_h_26(double *state, double *unused, double *out_4304130163390982150);
-void car_H_26(double *state, double *unused, double *out_7015957666371353736);
-void car_h_27(double *state, double *unused, double *out_5777930495884533471);
-void car_H_27(double *state, double *unused, double *out_1467472682193894332);
-void car_h_29(double *state, double *unused, double *out_4787540927070968960);
-void car_H_29(double *state, double *unused, double *out_245890044675656701);
-void car_h_28(double *state, double *unused, double *out_6006244842257137284);
-void car_H_28(double *state, double *unused, double *out_5328289061745187275);
-void car_h_31(double *state, double *unused, double *out_5508864205680823221);
-void car_H_31(double *state, double *unused, double *out_3243808385620337084);
+void car_err_fun(double *nom_x, double *delta_x, double *out_7550976477549507356);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_3781853638498010);
+void car_H_mod_fun(double *state, double *out_6037661802737362721);
+void car_f_fun(double *state, double dt, double *out_8701591355184024691);
+void car_F_fun(double *state, double dt, double *out_4180724957458131151);
+void car_h_25(double *state, double *unused, double *out_76571637109634956);
+void car_H_25(double *state, double *unused, double *out_7505390445787524055);
+void car_h_24(double *state, double *unused, double *out_6249911379334495626);
+void car_H_24(double *state, double *unused, double *out_2231896162874511461);
+void car_h_30(double *state, double *unused, double *out_198622425174870933);
+void car_H_30(double *state, double *unused, double *out_6413657297794419363);
+void car_h_26(double *state, double *unused, double *out_5679663062383844155);
+void car_H_26(double *state, double *unused, double *out_7199850309047971337);
+void car_h_27(double *state, double *unused, double *out_4925101630551210689);
+void car_H_27(double *state, double *unused, double *out_8637251368978362580);
+void car_h_29(double *state, double *unused, double *out_5200295692835716578);
+void car_H_29(double *state, double *unused, double *out_6923888642108811547);
+void car_h_28(double *state, double *unused, double *out_8899456413702232613);
+void car_H_28(double *state, double *unused, double *out_8887518913674137798);
+void car_h_31(double *state, double *unused, double *out_3714021165363657306);
+void car_H_31(double *state, double *unused, double *out_7474744483910563627);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void pose_err_fun(double *nom_x, double *delta_x, double *out_7322678164781620542);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6351713798129038522);
-void pose_H_mod_fun(double *state, double *out_7181373993335857162);
-void pose_f_fun(double *state, double dt, double *out_1544031639553813882);
-void pose_F_fun(double *state, double dt, double *out_4307246870374060335);
-void pose_h_4(double *state, double *unused, double *out_2329964328807098264);
-void pose_H_4(double *state, double *unused, double *out_3409566783491949590);
-void pose_h_10(double *state, double *unused, double *out_1764029327220956455);
-void pose_H_10(double *state, double *unused, double *out_5084421046690710562);
-void pose_h_13(double *state, double *unused, double *out_7799867727264054991);
-void pose_H_13(double *state, double *unused, double *out_2844964863810105486);
-void pose_h_14(double *state, double *unused, double *out_3740143774591228770);
-void pose_H_14(double *state, double *unused, double *out_6492355215787321886);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_1527400177482295711);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7620805951199866590);
+void pose_H_mod_fun(double *state, double *out_2041474721312435071);
+void pose_f_fun(double *state, double dt, double *out_3319995548118437592);
+void pose_F_fun(double *state, double dt, double *out_1297884238983223646);
+void pose_h_4(double *state, double *unused, double *out_8728651807226544018);
+void pose_H_4(double *state, double *unused, double *out_5315696800103384324);
+void pose_h_10(double *state, double *unused, double *out_3679201747496490736);
+void pose_H_10(double *state, double *unused, double *out_2420796569664890088);
+void pose_h_13(double *state, double *unused, double *out_4772104686591378688);
+void pose_H_13(double *state, double *unused, double *out_2103422974771051523);
+void pose_h_14(double *state, double *unused, double *out_3663326241180753804);
+void pose_H_14(double *state, double *unused, double *out_1352455943763899795);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }