add seeker measurement model, multiple pronav algorithm implementatio…

…ns, and mostly functioning 1 on 1 interception E2E test simulation

src/gncpy/control/__init__.py

@@ -5,3 +5,4 @@
 from gncpy.control._control import StateControl, StateControlParams
 from .lqr import LQR
 from .elqr import ELQR
+from .pronav import Pronav

src/gncpy/control/pronav.py

@@ -0,0 +1,136 @@
+import numpy as np
+import scipy.linalg as la
+
+# FIXME need to add in a way to output cost to go's to these algorithms
+
+class Pronav:
+    r"""Implements a proportional navigation (Pronav) controller.
+
+    Notes
+    -----
+    Pronav is a simplified case of ELQR for the target interception problem. 
+    This class implements multiple types of pronav:
+        PN: classic pronav:         assumes constant target velocity
+        APN: augmented pronav:      assumes constant target acceleration
+        EPN: extended pronav:       assumes constant target jerk 
+        OGL: optimal guidance law:  assumes constant target acceleration and an acceleration command lag
+    
+    Formulation of the problem and guidance laws are taken from "modern homing missile guidance theory and techniques" from Johns Hopkins APL
+    """
+
+    def __init__(self, maneuver_time_constant=0.0):
+        """Initialize an object.
+
+        Parameters
+        ----------
+        maneuver_time_constant: float, optional
+            Defines the time constant a_actual(s) / a_commanded(s)  = 1/(Ts+1),
+            Where T is the maneuver time constant. This is only used in the OGL method. 
+        """
+        super().__init__() # FIXME do I need this?
+        self._T = maneuver_time_constant
+
+
+    @property
+    def T(self):
+        """Read maneuver time constant"""
+        return self._T
+
+    @T.setter
+    def T(self, val):
+        self._T = val
+
+    def estimate_tgo(self, x_relative, v_relative):
+        # where x_rel = x_target - x_ego, same for v_rel
+        return -np.dot(x_relative, v_relative)/np.dot(v_relative, v_relative)
+
+    def PN(self, x_relative, v_relative, t_go):
+        r'''
+        Implements pure proportional navigation. Assumes target velocity is constant and there is no 
+        lag in ego vehicle acceleration commands
+
+        Outputs commanded accelerations in the same coordinate frame that x_relative and v_relative 
+        were input. 
+
+        Note: this outputs a 3D control command. Limiting this control to axis perpendicular to vehicle
+        velocity (so this doesn't command a change in thrust) must happen outside this function.
+        '''
+
+        u = 3/t_go**2 * (x_relative + v_relative * t_go)
+        return u
+
+    def APN(self, x_relative, v_relative, a_target, t_go):
+        r'''
+        Implements augmented proportional navigation. Assumes target acceleration is constant and there is no 
+        lag in ego vehicle acceleration commands
+
+        Outputs commanded accelerations in the same coordinate frame that x_relative and v_relative 
+        were input. 
+
+        Note: this outputs a 3D control command. Limiting this control to axis perpendicular to vehicle
+        velocity (so this doesn't command a change in thrust) must happen outside this function.
+        '''
+
+        u = 3/t_go**2 * (x_relative + v_relative * t_go + 1/2*a_target*t_go**2)
+        return u
+
+    def EPN(self, x_relative, v_relative, a_target, j_target, t_go):
+        r'''
+        Implements extended proportional navigation. Assumes target jerk is constant and there is no 
+        lag in ego vehicle acceleration commands
+
+        Outputs commanded accelerations in the same coordinate frame that x_relative and v_relative 
+        were input. 
+
+        Note: this outputs a 3D control command. Limiting this control to axis perpendicular to vehicle
+        velocity (so this doesn't command a change in thrust) must happen outside this function.
+        '''
+
+        u = 3/t_go**2 * (x_relative + v_relative * t_go + 1/2*a_target*t_go**2 + 1/6*j_target*t_go**3)
+        return u
+
+    def OGL(self, x_relative, v_relative, a_target, a_ego, t_go):
+        r'''
+        Implements the optimal guidance law for the interception problem. 
+        Assumes target acceleration is constant and incorporates lag in commanded ego vehicle accelerations
+
+        Outputs commanded accelerations in the same coordinate frame that x_relative and v_relative 
+        were input. 
+
+        Note: this outputs a 3D control command. Limiting this control to axis perpendicular to vehicle
+        velocity (so this doesn't command a change in thrust) must happen outside this function.
+        '''
+
+        # split u into components to improve code readability:
+        u = 6*(t_go/self.T)**2/t_go**2
+        lag_term = (t_go/self.T + np.exp(-t_go/self.T) - 1)
+        u *= lag_term
+        u *= (x_relative + v_relative*t_go + 1/2*t_go**2*a_target - self.T**2*(lag_term)*a_ego)
+        u /= (3 + 6*t_go/self.T - 6*(t_go/self.T)**2 + 2*(t_go/self.T)**3 - 12*(t_go/self.T)*np.exp(-t_go/self.T) - 3*np.exp(-2*t_go/self.T))
+        return u
+
+    def enu2vtc(self, x, u_ENU):
+        # converts a control vector u in the enu coordinate system to the vtc coordinate system
+        v = np.linalg.norm(x[3:])
+        vg = np.linalg.norm(x[3:5])
+
+        if v == 0 or vg == 0:
+            print(f'v = {v}, vg = {vg}')
+            raise ValueError("Velocity magnitude or ground speed magnitude is zero.")
+
+        T_ENU_VTC = np.array([[x[3]/v, -x[4]/vg, -x[3]*x[5]/(v*vg)],
+                                  [x[4]/v, x[3]/vg, -x[4]*x[5]/(v*vg)],
+                                  [x[5]/v, 0, vg**2/(v*vg)]])
+        u_VTC = np.linalg.inv(T_ENU_VTC) @ u_ENU
+        return u_VTC
+
+
+
+
+
+
+
+
+
+
+

