Issue #192: Additional observations.

docs/source/release_notes.rst

@@ -12,6 +12,8 @@ Development Version
   This fixes compatibility with RLlib 2.33.0+. Note that this means that the satellite
   object passed to the environment is not the same object as the one used in the environment,
   as is the case for rewarders and communication objects.
+* Add additional observation properties for satellites and opportunities.
+
 
 Version 1.0.1
 -------------

src/bsk_rl/obs/observations.py

@@ -10,6 +10,7 @@
 from gymnasium import spaces
 
 from bsk_rl.utils.functional import vectorize_nested_dict
+from bsk_rl.utils.orbital import rv2HN
 
 if TYPE_CHECKING:  # pragma: no cover
     from bsk_rl.sats import Satellite
@@ -290,16 +291,38 @@ def _target_angle(sat, opp):
     return np.arccos(np.dot(vector_target_spacecraft_P_hat, sat.fsw.c_hat_P))
 
 
+def _target_angle_rate(sat, opp):
+    r_BN_P = sat.dynamics.v_BN_P
+    v_BN_P = sat.dynamics.v_BN_P
+    r_LP_P = opp["object"].r_LP_P
+    omega_BP_P = sat.dynamics.omega_BP_P
+    omega_CP_ref = (
+        omega_BP_P
+        - np.cross(v_BN_P, r_LP_P - r_BN_P) / np.linalg.norm(r_LP_P - r_BN_P) ** 2
+    )
+    return np.linalg.norm(omega_CP_ref)
+
+
+def _r_LB_H(sat, opp):
+    r_LP_P = opp["object"].r_LP_P
+    r_BN_N = sat.dynamics.r_BN_N
+    r_TB_N = sat.simulator.world.PN.T @ r_LP_P - r_BN_N
+    HN = rv2HN(sat.dynamics.r_BN_N, sat.dynamics.v_BN_N)
+    return HN @ r_TB_N
+
+
 class OpportunityProperties(Observation):
 
     _fn_map = {
         "priority": lambda sat, opp: opp["object"].priority,
         "r_LP_P": lambda sat, opp: opp["r_LP_P"],
+        "r_LB_H": _r_LB_H,
         "opportunity_open": lambda sat, opp: opp["window"][0] - sat.simulator.sim_time,
         "opportunity_mid": lambda sat, opp: sum(opp["window"]) / 2
         - sat.simulator.sim_time,
         "opportunity_close": lambda sat, opp: opp["window"][1] - sat.simulator.sim_time,
         "target_angle": _target_angle,
+        "target_angle_rate": _target_angle_rate,
     }
 
     def __init__(
@@ -334,10 +357,12 @@ def __init__(
 
                     * ``priority``: Priority of the target.
                     * ``r_LP_P``: Location of the target in the planet-fixed frame.
+                    * ``r_LB_H``: Location of the target in the Hill frame.
                     * ``opportunity_open``: Time until the opportunity opens.
                     * ``opportunity_mid``: Time until the opportunity midpoint.
                     * ``opportunity_close``: Time until the opportunity closes.
                     * ``target_angle``: Angle between the target and the satellite instrument direction.
+                    * ``target_angle_rate``: Rate difference between the target pointing frame and the body frame.
 
                 * ``norm`` `optional`: Value to normalize property by. Defaults to 1.0.
 

src/bsk_rl/sim/dyn.py

@@ -71,7 +71,7 @@
     check_aliveness_checkers,
     default_args,
 )
-from bsk_rl.utils.orbital import random_orbit
+from bsk_rl.utils.orbital import random_orbit, rv2HN, rv2omega
 
 if TYPE_CHECKING:  # pragma: no cover
     from bsk_rl.sats import Satellite
@@ -239,6 +239,15 @@ def omega_BP_P(self):
         omega_BP_N = omega_BN_N - self.world.omega_PN_N
         return np.matmul(self.world.PN, omega_BP_N)
 
+    @property
+    def omega_BH_H(self):
+        """Body angular velocity relative to Hill frame in Hill frame [rad/s]."""
+        omega_BN_N = np.matmul(self.BN.T, self.omega_BN_B)
+        omega_HN_N = rv2omega(self.r_BN_N, self.v_BN_N)
+        omega_BH_N = omega_BN_N - omega_HN_N
+        HN = rv2HN(self.r_BN_N, self.v_BN_N)
+        return HN @ omega_BH_N
+
     @property
     def battery_charge(self):
         """Battery charge [W*s]."""

src/bsk_rl/sim/fsw.py

@@ -60,6 +60,7 @@
     check_aliveness_checkers,
     default_args,
 )
