Rocket Model Derrivation

In [2]:

    

from sympy import *
init_printing()


    

In [6]:

    

# State
x,y,vx,vy,theta,omega,m = symbols('x y vx vy theta, omega, m')
r = Matrix([x, y]) # Position
v = Matrix([vx, vy]) # Velocity
s = Matrix([r, v,[theta], [omega], [m]])

# Costate
lx,ly,lvx,lvy,ltheta,lomega,lm = symbols('lx ly lvx lvy ltheta lomega, lm')
lr = Matrix([lx, ly])
lv = Matrix([lvx,lvy])
l  = Matrix([lr, lv, [ltheta], [lomega], [lm]])

# Parametres
c1, c2, c3, g, a = symbols('c1 c2 c3 g a')
# Control
u, ut1, ut2 = symbols('u ut1 ut2')
c  = Matrix([u, ut1, ut2])

# Fullstate
fs = Matrix([s,l])


    

In [7]:

    

# State dynamics
dx     = vx
dy     = vy
dvx    = c1*u/m*ut1 - 1/2*rho*
dvy    = c1*u/m*ut2 - g
dtheta = omega
domega = -c1/c3*u/m*(ut1*cos(theta) + ut2*sin(theta))
dm     = -c1/c2*u
ds     = Matrix([dx, dy, dvx, dvy, dtheta, domega, dm])


    

In [8]:

    

# Homotopic Cost Lagrangian
L = a*c1/c2*u+(1-a)*c1**2/c2*u**2


    

In [16]:

    

# Hamiltonian
H = l.dot(ds) + L
print cse(H)


    

    




([(x0, 1/c2), (x1, c1*u*x0), (x2, c1*u/m)], [a*x1 + c1**2*u**2*x0*(-a + 1) - lm*x1 + ltheta*omega + lvx*ut1*x2 + lvy*(-g + ut2*x2) + lx*vx + ly*vy - lomega*x2*(ut1*cos(theta) + ut2*sin(theta))/c3])

In [10]:

    

# Costate dynamics
dl = -Matrix([diff(H, i) for i in s])


    

In [15]:

    

# Fullstate dynamics
dfs = Matrix([ds, dl])


    

In [20]:

    

lax = lvx - lomega /c3*cos(theta)
lay = lvy + lomega /c3*sin(theta)
la  = sqrt(lax**2 + lay**2)
ut1s = -lax / la
ut2s = -lay / la
uts = Matrix([ut1s, ut2s])
S = 1 - lm - la *c2 /m
us = 1/2/c1/(1-a)*(lm+la*c2/m-a)
us = (lm + c2/m * la - a)/(2*(1-a)*c1)
print cse(Matrix([ut1s, ut2s, S, us]))


    

    




([(x0, lomega/c3), (x1, x0*cos(theta)), (x2, x0*sin(theta)), (x3, sqrt((lvx - x1)**2 + (lvy + x2)**2)), (x4, 1/x3), (x5, c2*x3/m)], [Matrix([
[                x4*(-lvx + x1)],
[                x4*(-lvy - x2)],
[                  -lm - x5 + 1],
[(-a + lm + x5)/(c1*(-2*a + 2))]])])

In [ ]: