In [29]:

    

import matplotlib.pyplot as plt
import numpy as np
import seaborn 

plt.style.use('seaborn-whitegrid')

# from Utils.gpops import srp as srp_opt
from Utils.RK4 import RK4, Euler
from EntryGuidance.TrajPlot import TrajPlot


    

In [265]:

    

g0 = 1.62 * 1000/4000
mdry = 1020
m = np.linspace(1050, mdry, 500)

# g1 = 1.1 * 1000/1050
g1 = 1.1 * 1000/m
g2 = 1.1 * 1000/mdry
# g2 = 1.1 * 1000/950
# G = 0.5*(g1+g2)
G = g2
alpha_min = (g0+g2-g1)/G
plt.plot(m, alpha_min)
plt.xlabel("Current SC Mass (kg)")
plt.ylabel("Min Admissible \alpha")

np.polyfit(m/4, alpha_min, 1)


    

    
Out[265]:





array([ 0.00380921, -0.59566304])






    

In [204]:

    

rho = 1
alpha = 0.42
eps_max = 4*(1-alpha)/alpha - rho
eps_min = 4*alpha/(1-alpha) - rho
print((eps_min,eps_max)) #2.7-6 for rho=0, alpha=0.4


    

    




(1.8965517241379306, 4.523809523809525)

In [172]:

    

mu/R**2


    

    
Out[172]:





1.6178669184452057

In [274]:

    

# reference "Quasi Time Fuel Optimal Feedback Control of Perturbed Double Integrator"
# Recreating the example from the paper 

p = 40000.

R = 1.738e6
k = 5.5 
mu = 4.887e12


x0 = [4000, -15, 1050] # pos vel mass 

def dyn(x, t,):
    M = x[2]
    c = 1000/p
#     c = M/p
    u = controller_mfc(x*c,)
    dv = ( ((p+1*400*np.exp(-0.02*t))/M)*u - mu/(x[0] + R)**2 )
    
    dM = -k*np.abs(u)
    if np.abs(x[0]) + np.abs(x[1]) < 1:
        return 0,0,0
    else:
        return x[1], dv, dM
    

def controller_mfc(x, ):
    
    delta = 100 #small factor to check for zero
    def near_zero(z):
        return np.abs(x)[0] <= delta
        
    x1, x2, m = x
    
        
    rho = 0 # time weighting factor, 0 for fuel optimality 
    adaptive = False
    # for pure double integrator, F = 0, G1=G=G2=1
    # alpha < 0.5
    if adaptive:
#         coeff = np.array([ 0.00380573, -0.55245659]) # mdry = 950
        coeff = np.array([ 0.00380921, -0.59566304]) # mdry = 1020
        alpha = np.polyval(coeff, m)+ 0.05
        eps_max = 4*(1-alpha)/alpha - rho
        eps_min = 4*alpha/(1-alpha) - rho
        eps = min(eps_min + 0.1, eps_max)
#         print((alpha, eps))
    else:
        alpha = 0.2
#         eps = 1.9 
        eps_max = 4*(1-alpha)/alpha - rho
        eps_min = 4*alpha/(1-alpha) - rho
        eps = min(eps_min + 0.1, eps_max)
#         print(eps)

#     assert eps > eps_min and eps < eps_max, "Invalid choice of epsilon"
    
#     x1 -= 10/4. # Aims for 10 m above the ground 
    S1 = x1 + x2*np.abs(x2)*(4+rho+eps)/(2*(rho+eps))
    S2 = x1 + x2*np.abs(x2)*0.5/(1-alpha)
    S = S1*S2

    # Initialize the memory
    try:
        controller_mfc.u_prev 
    except:
        if not near_zero(S):
            u = -np.sign(np.sign(S1) + np.sign(S2))
        else:
            if np.abs(S1) < np.abs(S2):
                u = 0
            else:
                u = -np.sign(x2)
        controller_mfc.u_prev = u