+from bsk_rl.utils.orbital import rv2HN
 
 if TYPE_CHECKING:  # pragma: no cover
     from bsk_rl.sats import Satellite
@@ -634,6 +635,13 @@ def c_hat_P(self):
         c_hat_B = self.locPoint.pHat_B
         return np.matmul(self.dynamics.BP.T, c_hat_B)
 
+    @property
+    def c_hat_H(self):
+        """Instrument pointing direction in the hill frame."""
+        c_hat_B = self.locPoint.pHat_B
+        HN = rv2HN(self.satellite.dynamics.r_BN_N, self.satellite.dynamics.v_BN_N)
+        return HN @ self.satellite.dynamics.BN.T @ c_hat_B
+
     def _make_task_list(self) -> list[Task]:
         return super()._make_task_list() + [self.LocPointTask(self)]
 

src/bsk_rl/utils/orbital.py

@@ -459,6 +459,40 @@ def lla2ecef(lat: float, long: float, radius: float):
     )
 
 
+def rv2HN(r_N: np.ndarray, v_N: np.ndarray):
+    """Find the Hill frame rotation matrix from position and velocity.
+
+    Args:
+        r_N: Position vector in the inertial frame
+        v_N: Velocity vector in the inertial frame
+    Returns:
+        Hill frame rotation matrix HN
+    """
+    o_r_N = r_N / np.linalg.norm(r_N)
+    h_N = np.cross(r_N, v_N)
+    o_h_N = h_N / np.linalg.norm(h_N)
+    o_theta_N = np.cross(o_h_N, o_r_N)
+    HN = np.array([o_r_N, o_theta_N, o_h_N])
+    return HN
+
+
+def rv2omega(r_N: np.ndarray, v_N: np.ndarray):
+    """Find the Hill frame rotation rate from position and velocity.
+
+    Args:
+        r_N: Position vector in the inertial frame
+        v_N: Velocity vector in the inertial frame
+    Returns:
+        omega_HN_N: Angular velocity of the Hill frame in the inertial frame
+    """
+    o_r_N = r_N / np.linalg.norm(r_N)
+    h_N = np.cross(r_N, v_N)
+    o_h_N = h_N / np.linalg.norm(h_N)
+    o_theta_N = np.cross(o_h_N, o_r_N)
+    omega_HN_N = o_h_N * np.dot(v_N, o_theta_N) / np.linalg.norm(r_N)
+    return omega_HN_N
+
+
 __doc_title__ = "Orbital"
 __all__ = [
     "random_orbit",
@@ -467,4 +501,6 @@ def lla2ecef(lat: float, long: float, radius: float):
     "elevation",
     "walker_delta",
     "TrajectorySimulator",
+    "rv2HN",
+    "rv2omega",
 ]

tests/unittest/utils/test_orbital.py

@@ -177,3 +177,24 @@ def test_interpolators(self):
         assert (ts.r_BN_N(0) != ts.r_BN_N(1)).all()
         ts = orbital.TrajectorySimulator(self.epoch, oe=self.oe, mu=self.mu)
         assert (ts.r_BP_P(0) != ts.r_BP_P(1)).all()
+
+
+class TestFunctions:
+
+    def test_rv2HN(self):
+        r = np.array([1, 0, 0])
+        v = np.array([0, 1, 0])
+        HN = orbital.rv2HN(r, v)
+        assert np.allclose(HN, np.eye(3))
+
+    def test_rv2HN_nonunit(self):
+        r = np.array([1, 0, 0]) * 12345
+        v = np.array([0, 1, 0]) * 678.9
+        HN = orbital.rv2HN(r, v)
+        assert np.allclose(HN, np.eye(3))
+
+    def test_rv2omega(self):
+        r = np.array([1, 0, 0])
+        v = np.array([0, 1, 0])
+        omega = orbital.rv2omega(r, v)
+        assert np.allclose(omega, np.array([0, 0, 1]))