fix reentry vehicle control model bug where lift input wasnt limited

src/gncpy/dynamics/basic/reentry_vehicle.py

@@ -85,25 +85,37 @@ def g2(t, x, u, *args):
 
         def ENU_control_acceleration(t, x, u, *args):
             # assumes u is np.array([a_v, a_t, a_c])
+
+            # define VTC -> ENU rotation matrix
             v = np.linalg.norm(x[3:])
             vg = np.linalg.norm(x[3:5])
             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)]])
-            # calculate additional induced drag
-            total_lift_acceleration = np.linalg.norm(u[1:])
+
+            # calculate dynamic pressure 
             rho_NED = np.array([x[1], x[0], -(x[2] + wgs84.EQ_RAD)]) # spherical approximation
             veh_alt = np.linalg.norm(rho_NED) - wgs84.EQ_RAD
             coesa76_geom = atm.coesa76([veh_alt / 1000])  # geometric altitudes in km
             density = coesa76_geom.rho  # [kg/m^3]
             q = 1/2*density*v**2
-            # following equation derived from aero force equations and the CDL=CL**2/4 result from slender body potential flow theory (cite Cronvich)
-            induced_drag_acceleration = total_lift_acceleration**2*self.ballistic_coefficient*self.CD0/(4*q)
-            u_modified = copy.deepcopy(u)
-            u_modified[0] = u_modified[0] - induced_drag_acceleration # subtract induced drag from thrust
+
+            # limit maximum lift, assume CLMax = 3 
+            # (set high to poorly account for fact that thrust can be unaligned with velocity to help turn)
+            u_limited = copy.deepcopy(u).astype(float)
+            total_lift_acceleration = np.linalg.norm(u[1:])
+            lift_accel_max = 3*q*self.drag_parameter/self.CD0
+            if total_lift_acceleration > lift_accel_max:
+                u_limited[1:] = u_limited[1:]*lift_accel_max/total_lift_acceleration
+                total_lift_acceleration = np.linalg.norm(u_limited[1:])
+
+            # calculate additional induced drag
+            # following equation derived from CDL=CL**2/4 result from slender body potential flow theory (cite Cronvich Missile Aerodynamics)
+            induced_drag_acceleration = total_lift_acceleration**2/self.drag_parameter*self.CD0/(4*q)
+            u_limited[0] = u_limited[0] - induced_drag_acceleration # subtract induced drag from thrust
 
             # convert VTC accelerations to ENU
-            a_control_ENU = T_ENU_VTC @ u_modified
+            a_control_ENU = T_ENU_VTC @ u_limited
 
             return a_control_ENU
 

test/unit/test_reentry_vehicle.py

@@ -9,13 +9,20 @@
 
 def test_rv_prop():
     dt = 0.1
-    t1 = 10
+    t1 = 100
     time = np.arange(0, t1 + dt, dt)
 
     # Create dynamics object
     dynObj = gdyn.ReentryVehicle(dt=dt)
     state = np.zeros((time.size, len(dynObj.state_names)))
-    state[0] = np.array([10000, 10000, 10000, -1000, -1000, -100])
+    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)
+                                 ])
+    state[0] = np.concatenate(([100000, 100000, 100000], reentry_velocity))
 
     for kk, tt in enumerate(time[:-1]):
         state[kk + 1, :] = dynObj.propagate_state(tt, state[kk].reshape((-1, 1))).flatten()
@@ -85,18 +92,25 @@ def test_rv_prop():
 
 def test_rv_control():
     dt = 0.1
-    t1 = 10
+    t1 = 100
     time = np.arange(0, t1 + dt, dt)
 
     # Create dynamics object
     dynObj = gdyn.ReentryVehicle(dt=dt)
 
     # Control model already set up for RV: input constant 50 m/s2 right hand turn
-    u = np.array([0,-100,0]) # FIXME integration fails for high values; works for 10
+    u = np.array([0,-100,0]) 
 
     # simulate for some time
     state = np.zeros((time.size, len(dynObj.state_names)))
-    state[0] = np.array([10000, 10000, 10000, -1000, -1000, -100])
+    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)
+                                 ])
+    state[0] = np.concatenate(([100000, 100000, 100000], reentry_velocity))
 
     for kk, tt in enumerate(time[:-1]):
         state[kk + 1, :] = dynObj.propagate_state(
@@ -143,7 +157,7 @@ def test_rv_control():
         fig.axes[1].grid(True)
 
         fig.axes[2].plot(time, state[:, 5])
-        fig.axes[2].set_ylabel("N-vel (m/s)")
+        fig.axes[2].set_ylabel("U-vel (m/s)")
         fig.axes[2].set_xlabel("time (s)")
         fig.axes[2].grid(True)
 
@@ -179,8 +193,8 @@ def test_rv_control():
 
         plt.close("all")
 
-    #test_rv_prop()
-    test_rv_control()
+    test_rv_prop()
+    #test_rv_control()
 
     if DEBUG:
         plt.show()