src/gncpy/measurements/__init__.py

@@ -1,4 +1,5 @@
 from gncpy.measurements._measurements import (
     RangeAndBearing, RangeAndBearingParams,
     StateObservation, StateObservationParams
-)
+)
+from .seeker import Seeker

src/gncpy/measurements/seeker.py

@@ -0,0 +1,43 @@
+import numpy as np
+
+
+class Seeker():
+    r"""Implements a model of a seeker sensor that tracks objects
+    
+    Notes
+    -----
+    todo: implement sensor models with noise
+    """
+
+    def __init__(self, sensor_noise=np.array([0])):
+        """Initialize an object.
+
+        Parameters
+        ----------
+        sensor_noise: float, optional
+            defines the sensor noise covariance matrix for the seeker. 
+            If the input is 2D (x,y -> range, bearing), R is a 2x2 matrix
+            If input is 3D (x,y,z -> range, bearing, azimuth), R is 3x3
+        """
+        self._R = sensor_noise
+
+
+    @property
+    def R(self):
+        """Read sensor noise matrix"""
+        return self._R
+
+    @R.setter
+    def R(self, val):
+        self._R = val
+
+    def calc_relative_state(self, x_ego, x_target):
+        r'''
+        Not a 'sensor model' per-se as this isn't meant to be incorporated into a filter that observes
+        x,y,z from a sensor's range, bearing, azimuth. Meant to be used if incorporating a simulation 
+        where sensor noise/models/filters are not simulated. 
+
+        Returns the relative state of the target from the ego vehicle
+        '''
+        return x_target - x_ego
+

test/unit/test_reentry_vehicle.py

@@ -5,6 +5,8 @@
 import gncpy.dynamics.basic as gdyn
 import gncpy.control as gcont
 
+# FIXME want to put some asserts in here to actually function as a test, not just a debug/plotting function
+
 DEBUG = False
 
 def test_rv_prop():
@@ -182,6 +184,101 @@ def test_rv_control():
 
         fig.suptitle("Reentry vehicle simulation with RH 10m/s2 turn")
 
