Tests and simplification for ImuDeadReckoning

python/PiFinder/integrator.py

@@ -54,7 +54,6 @@ def integrator(shared_state, solver_queue, console_queue, log_queue, is_debug=Fa
 
         # Set up dead-reckoning tracking by the IMU:
         imu_dead_reckoning = ImuDeadReckoning(cfg.get_option("screen_direction"))
-        # imu_dead_reckoning.set_cam2scope_alignment(q_scope2cam)  # TODO: Enable when q_scope2cam is available from alignment
 
         # This holds the last image solve position info
         # so we can delta for IMU updates
@@ -116,7 +115,7 @@ def integrator(shared_state, solver_queue, console_queue, log_queue, is_debug=Fa
                     shared_state.set_solution(solved)
                     shared_state.set_solve_state(False)
 
-            if imu_dead_reckoning.tracking and not pointing_updated:
+            if imu_dead_reckoning.is_initialized() and not pointing_updated:
                 # Previous plate-solve exists so use IMU dead-reckoning from
                 # the last plate solved coordinates.
                 imu = shared_state.imu()
@@ -148,32 +147,24 @@ def integrator(shared_state, solver_queue, console_queue, log_queue, is_debug=Fa
 
 def update_plate_solve_and_imu(imu_dead_reckoning: ImuDeadReckoning, solved: dict):
     """
-    Wrapper for ImuDeadReckoning.update_plate_solve_and_imu() to
-    interface angles in degrees to radians.
-
-    This updates the pointing model with the plate-solved coordinates and the
-    IMU measurements which are assumed to have been taken at the same time.
+    Wrapper for ImuDeadReckoning.solve() that converts angles from degrees to
+    radians and builds RaDecRoll from the plate-solved camera_solve.
     """
     if (solved["RA"] is None) or (solved["Dec"] is None):
         return  # No update
+
+    if solved["imu_quat"] is None:
+        q_x2imu = quaternion.quaternion(np.nan)
     else:
-        # Successfully plate solved & camera pointing exists
-        if solved["imu_quat"] is None:
-            q_x2imu = quaternion.quaternion(np.nan)
-        else:
-            q_x2imu = solved["imu_quat"]  # IMU measurement at the time of plate solving
-
-        # Update:
-        solved_cam = RaDecRoll(
-            solved["camera_center"]["RA"],
-            solved["camera_center"]["Dec"],
-            solved["camera_center"]["Roll"],
-            deg=True
-        )
-        imu_dead_reckoning.update_plate_solve_and_imu(solved_cam, q_x2imu)
+        q_x2imu = solved["imu_quat"]
 
-        # Set alignment. TODO: Do this once at alignment. Move out of here.
-        set_cam2scope_alignment(imu_dead_reckoning, solved)
+    pointing = RaDecRoll(
+        solved["camera_solve"]["RA"],
+        solved["camera_solve"]["Dec"],
+        solved["camera_solve"]["Roll"],
+        deg=True,
+    )
+    imu_dead_reckoning.solve(pointing, q_x2imu)
 
 
 def update_imu(
@@ -183,64 +174,45 @@ def update_imu(
     imu: dict,
 ):
     """
-    Updates the solved dictionary using IMU dead-reckoning from the last
-    solved pointing.
+    Dead-reckon pointing from the latest IMU sample and write it into
+    solved["RA"/"Dec"/"Roll"].
     """
-    if not (last_image_solve and imu_dead_reckoning.tracking):
-        return  # Need all of these to do IMU dead-reckoning
+    if not (last_image_solve and imu_dead_reckoning.is_initialized()):
+        return
 
     assert isinstance(
         imu["quat"], quaternion.quaternion
-    ), "Expecting quaternion.quaternion type"  # TODO: Can be removed later
-    q_x2imu = imu["quat"]  # Current IMU measurement (quaternion)
+    ), "Expecting quaternion.quaternion type"
+    q_x2imu = imu["quat"]
     imu_time = time.time()
 
-    # When moving, switch to tracking using the IMU
     angle_moved = qt.get_quat_angular_diff(last_image_solve["imu_quat"], q_x2imu)
-    if angle_moved > IMU_MOVED_ANG_THRESHOLD:
-        # Estimate camera pointing using IMU dead-reckoning
-        logger.debug(
-            "Track using IMU: Angle moved since last_image_solve = "
-            "{:}(> threshold = {:}) | IMU quat = ({:}, {:}, {:}, {:})".format(
-                np.rad2deg(angle_moved),
-                np.rad2deg(IMU_MOVED_ANG_THRESHOLD),
-                q_x2imu.w,
-                q_x2imu.x,
-                q_x2imu.y,
-                q_x2imu.z,
-            )
+    if angle_moved <= IMU_MOVED_ANG_THRESHOLD:
+        return
+
+    logger.debug(
+        "Track using IMU: Angle moved since last_image_solve = "
+        "{:}(> threshold = {:}) | IMU quat = ({:}, {:}, {:}, {:})".format(
+            np.rad2deg(angle_moved),
+            np.rad2deg(IMU_MOVED_ANG_THRESHOLD),
+            q_x2imu.w,
+            q_x2imu.x,
+            q_x2imu.y,
+            q_x2imu.z,
         )
+    )
 
-        # Dead-reckoning using IMU
-        imu_dead_reckoning.update_imu(q_x2imu)  # Latest IMU measurement
-
-        # Store current camera pointing estimate:
-        cam_eq = imu_dead_reckoning.get_cam_radec()
-        (
-            solved["camera_center"]["RA"],
-            solved["camera_center"]["Dec"],
-            solved["camera_center"]["Roll"],
-        ) = cam_eq.get(deg=True)
-
-        # Store the current scope pointing estimate
-        scope_eq = imu_dead_reckoning.get_scope_radec()
-        solved["RA"], solved["Dec"], solved["Roll"] = scope_eq.get(deg=True)
-        solved["solve_time"] = imu_time
-        solved["solve_source"] = "IMU"
-
-        # Logging for states updated in solved:
-        logger.debug(
-            "IMU update: scope: RA: {:}, Dec: {:}, Roll: {:}".format(
-                solved["RA"], solved["Dec"], solved["Roll"]
-            )
-        )
-        logger.debug(
-            "IMU update: camera_center: RA: {:}, Dec: {:}, Roll: {:}".format(
-                solved["camera_center"]["RA"],
-                solved["camera_center"]["Dec"],
-                solved["camera_center"]["Roll"],
-            )
+    pointing = imu_dead_reckoning.predict(q_x2imu)
+    assert pointing is not None  # guaranteed by the is_initialized() check above
+    solved["RA"], solved["Dec"], solved["Roll"] = pointing.get(deg=True)
+    solved["solve_time"] = imu_time
+    solved["solve_source"] = "IMU"
+
+    logger.debug(
+        "IMU update: pointing: RA: {:}, Dec: {:}, Roll: {:}".format(
+            solved["RA"], solved["Dec"], solved["Roll"]
         )
+    )
 
 
 def get_constellation(RA_deg, Dec_deg) -> str:
@@ -265,22 +237,3 @@ def get_alt_az(RA_deg, Dec_deg, location, dt) -> tuple[float | None, float | Non
         return calc_utils.sf_utils.radec_to_altaz(RA_deg, Dec_deg, dt)
 
 
-def set_cam2scope_alignment(imu_dead_reckoning: ImuDeadReckoning, solved: dict):
-    """
-    Set alignment.
-    TODO: Do this once at alignment
-    """
-    # RA, Dec of camera center::
-    solved_cam = RaDecRoll(
-        solved["camera_center"]["RA"],
-        solved["camera_center"]["Dec"],
-        solved["camera_center"]["Roll"],
-        deg=True
-    )
-
-    # RA, Dec of target (where scope is pointing):
-    solved["Roll"] = 0  # Target roll isn't calculated by Tetra3. Set to zero here
-    solved_scope = RaDecRoll(solved["RA"], solved["Dec"], solved["Roll"], deg=True)
-
-    # Set alignment in imu_dead_reckoning
-    imu_dead_reckoning.set_cam2scope_alignment(solved_cam, solved_scope)