Pointing frame code

imap_processing/spice/kernels.py

@@ -3,9 +3,12 @@
 import functools
 import logging
 import os
+from pathlib import Path
 from typing import Any, Callable, Optional
 
+import numpy as np
 import spiceypy as spice
+from numpy.typing import NDArray
 from spiceypy.utils.exceptions import SpiceyError
 
 logger = logging.getLogger(__name__)
@@ -156,3 +159,215 @@ def wrapper_ensure_spice(*args: Any, **kwargs: Any) -> Any:
         return _decorator(f_py)
     else:
         return _decorator
+
+
+@ensure_spice
+def create_pointing_frame(pointing_frame_dir: Optional[Path] = None) -> Path:
+    """
+    Create the pointing frame.
+
+    Parameters
+    ----------
+    pointing_frame_dir : Path
+        Directory of where pointing frame will be saved.
+
+    Returns
+    -------
+    path_to_pointing_frame : Path
+        Path to pointing frame.
+    """
+    ck_kernel, _, _, _ = spice.kdata(0, "ck")
+    directory = Path(ck_kernel).parent
+
+    # Get timerange for the pointing frame kernel.
+    et_start, et_end, et_times = _get_et_times(ck_kernel)
+    # Create a rotation matrix
+    rotation_matrix = _create_rotation_matrix(et_times)
+
+    # Convert the rotation matrix to a quaternion.
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.m2q
+    q_avg = spice.m2q(rotation_matrix)
+
+    # TODO: come up with naming convention.
+    if pointing_frame_dir is None:
+        path_to_pointing_frame = directory / "imap_dps.bc"
+    else:
+        path_to_pointing_frame = pointing_frame_dir / "imap_dps.bc"
+
+    # Open a new CK file, returning the handle of the opened file.
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.ckopn
+    handle = spice.ckopn(str(path_to_pointing_frame), "CK", 0)
+    # Get the SCLK ID.
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.gipool
+    id_imap_sclk = spice.gipool("CK_-43000_SCLK", 0, 1)
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.sce2c
+    # Convert start and end times to SCLK.
+    sclk_begtim = spice.sce2c(int(id_imap_sclk), et_start)
+    sclk_endtim = spice.sce2c(int(id_imap_sclk), et_end)
+
+    # Get the pointing frame ID.
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.gipool
+    id_imap_dps = spice.gipool("FRAME_IMAP_DPS", 0, 1)
+    # TODO: Figure out how to write new pointings to same CK kernel.
+    # Create the pointing frame kernel.
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.ckw02
+    spice.ckw02(
+        # Handle of an open CK file.
+        handle,
+        # Start time of the segment.
+        sclk_begtim,
+        # End time of the segment.
+        sclk_endtim,
+        # Pointing frame ID.
+        int(id_imap_dps),
+        # Reference frame.
+        "ECLIPJ2000",  # Reference frame
+        # Identifier.
+        "IMAP_DPS",
+        # Number of pointing intervals.
+        1,
+        # Start times of individual pointing records within segment.
+        # Since there is only a single record this is equal to sclk_begtim.
+        np.array([sclk_begtim]),
+        # End times of individual pointing records within segment.
+        # Since there is only a single record this is equal to sclk_endtim.
+        np.array([sclk_endtim]),  # Single stop time
+        # Average quaternion.
+        q_avg,
+        # 0.0 Angular rotation terms.
+        np.array([0.0, 0.0, 0.0]),
+        # Rates (seconds per tick) at which the quaternion and
+        # angular velocity change.
+        np.array([1.0]),
+    )
+
+    # Close CK file.
+    spice.ckcls(handle)
+
+    return path_to_pointing_frame
+
+
+@ensure_spice
+def _get_et_times(ck_kernel: str) -> tuple[float, float, np.ndarray]:
+    """
+    Get times for pointing start and stop.
+
+    Parameters
+    ----------
+    ck_kernel : str
+        Path of ck_kernel used to create the pointing frame.
+
+    Returns
+    -------
+    et_start : float
+        Pointing start time.
+    et_end : float
+        Pointing end time.
+    et_times : numpy.ndarray
+        Array of times between et_start and et_end.
+    """
+    # Get the spacecraft ID.
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.gipool
+    id_imap_spacecraft = spice.gipool("FRAME_IMAP_SPACECRAFT", 0, 1)
+
+    # TODO: Queried pointing start and stop times here.
+
+    # Get the coverage window
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.ckcov
+    cover = spice.ckcov(ck_kernel, int(id_imap_spacecraft), True, "SEGMENT", 0, "TDB")
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.wnfetd
+    et_start, et_end = spice.wnfetd(cover, 0)
+    # 1 spin/15 seconds; 10 quaternions / spin
+    num_samples = (et_end - et_start) / 15 * 10
+    et_times = np.linspace(et_start, et_end, int(num_samples))
+
+    return et_start, et_end, et_times
+
+
+@ensure_spice
+def _average_quaternions(et_times: np.ndarray) -> NDArray:
+    """
+    Average the quaternions.
+
+    Parameters
+    ----------
+    et_times : numpy.ndarray
+        Array of times between et_start and et_end.
+
+    Returns
+    -------
+    q_avg : np.ndarray
+        Average quaternion.
+    """
+    aggregate = np.zeros((4, 4))
+    for tdb in et_times:
+        # we use a quick and dirty method here for grabbing the quaternions
+        # from the attitude kernel.  Depending on how well the kernel input
+        # data is built and sampled, there may or may not be aliasing with this
+        # approach.  If it turns out that we need to pull the quaternions
+        # directly from the CK there are several routines that exist to do this
+        # but it's not straight forward.  We'll revisit this if needed.
+
+        # Rotation matrix from IMAP spacecraft frame to ECLIPJ2000.
+        # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.pxform
+        body_rots = spice.pxform("IMAP_SPACECRAFT", "ECLIPJ2000", tdb)
+        # Convert rotation matrix to quaternion.
+        # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.m2q
+        body_quat = spice.m2q(body_rots)
+
+        # Standardize the quaternion so that they may be compared.
+        body_quat = body_quat * np.sign(body_quat[0])
+        # Aggregate quaternions into a single matrix.
+        aggregate += np.outer(body_quat, body_quat)
+
+    # Reference: "On Averaging Rotations"
+    # Link: https://link.springer.com/content/pdf/10.1023/A:1011129215388.pdf
+    aggregate /= len(et_times)
+
+    # Compute eigen values and vectors of the matrix A
+    # Eigenvalues tell you how much "influence" each
+    # direction (eigenvector) has.
+    # The largest eigenvalue corresponds to the direction
+    # that has the most influence.
+    # The eigenvector corresponding to the largest
+    # eigenvalue points in the direction that has the most
+    # combined rotation influence.
+    eigvals, eigvecs = np.linalg.eig(aggregate)
+    # q0: The scalar part of the quaternion.
+    # q1, q2, q3: The vector part of the quaternion.
+    q_avg = eigvecs[:, np.argmax(eigvals)]
+
+    return q_avg
+
+
+@ensure_spice
+def _create_rotation_matrix(et_times: np.ndarray) -> NDArray:
+    """
+    Create a rotation matrix.
+
+    Parameters
+    ----------
+    et_times : numpy.ndarray
+        Array of times between et_start and et_end.
+
+    Returns
+    -------
+    rotation_matrix : np.ndarray
+        Rotation matrix.
+    """
+    # Averaged quaternions.
+    q_avg = _average_quaternions(et_times)
+
+    # Converts the averaged quaternion (q_avg) into a rotation matrix
+    # and get inertial z axis.
+    # https://spiceypy.readthedocs.io/en/main/documentation.html#spiceypy.spiceypy.q2m
+    z_avg = spice.q2m(list(q_avg))[:, 2]
+    # y_avg is perpendicular to both z_avg and the standard Z-axis.
+    y_avg = np.cross(z_avg, [0, 0, 1])
+    # x_avg is perpendicular to y_avg and z_avg.
+    x_avg = np.cross(y_avg, z_avg)
+
+    # Construct the rotation matrix from x_avg, y_avg, z_avg
+    rotation_matrix = np.asarray([x_avg, y_avg, z_avg])
+
+    return rotation_matrix