#         print("Initialized u = {}".format(u))
        controller_mfc.S_history = [S]
        controller_mfc.S1_last = S1

        controller_mfc.u_history = [u]
        return u
   
    if S*controller_mfc.S_history[-1] < 0: # Have to update the memory for either branch here
        if S1*controller_mfc.S1_last < 0:
            u = 0
        else:
            u = -np.sign(x2)
        controller_mfc.u_prev = u
    else:
        if S < 0:
            u = controller_mfc.u_prev 

        else:
            u = -np.sign(np.sign(S1) + np.sign(S2))
    
    controller_mfc.S1_last = S1

    controller_mfc.S_history.append(S)
#     if np.max(np.abs(controller_mfc.u_history)) > 0:
#         u = np.clip(u, 0.3, 1)
    controller_mfc.u_history.append(u)    
    return u
    
def plotS(alpha=0.2, rho=0,eps=1.1):
    c = 1000/p
    x1 = np.linspace(0, 4200, 500)
    x2 = np.linspace(0, -450, 400)
    
    X1,X2 = np.meshgrid(x1*c, x2*c)
    
    S1 = X1 + (4+rho+eps)/(2*rho+2*eps)*X2*np.abs(X2)
    S2 = X1 + 1/(2-2*alpha)*X2*np.abs(X2)
    
    plt.contour(X1/c, X2/c, S1, 0, colors='k', linestyles='dashed')
    plt.contour(X1/c, X2/c, S2, 0, colors='b', linestyles='dashed')

    
tf = 100
t = np.linspace(0, tf, 2000)
x = Euler(dyn, x0, t, )

dx = np.linalg.norm(np.diff(x, axis=0), axis=1)

iterm = np.argmin(dx)

plotS()
plt.plot(x[:iterm,0], x[:iterm,1], 'r',linewidth=3)

plt.figure()
plt.plot(t[:iterm], controller_mfc.u_history[:iterm])

plt.figure()
plt.plot(t[:iterm], x[:iterm,2])

plt.figure()
plt.plot(t[:iterm], x[:iterm,:2])

print("Time of flight = {:.2f} s\nFinal altitude = {:.3f} m\nFinal velocity = {:.3f} m/s\nFuel consumed = {:.1f} kg".format(t[iterm],x[-1,0],x[-1,1],x0[2]-x[-1,2]))
J = np.sum(1+np.abs(controller_mfc.u_history[:iterm]))*t[1]
print("Objective = {:.1f}".format(J))


    

    




Time of flight = 24.46 s
Final altitude = -0.335 m
Final velocity = 0.615 m/s
Fuel consumed = 68.2 kg
Objective = 36.9






    













    













    













    

In [14]:

    

# reference "A Family of Global Stabilizers for Quasi-Optimal Control of Planar Linear Saturated Systems"
def dyn(x, t, controller):
    return x[1], controller(x)


# def controller_fuel(x,): # Quasi-fuel optimal control

def controller_time(x, k): # quasi time optimal
    u = -k*(x[0] + (x[1])*np.max([np.abs(x[1])/(2*k),2/np.sqrt(k)*np.ones_like(x[1])], axis=0))
    return np.clip(u, -1, 1)


    

In [21]:

    

N = 100
x0 = -1 + 2*np.random.random((2, N))
tf = 10
t = np.linspace(0, tf, 100)
k = 1
x = RK4(dyn, x0, t, args=(lambda s: controller_time(s,k),))
u = [controller_time(xi, k) for xi in x]
print(np.max(x[-1]))

plt.plot(x[:,0], x[:,1], 'k--')
plt.figure()
plt.plot(t, x[:,0])
plt.figure()
plt.plot(t, u)
plt.show()


    

    




0.0016515530318465309






    













    













    

In [13]:

    

# Finite time stabilization of double integrator 
def dyn(x, t, alpha):
    return x[1], controller(x, alpha)
    
def signed_fractional_exponent(x, alpha):
    return np.sign(x)*np.abs(x)**alpha

def controller(x, alpha):
    return -signed_fractional_exponent(x[1], alpha) - signed_fractional_exponent(x[0], alpha/(2-alpha))

