From 321579b3beb6bca83c6839b57fa22c7c4b03cb61 Mon Sep 17 00:00:00 2001 From: Richard Date: Sat, 16 May 2026 11:38:02 -0700 Subject: [PATCH 1/3] Tests and simplification for ImuDeadReckoning --- python/PiFinder/integrator.py | 142 ++++------ .../pointing_model/imu_dead_reckoning.py | 256 +++++------------- .../imu_dead_reckoning_legacy.py | 241 +++++++++++++++++ python/tests/test_imu_dead_reckoning.py | 206 ++++++++++++++ .../test_imu_dead_reckoning_equivalence.py | 134 +++++++++ 5 files changed, 699 insertions(+), 280 deletions(-) create mode 100644 python/PiFinder/pointing_model/imu_dead_reckoning_legacy.py create mode 100644 python/tests/test_imu_dead_reckoning.py create mode 100644 python/tests/test_imu_dead_reckoning_equivalence.py diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py index fb96a418..7fe0947b 100644 --- a/python/PiFinder/integrator.py +++ b/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_center. """ 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_center"]["RA"], + solved["camera_center"]["Dec"], + solved["camera_center"]["Roll"], + deg=True, + ) + imu_dead_reckoning.solve(pointing, q_x2imu) def update_imu( @@ -183,64 +174,54 @@ 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. + + Camera/scope alignment is no longer applied here, so both + solved["RA"/"Dec"/"Roll"] and solved["camera_center"][...] receive the + same predicted pointing. """ - 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 + ra_deg, dec_deg, roll_deg = pointing.get(deg=True) + solved["RA"], solved["Dec"], solved["Roll"] = ra_deg, dec_deg, roll_deg + ( + solved["camera_center"]["RA"], + solved["camera_center"]["Dec"], + solved["camera_center"]["Roll"], + ) = ra_deg, dec_deg, roll_deg + 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 +246,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) diff --git a/python/PiFinder/pointing_model/imu_dead_reckoning.py b/python/PiFinder/pointing_model/imu_dead_reckoning.py index 8ddc5755..091c07b9 100644 --- a/python/PiFinder/pointing_model/imu_dead_reckoning.py +++ b/python/PiFinder/pointing_model/imu_dead_reckoning.py @@ -1,12 +1,19 @@ """ -IMU dead-reckoning extrapolates the scope pointing from the last plate-solved -coordinate using the IMU measurements. +IMU dead-reckoning: estimate pointing in the equatorial frame between +plate-solves using IMU measurements. -See quaternion_transforms.py for conventions used for quaternions. +The class maintains a single slowly-drifting reference-frame quaternion +(q_eq2x) which is re-solved whenever a matched (plate-solve, IMU) pair is +available. Between solves, dead-reckoning predictions are computed from +the latest IMU sample. -NOTE: All angles are in radians. +The class treats the plate-solve output as the pointing direction directly; +camera/scope alignment, if any, is applied upstream of solve(). All angles +are in radians. See quaternion_transforms.py for conventions. """ +from typing import Optional + import numpy as np import quaternion @@ -15,218 +22,87 @@ class ImuDeadReckoning: - """ - Use the plate-solved coordinates and IMU measurements to estimate the - pointing using plate solving when available or dead-reckoning using the IMU - when plate solving isn't available (e.g. when the scope is moving or - between frames). - - For an explanation of the theory and conventions used, see - PiFinder/pointing_model/README.md. - - This class uses the Equatorial frame as the reference and expects - plate-solved coordinates in (ra, dec). + """Dead-reckoning of pointing from matched plate-solve / IMU samples. - All angles are in radians. None is not allowed as inputs (use np.nan). + Stored state: + q_imu2pointing: fixed body rotation from IMU frame to the pointing + frame, set at construction from `screen_direction`. + q_eq2x: rotation from EQ to the IMU's internal reference frame X. + Initialised to NaN; set by solve() and cleared by reset(). - EXAMPLE: - # Set up: - imu_dead_reckoning = ImuDeadReckoning('flat') - imu_dead_reckoning.set_cam2scope_alignment(solved_cam, solved_scope) - - # Update with plate solved and IMU data: - imu_dead_reckoning.update_plate_solve_and_imu(solved_cam, q_x2imu) - - # Dead-reckoning using IMU - imu_dead_reckoning.update_imu(q_x2imu) + Math: + solve: q_eq2x = q_eq2pointing * (q_x2imu * q_imu2pointing).conj() + predict: q_eq2pointing = q_eq2x * q_x2imu * q_imu2pointing """ - # Alignment: - q_cam2scope: quaternion.quaternion - # IMU orientation: - q_imu2cam: quaternion.quaternion - q_cam2imu: quaternion.quaternion - # Putting them together: - q_imu2scope: quaternion.quaternion - - # The poinging of the camera and scope frames wrt the Equatorial frame. - # These get updated by plate solving and IMU dead-reckoning. - q_eq2cam: quaternion.quaternion - - # True when q_eq2cam is estimated by IMU dead-reckoning. - # False when set by plate solving - dead_reckoning: bool = False - tracking: bool = False # True when previous plate solve exists and is tracking - - # The IMU's unkonwn drifting reference frame X. This is solved for - # every time we have a simultaneous plate solve and IMU measurement. - q_eq2x: quaternion.quaternion = quaternion.quaternion(np.nan) # nan means not set + q_imu2pointing: quaternion.quaternion + q_eq2x: quaternion.quaternion def __init__(self, screen_direction: str): - """ """ - # IMU-to-camera orientation. Fixed by PiFinder type - self._set_screen_direction(screen_direction) + self.q_imu2pointing = self._q_imu2pointing(screen_direction) + self.q_eq2x = quaternion.quaternion(np.nan) - def set_cam2scope_alignment(self, solved_cam: RaDecRoll, solved_scope: RaDecRoll): - """ - Set the alignment between the PiFinder camera center and the scope - pointing. + def solve( + self, pointing: RaDecRoll, q_x2imu: quaternion.quaternion + ) -> None: + """Solve for q_eq2x from a matched plate-solve and IMU sample. - INPUTS: - solved_cam: Equatorial coordinate of the camera center at alignment. - solved_scope: Equatorial coordinate of the scope center at alignement. - """ - # Calculate q_scope2cam (alignment) - q_eq2cam = qt.radec2q_eq(solved_cam.ra, solved_cam.dec, solved_cam.roll) - q_eq2scope = qt.radec2q_eq(solved_scope.ra, solved_scope.dec, solved_scope.roll) - q_scope2cam = q_eq2scope.conjugate() * q_eq2cam - - # Set the alignmen attributes: - self.q_cam2scope = q_scope2cam.normalized().conj() - self.q_imu2scope = self.q_imu2cam * self.q_cam2scope - - def update_plate_solve_and_imu( - self, - solved_cam: RaDecRoll, - q_x2imu: quaternion.quaternion, - ): + No-op if `pointing` is invalid or `q_x2imu` is NaN — both + observations are needed to fix the drifting reference frame. """ - Update the state with the az/alt measurements from plate solving in the - camera frame. If the IMU measurement (which should be taken at the same - time) is available, q_x2imu (the unknown drifting reference frame) will - be solved for. - - INPUTS: - solved_cam: RA/Dec/Roll of the camera pointing from plate solving. - q_x2imu: [quaternion] Raw IMU measurement quaternions. This is the IMU - frame orientation wrt unknown drifting reference frame X. - """ - if not solved_cam.valid: - return # No update - - # Update plate-solved coord: Camera frame relative to the Equatorial - # frame where the +y camera frame (i.e. "up") points to the North - # Celestial Pole (NCP) -- i.e. zero roll offset: - self.q_eq2cam = qt.radec2q_eq(solved_cam.ra, solved_cam.dec, solved_cam.roll) - self.dead_reckoning = False # Using plate solve, no dead_reckoning - - # Update IMU: Calculate the IMU's unknown reference frame X using the - # plate solved coordinates and IMU measurements taken from the same - # time. If the IMU measurement isn't provided (i.e. None), the existing - # q_hor2x will continue to be used. - if not np.isnan(q_x2imu): - self.q_eq2x = self.q_eq2cam * self.q_cam2imu * q_x2imu.conj() - self.q_eq2x = self.q_eq2x.normalized() - self.tracking = True # We have a plate solve and IMU measurement - - def update_imu(self, q_x2imu: quaternion.quaternion): - """ - Update the state with the raw IMU measurement. Does a dead-reckoning - estimate of the camera and scope pointing. + if not pointing.valid or np.isnan(q_x2imu): + return + q_eq2pointing = qt.radec2q_eq(pointing.ra, pointing.dec, pointing.roll) + self.q_eq2x = ( + q_eq2pointing * (q_x2imu * self.q_imu2pointing).conj() + ).normalized() - INPUTS: - q_x2imu: Quaternion of the IMU orientation w.r.t. an unknown and drifting - reference frame X used by the IMU. - """ - if not np.isnan(self.q_eq2x): - # Dead reckoning estimate by IMU if q_hor2x has been estimated by a - # previous plate solve. - self.q_eq2cam = (self.q_eq2x * q_x2imu * self.q_imu2cam).normalized() - self.q_eq2scope = (self.q_eq2cam * self.q_cam2scope).normalized() + def predict(self, q_x2imu: quaternion.quaternion) -> Optional[RaDecRoll]: + """Dead-reckon current pointing from the latest IMU sample. - self.dead_reckoning = True - - def get_cam_radec(self) -> RaDecRoll: - """ - Returns the (ra, dec, roll) of the camera centre and a Boolean - dead_reckoning to indicate if the estimate is from dead-reckoning - (True) or from plate solving (False). + Returns None if solve() has never produced a valid q_eq2x. """ - ra_dec_roll = RaDecRoll.from_quaternion(self.q_eq2cam) + if not self.is_initialized(): + return None + q_eq2pointing = ( + self.q_eq2x * q_x2imu * self.q_imu2pointing + ).normalized() + return RaDecRoll.from_quaternion(q_eq2pointing) - return ra_dec_roll + def is_initialized(self) -> bool: + """True once solve() has produced a valid q_eq2x.""" + return not bool(np.isnan(self.q_eq2x)) - def get_scope_radec(self) -> RaDecRoll: - """ - Returns the (ra, dec, roll) of the scope and a Boolean dead_reckoning - to indicate if the estimate is from dead-reckoning (True) or from plate - solving (False). - """ - ra_dec_roll = RaDecRoll.from_quaternion(self.q_eq2scope) - - return ra_dec_roll - - def reset(self): - """ - Resets the internal states so that q_eq2x will be re-estimated. - """ - self.q_eq2x = None - self.tracking = False - - def _set_screen_direction(self, screen_direction: str): - """ - Sets the screen direction which determines the fixed orientation between - the IMU and camera (q_imu2cam). - """ - self.q_imu2cam = self._get_q_imu2cam(screen_direction) - self.q_cam2imu = self.q_imu2cam.conj() + def reset(self) -> None: + """Clear q_eq2x so the next solve() re-establishes the reference.""" + self.q_eq2x = quaternion.quaternion(np.nan) @staticmethod - def _get_q_imu2cam(screen_direction: str) -> quaternion.quaternion: - """ - Returns the quaternion that rotates the IMU frame to the camera frame - based on the screen direction. - - INPUTS: - screen_direction: "flat" or "upright" + def _q_imu2pointing(screen_direction: str) -> quaternion.quaternion: + """Fixed IMU-to-pointing rotation for the given PiFinder geometry. - RETURNS: - q_imu2cam: Quaternion that rotates the IMU frame to the camera frame. + This is hardware geometry only; no per-unit calibration is applied. """ if screen_direction == "left": - # Left: - # Rotate 90° around x_imu so that z_imu' points along z_camera q1 = qt.axis_angle2quat([1, 0, 0], np.pi / 2) - # Rotate 90° around z_imu' to align with the camera cooridnates q2 = qt.axis_angle2quat([0, 0, 1], np.pi / 2) - q_imu2cam = (q1 * q2).normalized() - elif screen_direction == "right": - # Right: - # Rotate -90° around x_imu so that z_imu' points along z_camera + return (q1 * q2).normalized() + if screen_direction == "right": q1 = qt.axis_angle2quat([1, 0, 0], -np.pi / 2) - # Rotate 90° around z_imu' to align with the camera cooridnates q2 = qt.axis_angle2quat([0, 0, 1], np.pi / 2) - q_imu2cam = (q1 * q2).normalized() - elif screen_direction == "straight": - # Straight: - # Rotate 180° around y_imu so that z_imu' points along z_camera + return (q1 * q2).normalized() + if screen_direction == "straight": q1 = qt.axis_angle2quat([0, 1, 0], np.pi) - # Rotate -90° around z_imu' to align with the camera cooridnates q2 = qt.axis_angle2quat([0, 0, 1], -np.pi / 2) - q_imu2cam = (q1 * q2).normalized() - elif screen_direction == "flat3": - # Flat v3: - # Camera is tilted 30° further down from the screen compared to Flat v2 - # Rotate -120° around y_imu so that z_imu' points along z_camera + return (q1 * q2).normalized() + if screen_direction == "flat3": q1 = qt.axis_angle2quat([0, 1, 0], -np.pi * 2 / 3) - # Rotate -90° around z_imu' to align with the camera cooridnates q2 = qt.axis_angle2quat([0, 0, 1], -np.pi / 2) - q_imu2cam = (q1 * q2).normalized() - elif screen_direction == "flat": - # Flat v2: - # Rotate -90° around y_imu so that z_imu' points along z_camera + return (q1 * q2).normalized() + if screen_direction == "flat": q1 = qt.axis_angle2quat([0, 1, 0], -np.pi / 2) - # Rotate -90° around z_imu' to align with the camera cooridnates q2 = qt.axis_angle2quat([0, 0, 1], -np.pi / 2) - q_imu2cam = (q1 * q2).normalized() - elif screen_direction == "as_bloom": - # As Dream: - # Camera points back up from the screen - # NOTE: Need to check if the orientation of the camera is correct - # Rotate +90° around z_imu to align with the camera cooridnates - # (+y_cam is along -x_imu) - q_imu2cam = qt.axis_angle2quat([0, 0, 1], +np.pi / 2) - else: - raise ValueError(f"Unsupported screen_direction: {screen_direction}") - - return q_imu2cam + return (q1 * q2).normalized() + if screen_direction == "as_bloom": + return qt.axis_angle2quat([0, 0, 1], np.pi / 2) + raise ValueError(f"Unsupported screen_direction: {screen_direction}") diff --git a/python/PiFinder/pointing_model/imu_dead_reckoning_legacy.py b/python/PiFinder/pointing_model/imu_dead_reckoning_legacy.py new file mode 100644 index 00000000..3836ed9f --- /dev/null +++ b/python/PiFinder/pointing_model/imu_dead_reckoning_legacy.py @@ -0,0 +1,241 @@ +""" +LEGACY copy of the pre-refactor ImuDeadReckoning, preserved so the new +simplified class in imu_dead_reckoning.py can be cross-checked against it. + +This file is temporary. Delete it (and +tests/test_imu_dead_reckoning_equivalence.py) once the new class is +verified in production. + +Original docstring follows: + +IMU dead-reckoning extrapolates the scope pointing from the last plate-solved +coordinate using the IMU measurements. + +See quaternion_transforms.py for conventions used for quaternions. + +NOTE: All angles are in radians. +""" + +import numpy as np +import quaternion + +from PiFinder.types.coordinates import RaDecRoll +import PiFinder.pointing_model.quaternion_transforms as qt + + +class ImuDeadReckoningLegacy: + """ + Use the plate-solved coordinates and IMU measurements to estimate the + pointing using plate solving when available or dead-reckoning using the IMU + when plate solving isn't available (e.g. when the scope is moving or + between frames). + + For an explanation of the theory and conventions used, see + PiFinder/pointing_model/README.md. + + This class uses the Equatorial frame as the reference and expects + plate-solved coordinates in (ra, dec). + + All angles are in radians. None is not allowed as inputs (use np.nan). + + EXAMPLE: + # Set up: + imu_dead_reckoning = ImuDeadReckoning('flat') + imu_dead_reckoning.set_cam2scope_alignment(solved_cam, solved_scope) + + # Update with plate solved and IMU data: + imu_dead_reckoning.update_plate_solve_and_imu(solved_cam, q_x2imu) + + # Dead-reckoning using IMU + imu_dead_reckoning.update_imu(q_x2imu) + """ + + # Alignment: + q_cam2scope: quaternion.quaternion + # IMU orientation: + q_imu2cam: quaternion.quaternion + q_cam2imu: quaternion.quaternion + # Putting them together: + q_imu2scope: quaternion.quaternion + + # The poinging of the camera and scope frames wrt the Equatorial frame. + # These get updated by plate solving and IMU dead-reckoning. + q_eq2cam: quaternion.quaternion + + # True when q_eq2cam is estimated by IMU dead-reckoning. + # False when set by plate solving + dead_reckoning: bool = False + tracking: bool = False # True when previous plate solve exists and is tracking + + # The IMU's unkonwn drifting reference frame X. This is solved for + # every time we have a simultaneous plate solve and IMU measurement. + q_eq2x: quaternion.quaternion = quaternion.quaternion(np.nan) # nan means not set + + def __init__(self, screen_direction: str): + """ """ + # IMU-to-camera orientation. Fixed by PiFinder type + self._set_screen_direction(screen_direction) + + def set_cam2scope_alignment(self, solved_cam: RaDecRoll, solved_scope: RaDecRoll): + """ + Set the alignment between the PiFinder camera center and the scope + pointing. + + INPUTS: + solved_cam: Equatorial coordinate of the camera center at alignment. + solved_scope: Equatorial coordinate of the scope center at alignement. + """ + # Calculate q_scope2cam (alignment) + q_eq2cam = qt.radec2q_eq(solved_cam.ra, solved_cam.dec, solved_cam.roll) + q_eq2scope = qt.radec2q_eq(solved_scope.ra, solved_scope.dec, solved_scope.roll) + q_scope2cam = q_eq2scope.conjugate() * q_eq2cam + + # Set the alignmen attributes: + self.q_cam2scope = q_scope2cam.normalized().conj() + self.q_imu2scope = self.q_imu2cam * self.q_cam2scope + + def update_plate_solve_and_imu( + self, + solved_cam: RaDecRoll, + q_x2imu: quaternion.quaternion, + ): + """ + Update the state with the az/alt measurements from plate solving in the + camera frame. If the IMU measurement (which should be taken at the same + time) is available, q_x2imu (the unknown drifting reference frame) will + be solved for. + + INPUTS: + solved_cam: RA/Dec/Roll of the camera pointing from plate solving. + q_x2imu: [quaternion] Raw IMU measurement quaternions. This is the IMU + frame orientation wrt unknown drifting reference frame X. + """ + if not solved_cam.valid: + return # No update + + # Update plate-solved coord: Camera frame relative to the Equatorial + # frame where the +y camera frame (i.e. "up") points to the North + # Celestial Pole (NCP) -- i.e. zero roll offset: + self.q_eq2cam = qt.radec2q_eq(solved_cam.ra, solved_cam.dec, solved_cam.roll) + self.dead_reckoning = False # Using plate solve, no dead_reckoning + + # Update IMU: Calculate the IMU's unknown reference frame X using the + # plate solved coordinates and IMU measurements taken from the same + # time. If the IMU measurement isn't provided (i.e. None), the existing + # q_hor2x will continue to be used. + if not np.isnan(q_x2imu): + self.q_eq2x = self.q_eq2cam * self.q_cam2imu * q_x2imu.conj() + self.q_eq2x = self.q_eq2x.normalized() + self.tracking = True # We have a plate solve and IMU measurement + + def update_imu(self, q_x2imu: quaternion.quaternion): + """ + Update the state with the raw IMU measurement. Does a dead-reckoning + estimate of the camera and scope pointing. + + INPUTS: + q_x2imu: Quaternion of the IMU orientation w.r.t. an unknown and drifting + reference frame X used by the IMU. + """ + if not np.isnan(self.q_eq2x): + # Dead reckoning estimate by IMU if q_hor2x has been estimated by a + # previous plate solve. + self.q_eq2cam = (self.q_eq2x * q_x2imu * self.q_imu2cam).normalized() + self.q_eq2scope = (self.q_eq2cam * self.q_cam2scope).normalized() + + self.dead_reckoning = True + + def get_cam_radec(self) -> RaDecRoll: + """ + Returns the (ra, dec, roll) of the camera centre and a Boolean + dead_reckoning to indicate if the estimate is from dead-reckoning + (True) or from plate solving (False). + """ + ra_dec_roll = RaDecRoll.from_quaternion(self.q_eq2cam) + + return ra_dec_roll + + def get_scope_radec(self) -> RaDecRoll: + """ + Returns the (ra, dec, roll) of the scope and a Boolean dead_reckoning + to indicate if the estimate is from dead-reckoning (True) or from plate + solving (False). + """ + ra_dec_roll = RaDecRoll.from_quaternion(self.q_eq2scope) + + return ra_dec_roll + + def reset(self): + """ + Resets the internal states so that q_eq2x will be re-estimated. + """ + self.q_eq2x = None + self.tracking = False + + def _set_screen_direction(self, screen_direction: str): + """ + Sets the screen direction which determines the fixed orientation between + the IMU and camera (q_imu2cam). + """ + self.q_imu2cam = self._get_q_imu2cam(screen_direction) + self.q_cam2imu = self.q_imu2cam.conj() + + @staticmethod + def _get_q_imu2cam(screen_direction: str) -> quaternion.quaternion: + """ + Returns the quaternion that rotates the IMU frame to the camera frame + based on the screen direction. + + INPUTS: + screen_direction: "flat" or "upright" + + RETURNS: + q_imu2cam: Quaternion that rotates the IMU frame to the camera frame. + """ + if screen_direction == "left": + # Left: + # Rotate 90° around x_imu so that z_imu' points along z_camera + q1 = qt.axis_angle2quat([1, 0, 0], np.pi / 2) + # Rotate 90° around z_imu' to align with the camera cooridnates + q2 = qt.axis_angle2quat([0, 0, 1], np.pi / 2) + q_imu2cam = (q1 * q2).normalized() + elif screen_direction == "right": + # Right: + # Rotate -90° around x_imu so that z_imu' points along z_camera + q1 = qt.axis_angle2quat([1, 0, 0], -np.pi / 2) + # Rotate 90° around z_imu' to align with the camera cooridnates + q2 = qt.axis_angle2quat([0, 0, 1], np.pi / 2) + q_imu2cam = (q1 * q2).normalized() + elif screen_direction == "straight": + # Straight: + # Rotate 180° around y_imu so that z_imu' points along z_camera + q1 = qt.axis_angle2quat([0, 1, 0], np.pi) + # Rotate -90° around z_imu' to align with the camera cooridnates + q2 = qt.axis_angle2quat([0, 0, 1], -np.pi / 2) + q_imu2cam = (q1 * q2).normalized() + elif screen_direction == "flat3": + # Flat v3: + # Camera is tilted 30° further down from the screen compared to Flat v2 + # Rotate -120° around y_imu so that z_imu' points along z_camera + q1 = qt.axis_angle2quat([0, 1, 0], -np.pi * 2 / 3) + # Rotate -90° around z_imu' to align with the camera cooridnates + q2 = qt.axis_angle2quat([0, 0, 1], -np.pi / 2) + q_imu2cam = (q1 * q2).normalized() + elif screen_direction == "flat": + # Flat v2: + # Rotate -90° around y_imu so that z_imu' points along z_camera + q1 = qt.axis_angle2quat([0, 1, 0], -np.pi / 2) + # Rotate -90° around z_imu' to align with the camera cooridnates + q2 = qt.axis_angle2quat([0, 0, 1], -np.pi / 2) + q_imu2cam = (q1 * q2).normalized() + elif screen_direction == "as_bloom": + # As Dream: + # Camera points back up from the screen + # NOTE: Need to check if the orientation of the camera is correct + # Rotate +90° around z_imu to align with the camera cooridnates + # (+y_cam is along -x_imu) + q_imu2cam = qt.axis_angle2quat([0, 0, 1], +np.pi / 2) + else: + raise ValueError(f"Unsupported screen_direction: {screen_direction}") + + return q_imu2cam diff --git a/python/tests/test_imu_dead_reckoning.py b/python/tests/test_imu_dead_reckoning.py new file mode 100644 index 00000000..09c97b13 --- /dev/null +++ b/python/tests/test_imu_dead_reckoning.py @@ -0,0 +1,206 @@ +""" +Unit tests for ImuDeadReckoning in PiFinder.pointing_model.imu_dead_reckoning. + +Tests pin the contract of the refactored class: + - solve(pointing, q_x2imu): solves for q_eq2x; no-op on invalid input + - predict(q_x2imu): returns RaDecRoll | None + - is_initialized(): True after a valid solve, False otherwise + - reset(): clears q_eq2x + +Equivalence with the pre-refactor implementation is tested separately in +test_imu_dead_reckoning_equivalence.py. +""" + +import numpy as np +import pytest +import quaternion + +import PiFinder.pointing_model.quaternion_transforms as qt +from PiFinder.pointing_model.imu_dead_reckoning import ImuDeadReckoning +from PiFinder.types.coordinates import RaDecRoll + + +# ---------------------------------------------------------------- helpers --- + +def assert_quat_close(q1, q2, abs_tol=1e-9): + """Compare two quaternions modulo the double cover.""" + assert qt.get_quat_angular_diff(q1, q2) == pytest.approx(0.0, abs=abs_tol) + + +def make_imu(axis=(0.0, 0.0, 1.0), theta=0.3): + return qt.axis_angle2quat(list(axis), theta).normalized() + + +@pytest.fixture +def dr(): + return ImuDeadReckoning("flat") + + +# ============================================== 1. solve =================== + +@pytest.mark.unit +class TestSolve: + """Behaviour of ImuDeadReckoning.solve().""" + + def test_solve_sets_q_eq2x_and_initializes(self, dr): + ra, dec, roll = 1.234, -0.4, 0.1 + q_x2imu = make_imu(theta=0.3) + + dr.solve(RaDecRoll(ra, dec, roll), q_x2imu) + + expected_q_eq2pointing = qt.radec2q_eq(ra, dec, roll) + expected_q_eq2x = ( + expected_q_eq2pointing * (q_x2imu * dr.q_imu2pointing).conj() + ).normalized() + assert_quat_close(dr.q_eq2x, expected_q_eq2x) + assert dr.is_initialized() is True + assert abs(dr.q_eq2x.norm() - 1.0) < 1e-12 + + def test_solve_with_invalid_pointing_is_no_op(self, dr): + invalid = RaDecRoll(np.nan, np.nan, np.nan) + assert invalid.valid is False + + dr.solve(invalid, make_imu()) + assert dr.is_initialized() is False + + def test_solve_with_nan_imu_is_no_op(self, dr): + dr.solve(RaDecRoll(1.0, 0.2, 0.0), quaternion.quaternion(np.nan)) + assert dr.is_initialized() is False + + def test_resolve_renews_q_eq2x(self, dr): + dr.solve(RaDecRoll(1.0, 0.2, 0.0), make_imu(theta=0.0)) + first = quaternion.quaternion(dr.q_eq2x) + + dr.solve(RaDecRoll(1.1, 0.25, 0.0), make_imu(theta=0.4)) + assert qt.get_quat_angular_diff(first, dr.q_eq2x) > 1e-6 + + +# ============================================== 2. predict ================= + +@pytest.mark.unit +class TestPredict: + """Behaviour of ImuDeadReckoning.predict().""" + + def test_predict_before_solve_returns_none(self, dr): + assert dr.predict(make_imu()) is None + + def test_predict_with_same_quat_returns_input_radec(self, dr): + ra, dec, roll = 1.0, 0.2, 0.0 + q = make_imu(theta=0.3) + dr.solve(RaDecRoll(ra, dec, roll), q) + + out = dr.predict(q) + assert out is not None + out_ra, out_dec, out_roll = out.get() + ra_diff = (out_ra - ra + np.pi) % (2 * np.pi) - np.pi + assert ra_diff == pytest.approx(0.0, abs=1e-9) + assert out_dec == pytest.approx(dec, abs=1e-9) + assert out_roll == pytest.approx(roll, abs=1e-9) + + def test_predict_rotates_by_imu_delta(self, dr): + q0 = make_imu(theta=0.0) + dr.solve(RaDecRoll(1.0, 0.2, 0.0), q0) + q_eq2pointing_at_solve = ( + dr.q_eq2x * q0 * dr.q_imu2pointing + ).normalized() + + delta = 0.05 + q1 = (q0 * qt.axis_angle2quat([0, 0, 1], delta)).normalized() + out = dr.predict(q1) + q_eq2pointing_pred = ( + dr.q_eq2x * q1 * dr.q_imu2pointing + ).normalized() + moved = qt.get_quat_angular_diff( + q_eq2pointing_at_solve, q_eq2pointing_pred + ) + assert moved == pytest.approx(delta, abs=1e-9) + # Sanity: predicted RaDecRoll matches the predicted quaternion. + assert_quat_close(out.as_quaternion(), q_eq2pointing_pred) + + def test_predict_returns_unit_quaternion_radec(self, dr): + dr.solve(RaDecRoll(1.0, 0.2, 0.0), make_imu(theta=0.0)) + out = dr.predict(make_imu(theta=0.4)) + assert isinstance(out, RaDecRoll) + assert out.valid is True + + @pytest.mark.parametrize( + "ra,dec,roll", + [ + (0.5, 0.3, 0.0), + (3.0, -0.7, 0.2), + (1.7, 0.0, -0.4), + (5.9, 0.45, 0.1), + ], + ) + def test_solve_then_predict_round_trip(self, dr, ra, dec, roll): + """solve(radec, q) then predict(q) returns radec (mod 2pi on RA).""" + q = make_imu(theta=0.2) + dr.solve(RaDecRoll(ra, dec, roll), q) + out_ra, out_dec, out_roll = dr.predict(q).get() + ra_diff = (out_ra - ra + np.pi) % (2 * np.pi) - np.pi + assert ra_diff == pytest.approx(0.0, abs=1e-9) + assert out_dec == pytest.approx(dec, abs=1e-9) + assert out_roll == pytest.approx(roll, abs=1e-9) + + +# ============================================== 3. reset =================== + +@pytest.mark.unit +class TestReset: + """Behaviour of ImuDeadReckoning.reset().""" + + def test_reset_clears_initialization(self, dr): + dr.solve(RaDecRoll(1.0, 0.2, 0.0), make_imu()) + assert dr.is_initialized() is True + + dr.reset() + assert dr.is_initialized() is False + assert isinstance(dr.q_eq2x, quaternion.quaternion) + assert np.isnan(dr.q_eq2x) + + def test_predict_after_reset_returns_none(self, dr): + dr.solve(RaDecRoll(1.0, 0.2, 0.0), make_imu()) + dr.reset() + assert dr.predict(make_imu(theta=0.5)) is None + + def test_can_re_solve_after_reset(self, dr): + dr.solve(RaDecRoll(1.0, 0.2, 0.0), make_imu()) + dr.reset() + + dr.solve(RaDecRoll(2.0, -0.1, 0.0), make_imu(theta=0.4)) + assert dr.is_initialized() is True + assert not np.isnan(dr.q_eq2x) + + +# =============================== 4. State sequence ========================= + +@pytest.mark.unit +def test_full_sequence_solve_predict_solve(): + """solve(A) -> predict -> solve(B) renews q_eq2x and preserves predict().""" + dr = ImuDeadReckoning("flat") + assert dr.is_initialized() is False + + radec_A = RaDecRoll(1.0, 0.2, 0.0) + q_imu_A = make_imu(theta=0.0) + dr.solve(radec_A, q_imu_A) + q_eq2x_after_A = quaternion.quaternion(dr.q_eq2x) + assert dr.is_initialized() is True + + # Predict mid-flight. + q_mid = (q_imu_A * qt.axis_angle2quat([0, 0, 1], 0.05)).normalized() + mid = dr.predict(q_mid) + assert mid is not None + + # Second solve at a different pointing renews q_eq2x. + radec_B = RaDecRoll(1.05, 0.21, 0.0) + q_imu_B = (q_imu_A * qt.axis_angle2quat([0, 0, 1], 0.10)).normalized() + dr.solve(radec_B, q_imu_B) + assert qt.get_quat_angular_diff(q_eq2x_after_A, dr.q_eq2x) > 1e-6 + + +# =============================== 5. Constructor ============================ + +@pytest.mark.unit +def test_invalid_screen_direction_raises(): + with pytest.raises(ValueError): + ImuDeadReckoning("sideways") diff --git a/python/tests/test_imu_dead_reckoning_equivalence.py b/python/tests/test_imu_dead_reckoning_equivalence.py new file mode 100644 index 00000000..83b02b00 --- /dev/null +++ b/python/tests/test_imu_dead_reckoning_equivalence.py @@ -0,0 +1,134 @@ +""" +Equivalence tests: the refactored ImuDeadReckoning must produce the same +scope-pointing results as the pre-refactor ImuDeadReckoningLegacy when the +legacy class is configured with identity alignment (solved_cam == solved_scope). + +This file is temporary. Delete it (and +PiFinder/pointing_model/imu_dead_reckoning_legacy.py) once the new class +is verified in production. +""" + +import numpy as np +import pytest + +import PiFinder.pointing_model.quaternion_transforms as qt +from PiFinder.pointing_model.imu_dead_reckoning import ImuDeadReckoning +from PiFinder.pointing_model.imu_dead_reckoning_legacy import ( + ImuDeadReckoningLegacy, +) +from PiFinder.types.coordinates import RaDecRoll + + +SCREEN_DIRECTIONS = ["flat", "flat3", "left", "right", "straight", "as_bloom"] + + +def make_imu(axis=(0.0, 0.0, 1.0), theta=0.0): + return qt.axis_angle2quat(list(axis), theta).normalized() + + +def assert_radec_close(new_pt, old_pt, abs_tol=1e-9): + """Compare two RaDecRoll outputs; RA wrap handled via modulo 2pi.""" + assert new_pt is not None and old_pt is not None + ra_diff = (new_pt.ra - old_pt.ra + np.pi) % (2 * np.pi) - np.pi + assert ra_diff == pytest.approx(0.0, abs=abs_tol), ( + f"ra: new={new_pt.ra} old={old_pt.ra}" + ) + assert new_pt.dec == pytest.approx(old_pt.dec, abs=abs_tol), ( + f"dec: new={new_pt.dec} old={old_pt.dec}" + ) + assert new_pt.roll == pytest.approx(old_pt.roll, abs=abs_tol), ( + f"roll: new={new_pt.roll} old={old_pt.roll}" + ) + + +@pytest.mark.unit +@pytest.mark.parametrize("screen_direction", SCREEN_DIRECTIONS) +def test_equivalence_mixed_sequence(screen_direction): + """New `predict` matches legacy `get_scope_radec` through a mixed sequence. + + Legacy class is configured with identity alignment so that scope == cam, + matching the new class's assumption that the plate-solve input *is* the + pointing frame. + """ + new = ImuDeadReckoning(screen_direction) + old = ImuDeadReckoningLegacy(screen_direction) + + # Identity alignment so legacy's scope == legacy's cam == new's pointing. + align_radec = RaDecRoll(1.0, 0.3, 0.0) + old.set_cam2scope_alignment(align_radec, align_radec) + + scenarios = [ + ("solve", RaDecRoll(1.0, 0.3, 0.0), make_imu(theta=0.0)), + ("predict", None, make_imu(theta=0.1)), + ("predict", None, make_imu(axis=(1, 0, 0), theta=0.05)), + ("solve", RaDecRoll(1.2, 0.25, 0.1), make_imu(axis=(0, 1, 0), theta=0.2)), + ("predict", None, make_imu(axis=(0, 1, 0), theta=0.3)), + ("predict", None, make_imu(axis=(1, 1, 0), theta=0.15)), + ] + + for action, radec, q in scenarios: + if action == "solve": + new.solve(radec, q) + old.update_plate_solve_and_imu(radec, q) + old.update_imu(q) # legacy needs explicit update_imu to fill q_eq2scope + else: + old.update_imu(q) + + new_pt = new.predict(q) + old_pt = old.get_scope_radec() + assert_radec_close(new_pt, old_pt) + + +@pytest.mark.unit +@pytest.mark.parametrize("screen_direction", SCREEN_DIRECTIONS) +def test_equivalence_after_reset(screen_direction): + """Reset and re-solve produces identical results in both classes.""" + new = ImuDeadReckoning(screen_direction) + old = ImuDeadReckoningLegacy(screen_direction) + + align = RaDecRoll(0.5, 0.1, 0.0) + old.set_cam2scope_alignment(align, align) + + new.solve(RaDecRoll(0.5, 0.1, 0.0), make_imu(theta=0.0)) + old.update_plate_solve_and_imu(RaDecRoll(0.5, 0.1, 0.0), make_imu(theta=0.0)) + + new.reset() + # Legacy reset() is buggy (sets q_eq2x = None) -- emulate the intended + # reset semantics by reinstantiating with a fresh alignment instead. + old = ImuDeadReckoningLegacy(screen_direction) + old.set_cam2scope_alignment(align, align) + + assert not new.is_initialized() + assert new.predict(make_imu(theta=0.2)) is None + + radec = RaDecRoll(2.0, -0.2, 0.05) + q = make_imu(axis=(0, 1, 0), theta=0.3) + new.solve(radec, q) + old.update_plate_solve_and_imu(radec, q) + old.update_imu(q) + + assert_radec_close(new.predict(q), old.get_scope_radec()) + + +@pytest.mark.unit +def test_equivalence_predict_before_solve_returns_none(): + """New class returns None before any solve; legacy never wrote outputs.""" + new = ImuDeadReckoning("flat") + assert new.predict(make_imu(theta=0.1)) is None + + +@pytest.mark.unit +def test_equivalence_invalid_pointing_is_no_op_for_both(): + """Invalid plate-solve input leaves both classes unchanged.""" + new = ImuDeadReckoning("flat") + old = ImuDeadReckoningLegacy("flat") + align = RaDecRoll(1.0, 0.3, 0.0) + old.set_cam2scope_alignment(align, align) + + invalid = RaDecRoll(np.nan, np.nan, np.nan) + q = make_imu(theta=0.1) + new.solve(invalid, q) + old.update_plate_solve_and_imu(invalid, q) + + assert not new.is_initialized() + assert old.tracking is False From 55db7673ab83e59917317d6e4055aa3aad071bb3 Mon Sep 17 00:00:00 2001 From: Richard Date: Mon, 18 May 2026 14:37:50 -0700 Subject: [PATCH 2/3] Rename q_imu2pointing to q_imu2cam in ImuDeadReckoning The body rotation set from screen_direction is the IMU-to-camera hardware geometry; naming it for that physical relationship is clearer than naming it for its role in the math. Docstring notes that the class treats the camera frame as the pointing frame. Co-Authored-By: Claude Opus 4.7 (1M context) --- .../pointing_model/imu_dead_reckoning.py | 24 ++++++++++--------- python/tests/test_imu_dead_reckoning.py | 6 ++--- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/python/PiFinder/pointing_model/imu_dead_reckoning.py b/python/PiFinder/pointing_model/imu_dead_reckoning.py index 091c07b9..ddbf4a2e 100644 --- a/python/PiFinder/pointing_model/imu_dead_reckoning.py +++ b/python/PiFinder/pointing_model/imu_dead_reckoning.py @@ -25,21 +25,23 @@ class ImuDeadReckoning: """Dead-reckoning of pointing from matched plate-solve / IMU samples. Stored state: - q_imu2pointing: fixed body rotation from IMU frame to the pointing - frame, set at construction from `screen_direction`. + q_imu2cam: fixed body rotation from IMU frame to the camera + frame (hardware geometry), set at construction from + `screen_direction`. The class treats the camera frame as the + pointing frame. q_eq2x: rotation from EQ to the IMU's internal reference frame X. Initialised to NaN; set by solve() and cleared by reset(). Math: - solve: q_eq2x = q_eq2pointing * (q_x2imu * q_imu2pointing).conj() - predict: q_eq2pointing = q_eq2x * q_x2imu * q_imu2pointing + solve: q_eq2x = q_eq2pointing * (q_x2imu * q_imu2cam).conj() + predict: q_eq2pointing = q_eq2x * q_x2imu * q_imu2cam """ - q_imu2pointing: quaternion.quaternion + q_imu2cam: quaternion.quaternion q_eq2x: quaternion.quaternion def __init__(self, screen_direction: str): - self.q_imu2pointing = self._q_imu2pointing(screen_direction) + self.q_imu2cam = self._q_imu2cam(screen_direction) self.q_eq2x = quaternion.quaternion(np.nan) def solve( @@ -54,7 +56,7 @@ def solve( return q_eq2pointing = qt.radec2q_eq(pointing.ra, pointing.dec, pointing.roll) self.q_eq2x = ( - q_eq2pointing * (q_x2imu * self.q_imu2pointing).conj() + q_eq2pointing * (q_x2imu * self.q_imu2cam).conj() ).normalized() def predict(self, q_x2imu: quaternion.quaternion) -> Optional[RaDecRoll]: @@ -65,7 +67,7 @@ def predict(self, q_x2imu: quaternion.quaternion) -> Optional[RaDecRoll]: if not self.is_initialized(): return None q_eq2pointing = ( - self.q_eq2x * q_x2imu * self.q_imu2pointing + self.q_eq2x * q_x2imu * self.q_imu2cam ).normalized() return RaDecRoll.from_quaternion(q_eq2pointing) @@ -78,10 +80,10 @@ def reset(self) -> None: self.q_eq2x = quaternion.quaternion(np.nan) @staticmethod - def _q_imu2pointing(screen_direction: str) -> quaternion.quaternion: - """Fixed IMU-to-pointing rotation for the given PiFinder geometry. + def _q_imu2cam(screen_direction: str) -> quaternion.quaternion: + """Fixed IMU-to-camera rotation for the given PiFinder geometry. - This is hardware geometry only; no per-unit calibration is applied. + Hardware geometry only; no per-unit calibration is applied. """ if screen_direction == "left": q1 = qt.axis_angle2quat([1, 0, 0], np.pi / 2) diff --git a/python/tests/test_imu_dead_reckoning.py b/python/tests/test_imu_dead_reckoning.py index 09c97b13..45274e5b 100644 --- a/python/tests/test_imu_dead_reckoning.py +++ b/python/tests/test_imu_dead_reckoning.py @@ -50,7 +50,7 @@ def test_solve_sets_q_eq2x_and_initializes(self, dr): expected_q_eq2pointing = qt.radec2q_eq(ra, dec, roll) expected_q_eq2x = ( - expected_q_eq2pointing * (q_x2imu * dr.q_imu2pointing).conj() + expected_q_eq2pointing * (q_x2imu * dr.q_imu2cam).conj() ).normalized() assert_quat_close(dr.q_eq2x, expected_q_eq2x) assert dr.is_initialized() is True @@ -101,14 +101,14 @@ def test_predict_rotates_by_imu_delta(self, dr): q0 = make_imu(theta=0.0) dr.solve(RaDecRoll(1.0, 0.2, 0.0), q0) q_eq2pointing_at_solve = ( - dr.q_eq2x * q0 * dr.q_imu2pointing + dr.q_eq2x * q0 * dr.q_imu2cam ).normalized() delta = 0.05 q1 = (q0 * qt.axis_angle2quat([0, 0, 1], delta)).normalized() out = dr.predict(q1) q_eq2pointing_pred = ( - dr.q_eq2x * q1 * dr.q_imu2pointing + dr.q_eq2x * q1 * dr.q_imu2cam ).normalized() moved = qt.get_quat_angular_diff( q_eq2pointing_at_solve, q_eq2pointing_pred From f41cb534b74b5e2a0bef19f951efee13a1c5e17a Mon Sep 17 00:00:00 2001 From: Richard Date: Mon, 18 May 2026 15:00:51 -0700 Subject: [PATCH 3/3] Remove camera_center from the solved dict camera_center duplicated the IMU-tracked camera FoV pointing that's already in top-level RA/Dec/Roll after the ImuDeadReckoning refactor. The chart in non-align mode now reads top-level RA/Dec/Roll directly; align mode continues to use camera_solve (frozen at the last plate solve). The integrator's solve() input switches to camera_solve, which is value-equivalent at solve time and the right semantic source. Co-Authored-By: Claude Opus 4.7 (1M context) --- python/PiFinder/integrator.py | 23 +++++++---------------- python/PiFinder/solver.py | 14 -------------- python/PiFinder/solver_main.py | 14 -------------- python/PiFinder/ui/align.py | 2 +- 4 files changed, 8 insertions(+), 45 deletions(-) diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py index 7fe0947b..7cddfe9e 100644 --- a/python/PiFinder/integrator.py +++ b/python/PiFinder/integrator.py @@ -148,7 +148,7 @@ 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.solve() that converts angles from degrees to - radians and builds RaDecRoll from the plate-solved camera_center. + radians and builds RaDecRoll from the plate-solved camera_solve. """ if (solved["RA"] is None) or (solved["Dec"] is None): return # No update @@ -159,9 +159,9 @@ def update_plate_solve_and_imu(imu_dead_reckoning: ImuDeadReckoning, solved: dic q_x2imu = solved["imu_quat"] pointing = RaDecRoll( - solved["camera_center"]["RA"], - solved["camera_center"]["Dec"], - solved["camera_center"]["Roll"], + solved["camera_solve"]["RA"], + solved["camera_solve"]["Dec"], + solved["camera_solve"]["Roll"], deg=True, ) imu_dead_reckoning.solve(pointing, q_x2imu) @@ -174,11 +174,8 @@ def update_imu( imu: dict, ): """ - Dead-reckon pointing from the latest IMU sample and write it into solved. - - Camera/scope alignment is no longer applied here, so both - solved["RA"/"Dec"/"Roll"] and solved["camera_center"][...] receive the - same predicted 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.is_initialized()): return @@ -207,13 +204,7 @@ def update_imu( pointing = imu_dead_reckoning.predict(q_x2imu) assert pointing is not None # guaranteed by the is_initialized() check above - ra_deg, dec_deg, roll_deg = pointing.get(deg=True) - solved["RA"], solved["Dec"], solved["Roll"] = ra_deg, dec_deg, roll_deg - ( - solved["camera_center"]["RA"], - solved["camera_center"]["Dec"], - solved["camera_center"]["Roll"], - ) = ra_deg, dec_deg, roll_deg + solved["RA"], solved["Dec"], solved["Roll"] = pointing.get(deg=True) solved["solve_time"] = imu_time solved["solve_source"] = "IMU" diff --git a/python/PiFinder/solver.py b/python/PiFinder/solver.py index 622b916d..ad1a7e7c 100644 --- a/python/PiFinder/solver.py +++ b/python/PiFinder/solver.py @@ -440,11 +440,6 @@ def solver( logger.warning("Long solver time: %i", total_tetra_time) if solved["RA"] is not None: - # RA, Dec, Roll at the center of the camera's FoV: - solved["camera_center"]["RA"] = solved["RA"] - solved["camera_center"]["Dec"] = solved["Dec"] - solved["camera_center"]["Roll"] = solved["Roll"] - # RA, Dec, Roll at the camera center from plate-solve (no IMU compensation) solved["camera_solve"]["RA"] = solved["RA"] solved["camera_solve"]["Dec"] = solved["Dec"] @@ -543,15 +538,6 @@ def get_initialized_solved_dict() -> dict: "RA": None, "Dec": None, "Roll": None, - # RA, Dec, Roll [deg] solved at the center of the camera FoV - # update by the IMU in the integrator - "camera_center": { - "RA": None, - "Dec": None, - "Roll": None, - "Alt": None, # NOTE: Altaz needed by catalogs for altaz mounts - "Az": None, - }, # RA, Dec, Roll [deg] from the camera, not updated by IMU in integrator "camera_solve": { "RA": None, diff --git a/python/PiFinder/solver_main.py b/python/PiFinder/solver_main.py index 5964e0e1..c8ab5180 100644 --- a/python/PiFinder/solver_main.py +++ b/python/PiFinder/solver_main.py @@ -46,15 +46,6 @@ def solver( align_ra = 0 align_dec = 0 solved = { - # RA, Dec, Roll solved at the center of the camera FoV - # update by integrator - "camera_center": { - "RA": None, - "Dec": None, - "Roll": None, - "Alt": None, - "Az": None, - }, # RA, Dec, Roll from the camera, not # affected by IMU in integrator "camera_solve": { @@ -206,11 +197,6 @@ def solver( logger.warning("Long solver time: %i", total_tetra_time) if solved["RA"] is not None: - # RA, Dec, Roll at the center of the camera's FoV: - solved["camera_center"]["RA"] = solved["RA"] - solved["camera_center"]["Dec"] = solved["Dec"] - solved["camera_center"]["Roll"] = solved["Roll"] - # RA, Dec, Roll at the center of the camera's not imu: solved["camera_solve"]["RA"] = solved["RA"] solved["camera_solve"]["Dec"] = solved["Dec"] diff --git a/python/PiFinder/ui/align.py b/python/PiFinder/ui/align.py index 71f9aa8b..1eff6696 100644 --- a/python/PiFinder/ui/align.py +++ b/python/PiFinder/ui/align.py @@ -222,7 +222,7 @@ def update(self, force=False): # the reticle to the star chart_center = self.solution["camera_solve"] else: - chart_center = self.solution["camera_center"] + chart_center = self.solution chart_rot_angle = get_chart_rotation_angle( chart_center["RA"], chart_center["Dec"],