From 8c9b53dc2d738291e3ee8e12bd8f29634b164909 Mon Sep 17 00:00:00 2001 From: westyvi <79417323+westyvi@users.noreply.github.com> Date: Mon, 29 Apr 2024 00:50:10 -0500 Subject: [PATCH] add seeker measurement model, multiple pronav algorithm implementations, and mostly functioning 1 on 1 interception E2E test simulation --- src/gncpy/control/__init__.py | 1 + src/gncpy/control/pronav.py | 136 ++++++++++++++++++ src/gncpy/measurements/__init__.py | 3 +- src/gncpy/measurements/seeker.py | 43 ++++++ test/unit/test_reentry_vehicle.py | 98 +++++++++++++ test/validation/test_icbm_interception.py | 160 ++++++++++++++++++++++ 6 files changed, 440 insertions(+), 1 deletion(-) create mode 100644 src/gncpy/control/pronav.py create mode 100644 src/gncpy/measurements/seeker.py create mode 100644 test/validation/test_icbm_interception.py diff --git a/src/gncpy/control/__init__.py b/src/gncpy/control/__init__.py index b3eef469..048249e0 100644 --- a/src/gncpy/control/__init__.py +++ b/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 diff --git a/src/gncpy/control/pronav.py b/src/gncpy/control/pronav.py new file mode 100644 index 00000000..dcca5ffb --- /dev/null +++ b/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 + + + + + + + + + + + diff --git a/src/gncpy/measurements/__init__.py b/src/gncpy/measurements/__init__.py index ac9b64ea..28bf452d 100644 --- a/src/gncpy/measurements/__init__.py +++ b/src/gncpy/measurements/__init__.py @@ -1,4 +1,5 @@ from gncpy.measurements._measurements import ( RangeAndBearing, RangeAndBearingParams, StateObservation, StateObservationParams -) \ No newline at end of file +) +from .seeker import Seeker diff --git a/src/gncpy/measurements/seeker.py b/src/gncpy/measurements/seeker.py new file mode 100644 index 00000000..d4132d8d --- /dev/null +++ b/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 + diff --git a/test/unit/test_reentry_vehicle.py b/test/unit/test_reentry_vehicle.py index 5b79a510..17d7fcda 100644 --- a/test/unit/test_reentry_vehicle.py +++ b/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() diff --git a/test/validation/test_icbm_interception.py b/test/validation/test_icbm_interception.py new file mode 100644 index 00000000..b0c87e82 --- /dev/null +++ b/test/validation/test_icbm_interception.py @@ -0,0 +1,160 @@ +import pytest +import numpy as np +import numpy.testing as test + +import gncpy.dynamics.basic as gdyn +import gncpy.control as gctrl +import gncpy.measurements as sensors + +# FIXME eventually want this to be an actual E2E test that asserts the interceptor hit the icbm + +def test_icbm_interception(): + dt = 0.1 + t1 = 70 + time = np.arange(0, t1 + dt, dt) + + # Create dynamics object + target = gdyn.ReentryVehicle(dt=dt) + chaser = gdyn.ReentryVehicle(dt=dt) + + # initialize icbm as coming from NE towards origin, just entering atmosphere + # parameters based on "Data for ICBM entries" + t_state = np.zeros((time.size, len(target.state_names))) + reentry_speed = 7500 # 24300 ft/s + reentry_angle = np.deg2rad(10) + reentry_heading = np.deg2rad(45+180) # assume 45 degree heading angle towards origin + reentry_velocity = np.array([reentry_speed*np.cos(reentry_heading)*np.cos(reentry_angle), + reentry_speed*np.sin(reentry_heading)*np.cos(reentry_angle), + -reentry_speed*np.sin(reentry_angle) + ]) + t_state[0] = np.concatenate(([300000, 300000, 100000], reentry_velocity)) + + # init interceptor ('chaser') vehicle at origin with marginal launch velocity in direction + # of icbm at 30 degree up angle + c_state = np.zeros((time.size, len(chaser.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) + ]) + c_state[0] = np.concatenate(([0, 0, 0], launch_velocity)) + chaser_specific_thrust = 200 # m/s (aka about 20g's acceleration at launch) + + R = np.zeros((2,2)) + seeker = sensors.Seeker(R) + pronav = gctrl.Pronav() + hit_distance_threshold = 10 # meters + + # simulate with no icbm evasion control and pronav chaser control + # this simulation has no sensor noise (perfect knowledge of states) + for kk, tt in enumerate(time[:-1]): + # compute control command for interceptor + relative_state = seeker.calc_relative_state(c_state[kk,:], t_state[kk,:]) + t_go = pronav.estimate_tgo(relative_state[:3], relative_state[3:]) + u_ENU = pronav.PN(relative_state[:3], relative_state[3:], t_go) # 3D pronav control + u_ENU[2] = u_ENU[2] + 9.81 # account (roughly/flat earth assumption) for gravity in control vector; akin to setting u nominal + u_VTC = pronav.enu2vtc(c_state[kk, :], u_ENU) # convert pronav control in ENU frame to VTC frame + u_VTC[0] = u_VTC[0] + 100 # add specific thrust to pronav control FIXME this assumes thrust is variable as commanded by pronav + + # propogate interceptor and target forward + c_state[kk + 1, :] = chaser.propagate_state(tt, c_state[kk].reshape((-1, 1)), u=u_VTC).flatten() + t_state[kk + 1, :] = chaser.propagate_state(tt, t_state[kk].reshape((-1, 1)), u=np.zeros(3)).flatten() + + if np.linalg.norm(c_state[kk + 1, :3] - t_state[kk+1, :3]) <= hit_distance_threshold: + # FIXME this doesn't trip, most likely because the simulation isn't outputting the discretely + # simulated states at the exact time they intercept (the 0.1 second dt is too large a window) + # Fix with interpolation? + print('interception! Touchdown Alabama') + + + # 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, t_state[:, 0], label='target') + ax1.plot(time, c_state[:, 0], label='interceptor') + ax1.set_ylabel("E-pos (m)") + ax1.grid(True) + + ax2.plot(time, t_state[:, 1], label='target') + ax2.plot(time, c_state[:, 1], label='interceptor') + ax2.set_ylabel("N-pos (m)") + ax2.grid(True) + + ax3.plot(time, t_state[:, 2], label='target') + ax3.plot(time, c_state[:, 2], label='interceptor') + ax3.set_ylabel("U-pos (m)") + ax3.set_xlabel("time (s)") + ax3.grid(True) + + fig.suptitle("Uncontrolled Reentry vehicle vs. 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, t_state[:, 3], label='target') + fig.axes[0].plot(time, c_state[:, 3], label='interceptor') + fig.axes[0].set_ylabel("E-vel (m/s)") + fig.axes[0].grid(True) + + fig.axes[1].plot(time, t_state[:, 4], label='target') + fig.axes[1].plot(time, c_state[:, 4], label='interceptor') + fig.axes[1].set_ylabel("N-vel (m/s)") + fig.axes[1].grid(True) + + fig.axes[2].plot(time, t_state[:, 5], label='target') + fig.axes[2].plot(time, c_state[:, 5], label='interceptor') + fig.axes[2].set_ylabel("U-vel (m/s)") + fig.axes[2].set_xlabel("time (s)") + fig.axes[2].grid(True) + + fig.suptitle("Uncontrolled Reentry vehicle vs. 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(t_state[:,0], t_state[:, 1], label='target') + fig.axes[0].plot(c_state[:,0], c_state[:, 1], label='interceptor') + 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(t_state[:,0], t_state[:, 2], label='target') + fig.axes[1].plot(c_state[:,0], c_state[:, 2], label='interceptor') + 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("Uncontrolled Reentry vehicle vs. Interceptor simulation") + + +if __name__ == "__main__": + DEBUG = True + if DEBUG: + import matplotlib.pyplot as plt + import matplotlib + + matplotlib.use("WebAgg") + + plt.close("all") + + test_icbm_interception() + + if DEBUG: + plt.show() + \ No newline at end of file