X = np.random.multivariate_normal([0,0], np.diag([1,1]), 1000)

u = [controller(x, 0.25) for x in X]

plt.scatter(X.T[0], X.T[1], c=u)
plt.colorbar()
plt.show()


    

In [12]:

    

N = 1000
x0 = -1 + 2*np.random.random((2, N))
tf = 5
t = np.linspace(0, tf, 100)
alpha = 0.25
x = RK4(dyn, x0, t, args=(alpha,))
u = [controller(xi, alpha) for xi in x]
print(np.max(x[-1]))

plt.plot(x[:,0], x[:,1], 'k--')
# plt.figure()
# plt.plot(t, x)
plt.figure()
plt.plot(t, u)
plt.show()


    

    




0.04460718180532759






    













    

In [3]:

    

x0 = [-3200., 0., 3200., 625., 60., -270., 8500.]
traj = srp_opt((x0,))
# print((traj['time'][-1]))
u = np.array(traj['control']).T
plt.figure(1, figsize=(14,8))
TrajPlot(*(np.array(traj['state'])[:,0:3].T), T=(u[0]*u[1:4]).T/1000 , show=True, figNum=1)


    

In [6]:

    

from scipy.interpolate import interp1d 

class SRPController:
    """ Manages the reference trajectory, gains, and differential states
    required to implement a nonlinear controller that is robust to uncertainty 
    """
    def __init__(self, reference, gains):
        u = np.array(reference['control']).T # Magnitude and unit vector representation         
        T = (u[0]*u[1:4]).T
        
        x = np.array(reference['state'])
        t = np.array(reference['time']).squeeze()
        
        self.reference_state = interp1d(t, x, axis=0, kind='cubic')
        self.reference_control = interp1d(t, T, axis=0, kind='cubic')
        self.gains = gains 
        
        self.gain_state = None
        self.gain_rate = None 
        self.gain_time = 0. 
        return 
        
        
    def controller(self, time, state, thrust_bounds):
        
        c, k, alpha, beta = self.gains # all gains could be a triple (per axis) but for now we'll assume scalars 
        
        ref_state = self.reference_state(time)
        ref_control = self.reference_control(time)
        if state.ndim>1:
            ref_state = ref_state[:,None]
            ref_control = ref_control[:,None]
            
        ds = ref_state - state
        e1 = ds[0:3]
        e1_dot = ds[3:6]
        e = e1 + c*e1_dot 
#         K = np.diag(k)
        
        if self.gain_state is None:
            self.gain_state = np.zeros_like(e)
            self.gain_rate = k*alpha*e + beta*np.sign(e)
            self.e0 = e 
        
        self.gain_rate = k*alpha*e + beta*np.sign(e) # This is the current gain_rate. 
#         self.gain_rate = 0.5*(self.gain_rate + k*alpha*e + beta*np.sign(e))  #We could also average it with the previous gain rate 
        self.gain_state += self.gain_rate * (time-self.gain_time)
        self.gain_time = time
    
        T = ref_control + k*(e-0*self.e0) + self.gain_state 
        
        # Clip the total magnitude to be within the bounds 
        Tmag = np.linalg.norm(T, axis=0)
        Tunit = T/Tmag 
        T = np.clip(Tmag, *thrust_bounds) * Tunit 
        return T

def dynamics( x, t, u, vei):
        
        p = x[0:3]
        v = x[3:6] 
        m = x[6]
        
        g = np.array([0, 0, 3.71])
        if x.ndim>1:
            g = g[:,None]
            
        dx = np.empty(x.shape, dtype=x.dtype)     
        dx[0:3] = v
        dx[3:6] = u/m - g
        dx[6] = -vei * (u[0]**2 + u[1]**2 + u[2]**2)**0.5   
        return dx
    
def simulation(x0, tf, controller, param=(1./(9.81*290), 40*8500., 70*8500.), cycle=1):
    vei, Tmin, Tmax = param # Inverse escape velocity, min thrust, max thrust 

