diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py index fb96a418..7cddfe9e 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_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) diff --git a/python/PiFinder/pointing_model/imu_dead_reckoning.py b/python/PiFinder/pointing_model/imu_dead_reckoning.py index 8ddc5755..ddbf4a2e 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,89 @@ 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). - - 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) + """Dead-reckoning of pointing from matched plate-solve / IMU samples. + + Stored state: + 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_imu2cam).conj() + predict: q_eq2pointing = q_eq2x * q_x2imu * q_imu2cam """ - # 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_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_imu2cam = self._q_imu2cam(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, - ): - """ - 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): + 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 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_imu2cam).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() - - self.dead_reckoning = True + def predict(self, q_x2imu: quaternion.quaternion) -> Optional[RaDecRoll]: + """Dead-reckon current pointing from the latest IMU sample. - 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). + Returns None if solve() has never produced a valid q_eq2x. """ - ra_dec_roll = RaDecRoll.from_quaternion(self.q_eq2scope) + if not self.is_initialized(): + return None + q_eq2pointing = ( + self.q_eq2x * q_x2imu * self.q_imu2cam + ).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 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_imu2cam(screen_direction: str) -> quaternion.quaternion: + """Fixed IMU-to-camera rotation for the given PiFinder geometry. - RETURNS: - q_imu2cam: Quaternion that rotates the IMU frame to the camera frame. + 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/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"], diff --git a/python/tests/test_imu_dead_reckoning.py b/python/tests/test_imu_dead_reckoning.py new file mode 100644 index 00000000..45274e5b --- /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_imu2cam).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_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_imu2cam + ).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