+# function to test that the Reentry vehicle class works for simulating a launched missile to intercept as well
+
+def test_interceptor_control():
+    dt = 0.1
+    t1 = 50
+    time = np.arange(0, t1 + dt, dt)
+
+    # Create dynamics object
+    dynObj = gdyn.ReentryVehicle(dt=dt)
+
+    # Control model already set up for interceptor: 
+    # Constant specific thrust of 100 m/s2 and constant 30 m/s2 right hand turn
+    # need 10 m/s climb command to counteract gravity
+    u = np.array([100,0,9.8]) 
+
+    # simulate for some time
+    state = np.zeros((time.size, len(dynObj.state_names)))
+    launch_speed = 10 # m/s
+    launch_angle = np.deg2rad(30)
+    launch_heading = np.deg2rad(45) # assume 45 degree heading angle away from origin
+    launch_velocity = np.array([launch_speed*np.cos(launch_heading)*np.cos(launch_angle), 
+                                 launch_speed*np.sin(launch_heading)*np.cos(launch_angle),
+                                 launch_speed*np.sin(launch_angle)
+                                 ])
+    state[0] = np.concatenate(([0, 0, 0], launch_velocity))
+
+    for kk, tt in enumerate(time[:-1]):
+        state[kk + 1, :] = dynObj.propagate_state(
+            tt, state[kk].reshape((-1, 1)), u=u
+        ).flatten()
+
+    # debug plots
+    if DEBUG:
+        # --------------------- position-time plots ----------------------
+        # Create subplots
+        fig = plt.figure()
+        ax1 = fig.add_subplot(3, 1, 1)
+        ax2 = fig.add_subplot(3, 1, 2)
+        ax3 = fig.add_subplot(3, 1, 3)
+
+        # Plot on each subplot
+        ax1.plot(time, state[:, 0])
+        ax1.set_ylabel("E-pos (m)")
+        ax1.grid(True)
+
+        ax2.plot(time, state[:, 1])
+        ax2.set_ylabel("N-pos (m)")
+        ax2.grid(True)
+
+        ax3.plot(time, state[:, 2])
+        ax3.set_ylabel("U-pos (m)")
+        ax3.set_xlabel("time (s)")
+        ax3.grid(True)
+
+        fig.suptitle("Interceptor simulation")
+
+        # --------------------- velocity-time plots ----------------------
+        fig = plt.figure()
+        fig.add_subplot(3, 1, 1)
+        fig.add_subplot(3, 1, 2)
+        fig.add_subplot(3, 1, 3)
+
+        fig.axes[0].plot(time, state[:, 3])
+        fig.axes[0].set_ylabel("E-vel (m/s)")
+        fig.axes[0].grid(True)
+
+        fig.axes[1].plot(time, state[:, 4])
+        fig.axes[1].set_ylabel("N-vel (m/s)")
+        fig.axes[1].grid(True)
+
+        fig.axes[2].plot(time, state[:, 5])
+        fig.axes[2].set_ylabel("U-vel (m/s)")
+        fig.axes[2].set_xlabel("time (s)")
+        fig.axes[2].grid(True)
+
+        fig.suptitle("Interceptor simulation")
+
+        # --------------------- x-y, y-z plots ----------------------
+        fig = plt.figure()
+        fig.add_subplot(3, 1, 1)
+        fig.add_subplot(3, 1, 2)
+
+        fig.axes[0].plot(state[:,0], state[:, 1])
+        fig.axes[0].set_ylabel("North position (m)")
+        fig.axes[0].grid(True)
+        #fig.axes[0].set_aspect('equal', adjustable='box')
+
+        fig.axes[1].plot(state[:,0], state[:, 2])
+        fig.axes[1].set_ylabel("Up position (m)")
+        fig.axes[1].grid(True)
+        fig.axes[1].set_xlabel("East position (m)")
+        #fig.axes[1].set_aspect('equal', adjustable='box')
+
+
+        fig.suptitle("Interceptor simulation")
 
 if __name__ == "__main__":
     DEBUG = True
@@ -195,6 +292,7 @@ def test_rv_control():
 
     test_rv_prop()
     test_rv_control()
+    test_interceptor_control()
 
     if DEBUG:
         plt.show()