#     cycle = 1 # seconds 
    tf_int = int(tf)
    t = np.linspace(0, tf_int, tf_int/cycle)
    t = np.append(t, tf)
    dt = np.diff(t)
    
    X = [x0]
    U = []
    for time, next_time in zip(t, t[1:]):
        u = controller(time, X[-1], (Tmin, Tmax))
        U.append(u)
        
        x = RK4(dynamics, X[-1], np.linspace(time, next_time, 10), args=(u, vei))
        X.append(x[-1])
        
    U.append(U[-1]) # So its the same length 
#     controller.gain_state = None # essentially resets the controller, allowing it to be used for another simulation without recreating 
    return t, np.array(X), np.array(U)
    
    
def plot(t, x, u, label=""):

        T = np.linalg.norm(u, axis=1)
    
        label = label
        
        plt.figure(1)
        plt.plot(t, x.T[-1], '--', label=label)
        plt.xlabel('Time (s)')
        plt.ylabel('Mass (kg)')
        plt.legend()
        
        plt.figure(2)
        plt.plot(t, x.T[0:3].T, '--', label=label)
        plt.xlabel('Time (s)')
        plt.ylabel('Position (m)')
        plt.legend()
        
        plt.figure(3)
        plt.plot(t, x.T[3:6].T, '--', label=label)
        plt.xlabel('Time (s)')
        plt.ylabel('Velocity (m/s)')
        plt.legend()
        
        plt.figure(4)
        plt.plot(t, T, '--', label=label)
        plt.xlabel('Time (s)')
        plt.ylabel('Thrust (N)')
        plt.legend()
        
#         plt.figure(5)
#         plt.plot(t, u, '--', label=label)
#         plt.xlabel('Time (s)')
#         plt.ylabel('Control Directions')
#         plt.legend()
        
        return


    

In [25]:

    

import chaospy as cp
x0 = [-3200., 0., 3200., 625., 0., -270., 8500.]
ux = cp.Uniform(x0[0]-400,x0[0]+400)
uy = cp.Uniform(x0[1]-1000,x0[1]+1000)
uz = cp.Uniform(x0[2]-600,x0[2]+800)
uu = cp.Uniform(x0[3]-40,x0[3]+100)
uv = cp.Uniform(x0[4]-100,x0[4]+100)
uw = cp.Uniform(x0[5]-40,x0[5]+40)
D = cp.J(ux,uy,uz,uu,uv,uw)

R = D.sample(1000, 'S')
print(R.shape)
X0 = np.append(R, x0[-1]*np.ones_like(R[:1]),axis=0)


tf = float(np.array(traj['time'])[-1])

print(X0.shape)
C = SRPController(traj, gains=[7.65193911e+00, 9.47145179e+03, 5.01265421e-01, 7.50637837e+03])

t,x,u = simulation(X0, tf, C.controller, cycle=0.1)

xf = x[-1]
pos_err = (np.linalg.norm(xf[0:3], axis=0))
vel_err = (np.linalg.norm(xf[3:6], axis=0))

print(t.shape)
print(x.shape)
print(u.shape)


    

    




(6, 1001)
(7, 1001)






    




E:\Anaconda3\lib\site-packages\ipykernel\__main__.py:79: DeprecationWarning: object of type <class 'float'> cannot be safely interpreted as an integer.






    




(131,)
(131, 7, 1001)
(131, 3, 1001)

In [26]:

    

plt.figure()

plt.figure(figsize=(10,6))
seaborn.distplot(pos_err, kde=False, hist_kws=dict(edgecolor="k", linewidth=2))
plt.xlabel('Final Miss Distance (m)',fontsize=26)
plt.ylabel('Frequency', fontsize=26)
plt.xticks(fontsize=20)
plt.yticks(fontsize=20)

plt.figure(figsize=(10,6))
seaborn.distplot(vel_err, kde=False, hist_kws=dict(edgecolor="k", linewidth=2))
plt.xlabel('Final Velocity Error (m/s)',fontsize=26)
plt.ylabel('Frequency', fontsize=26)
plt.xticks(fontsize=20)
plt.yticks(fontsize=20)


    

    
Out[26]:





(array([  0.,  50., 100., 150., 200., 250., 300., 350., 400., 450.]),
 <a list of 10 Text yticklabel objects>)






    






<matplotlib.figure.Figure at 0x2a4fc2e6c18>






    













    

In [63]:

    

plot(t, x, u, label="Feedback")
plot(t, C.reference_state(t), C.reference_control(t), label="Ref")


    

    




---------------------------------------------------------------------------
ValueError                                Traceback (most recent call last)
<ipython-input-63-4c2eb4f85c84> in <module>()
----> 1 plot(t, x, u, label="Feedback")
      2 plot(t, C.reference_state(t), C.reference_control(t), label="Ref")

<ipython-input-58-105a830aa0ea> in plot(t, x, u, label)
    102 
    103         plt.figure(1)
--> 104         plt.plot(t, x.T[-1], '--', label=label)
    105         plt.xlabel('Time (s)')
    106         plt.ylabel('Mass (kg)')

E:\Anaconda3\lib\site-packages\matplotlib\pyplot.py in plot(*args, **kwargs)
   3259                       mplDeprecation)
   3260     try:
-> 3261         ret = ax.plot(*args, **kwargs)
   3262     finally:
   3263         ax._hold = washold

E:\Anaconda3\lib\site-packages\matplotlib\__init__.py in inner(ax, *args, **kwargs)
   1715                     warnings.warn(msg % (label_namer, func.__name__),
   1716                                   RuntimeWarning, stacklevel=2)
-> 1717             return func(ax, *args, **kwargs)
   1718         pre_doc = inner.__doc__
   1719         if pre_doc is None:

E:\Anaconda3\lib\site-packages\matplotlib\axes\_axes.py in plot(self, *args, **kwargs)
   1370         kwargs = cbook.normalize_kwargs(kwargs, _alias_map)
   1371 
-> 1372         for line in self._get_lines(*args, **kwargs):
   1373             self.add_line(line)
   1374             lines.append(line)

E:\Anaconda3\lib\site-packages\matplotlib\axes\_base.py in _grab_next_args(self, *args, **kwargs)
    402                 this += args[0],
    403                 args = args[1:]
--> 404             for seg in self._plot_args(this, kwargs):
    405                 yield seg
    406 

E:\Anaconda3\lib\site-packages\matplotlib\axes\_base.py in _plot_args(self, tup, kwargs)
    382             x, y = index_of(tup[-1])
    383 
--> 384         x, y = self._xy_from_xy(x, y)
    385 
    386         if self.command == 'plot':

E:\Anaconda3\lib\site-packages\matplotlib\axes\_base.py in _xy_from_xy(self, x, y)
    241         if x.shape[0] != y.shape[0]:
    242             raise ValueError("x and y must have same first dimension, but "
--> 243                              "have shapes {} and {}".format(x.shape, y.shape))
    244         if x.ndim > 2 or y.ndim > 2:
    245             raise ValueError("x and y can be no greater than 2-D, but have "

ValueError: x and y must have same first dimension, but have shapes (131,) and (7, 131)





    

In [17]:

    

dx = np.abs(x - C.reference_state(t))
du = np.abs(u - C.reference_control(t))
plot(t, dx, du, label="Error State")


    

In [4]:

    

from scipy.optimize import differential_evolution


    

In [ ]:

    

import chaospy as cp
x0 =np.array([-3200., 0., 3200., 625., 0., -270., 8500.]) 
# ux = cp.Uniform(x0[0]-400,x0[0]+400)
# uy = cp.Uniform(x0[1]-100,x0[1]+100)
# uz = cp.Uniform(x0[2]-600,x0[2]+800)
# uu = cp.Uniform(x0[3]-40,x0[3]+10)
# uv = cp.Uniform(x0[4]-10,x0[4]+100)
# uw = cp.Uniform(x0[5]-40,x0[5]+40)
# D = cp.J(ux,uy,uz,uu,uv,uw)

# R = D.sample(1000, 'S')
# print(R.shape)
# X0 = np.append(R, x0[-1]*np.ones_like(R[:1]),axis=0)
P = np.diag([150,30,200, 10, 10, 10, 0.0001])**1
D = cp.MvNormal(x0, P)
X0 = D.sample(1000, 'S')

print(X0.T[0])
tf = float(np.array(traj['time'])[-1])

best = [ (np.zeros((4,)), 1e12) ]

def objective(c):
    C = SRPController(traj, gains=c)

    t,x,u = simulation(X0, tf, C.controller, cycle=0.1)

    xf = x[-1]
    pos_err = (np.linalg.norm(xf[0:3], axis=0))
    vel_err = (np.linalg.norm(xf[3:6], axis=0))
    
    J = np.linalg.norm(pos_err) + np.linalg.norm(vel_err)*10
    if J < best[-1][1]:
        best.append((c, J))
        print("Gains = {}".format(c))
        print("Cost = {}".format(J))
        print("Max dist error = {} m".format(pos_err.max()))
        print("Max vel  error = {} m/s \n".format(vel_err.max()))
    return J

sol = differential_evolution(objective, bounds=((0,300), (0, 1e4), (0.5, 5), (0, 1e4) ), polish=False, disp=True)


    

In [83]:

    

x = np.linspace(-3,3)
y = x*np.clip(x,-2,2)
plt.plot(x,y)
plt.plot(x, x**2, 'k--')


    

    
Out[83]:





[<matplotlib.lines.Line2D at 0x233251c7da0>]






    

In [125]:

    

import chaospy as cp
import numpy as np
import matplotlib.pyplot as plt
plt.style.use("seaborn-whitegrid")
from scipy.interpolate import interp1d
from Utils.RK4 import RK4

D = cp.Uniform(-2,2)
# D = cp.Uniform(2, 2.5)
# X0 = cp.Normal(0,1).sample(2000, 'H')
X0 = D.sample(1000, 'M')
P0 = cp.Var(D)

def ufun(u, umax):
    return np.clip(u, -umax, umax)

def f(x, t, a, b, k, umax):
    return a*x + b*ufun(k*x, umax)

def fsat(x,y,a,b,umax):
    return a*x + b*np.sign(k*x)*umax

def cov(p, t, X, a, b, k, umax):
    x = X(t)
    xbar = np.mean(x)
    E = np.mean(x*ufun(k*x, umax))
    stuff = b*(E-k*xbar**2)
    return 2*a*p + 2*stuff

a,b,k,umax = 0.5,1,-0.75,1
t = np.linspace(0,5,300)
X = RK4(f, X0, t, args=(a,b,k,umax))
P = P0*np.exp(2*(a+b*k)*t) # Naive linear propagation - no saturation effects accounted for 
# Psat = RK4(cov, [P0], t, args=(interp1d(t,X,axis=0),a,b,k,umax))
Xf = X[-1]
stable = np.abs(Xf) <= np.abs(X0)
# print(X.shape)

plt.figure(figsize=(14,5))
plt.title("Trajectories")
try:
    plt.plot(t, X[:, stable], 'k', alpha=0.1)
except ZeroDivisionError:
    pass 

if not a==0:
    plt.plot(t, np.ones_like(t)*-b*umax/a,'b*--',label="Convergence Limit")
    plt.plot(t, np.ones_like(t)*b*umax/a, 'b*--')
plt.plot(t, np.ones_like(t)*umax/k, 'c*--', label='Saturation Limit')
plt.plot(t, np.ones_like(t)*-umax/k, 'c*--')

try:
    plt.plot(t, X[:, np.logical_not(stable)], 'r')
except ZeroDivisionError:
    pass 
plt.legend()

plt.figure()
plt.plot(t, np.var(X, axis=1), label="Sample Variance")
# plt.plot(t, P, 'k--', label="Integrated without saturation")
# plt.plot(t, Psat, label="Integrated with saturation")

plt.legend()
plt.ylabel("Variance")
plt.xlabel("Time")

plt.show()