def setupModel(dae, conf):
# PARAMETERS OF THE KITE :
# ##############
m = conf["kite"]["mass"] # mass of the kite # [ kg ]
# PHYSICAL CONSTANTS :
# ##############
g = conf["env"]["g"] # gravitational constant # [ m /s^2]
# PARAMETERS OF THE CABLE :
# ##############
# INERTIA MATRIX (Kurt's direct measurements)
j1 = conf["kite"]["j1"]
j31 = conf["kite"]["j31"]
j2 = conf["kite"]["j2"]
j3 = conf["kite"]["j3"]
# Carousel Friction & inertia
jCarousel = conf["carousel"]["jCarousel"]
cfric = conf["carousel"]["cfric"]
zt = conf["kite"]["zt"]
rA = conf["carousel"]["rArm"]
########### model integ ###################
e11 = dae.x("e11")
e12 = dae.x("e12")
e13 = dae.x("e13")
e21 = dae.x("e21")
e22 = dae.x("e22")
e23 = dae.x("e23")
e31 = dae.x("e31")
e32 = dae.x("e32")
e33 = dae.x("e33")
x = dae.x("x")
y = dae.x("y")
z = dae.x("z")
dx = dae.x("dx")
dy = dae.x("dy")
dz = dae.x("dz")
w1 = dae.x("w1")
w2 = dae.x("w2")
w3 = dae.x("w3")
delta = 0
ddelta = 0
r = dae.x("r")
dr = dae.x("dr")
ddr = dae.u("ddr")
# wind
z0 = conf["wind shear"]["z0"]
zt_roughness = conf["wind shear"]["zt_roughness"]
zsat = 0.5 * (z + C.sqrt(z * z))
wind_x = dae.p("w0") * C.log((zsat + zt_roughness + 2) / zt_roughness) / C.log(z0 / zt_roughness)
dae.addOutput("wind at altitude", wind_x)
dae.addOutput("w0", dae.p("w0"))
dp_carousel_frame = C.veccat([dx - ddelta * y, dy + ddelta * (rA + x), dz]) - C.veccat(
[C.cos(delta) * wind_x, C.sin(delta) * wind_x, 0]
)
R_c2b = C.veccat(dae.x(["e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32", "e33"])).reshape((3, 3))
# Aircraft velocity w.r.t. inertial frame, given in its own reference frame
# (needed to compute the aero forces and torques !)
dpE = C.mul(R_c2b, dp_carousel_frame)
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(
dae,
conf,
dp_carousel_frame,
dpE,
dae.x(("w1", "w2", "w3")),
dae.x(("e21", "e22", "e23")),
dae.u(("aileron", "elevator")),
)
# mass matrix
mm = C.SXMatrix(7, 7)
mm[0, 0] = m
mm[0, 1] = 0
mm[0, 2] = 0
mm[0, 3] = 0
mm[0, 4] = 0
mm[0, 5] = 0
mm[0, 6] = x + zt * e31
mm[1, 0] = 0
mm[1, 1] = m
mm[1, 2] = 0
mm[1, 3] = 0
mm[1, 4] = 0
mm[1, 5] = 0
mm[1, 6] = y + zt * e32
mm[2, 0] = 0
mm[2, 1] = 0
mm[2, 2] = m
mm[2, 3] = 0
mm[2, 4] = 0
mm[2, 5] = 0
mm[2, 6] = z + zt * e33
mm[3, 0] = 0
mm[3, 1] = 0
mm[3, 2] = 0
mm[3, 3] = j1
mm[3, 4] = 0
mm[3, 5] = j31
mm[3, 6] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
mm[4, 0] = 0
mm[4, 1] = 0
mm[4, 2] = 0
mm[4, 3] = 0
mm[4, 4] = j2
mm[4, 5] = 0
mm[4, 6] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
mm[5, 0] = 0
mm[5, 1] = 0
mm[5, 2] = 0
mm[5, 3] = j31
mm[5, 4] = 0
mm[5, 5] = j3
mm[5, 6] = 0
mm[6, 0] = x + zt * e31
mm[6, 1] = y + zt * e32
mm[6, 2] = z + zt * e33
mm[6, 3] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
mm[6, 4] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
mm[6, 5] = 0
mm[6, 6] = 0
# right hand side
zt2 = zt * zt
rhs = C.veccat(
[
f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) + ddelta * dy * m,
f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m,
f3 - g * m,
t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3,
t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3),
t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2,
ddr * r
- (
zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
+ zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
)
* (w3 - ddelta * e33)
- dx * (dx - zt * e21 * (w1 - ddelta * e13) + zt * e11 * (w2 - ddelta * e23))
- dy * (dy - zt * e22 * (w1 - ddelta * e13) + zt * e12 * (w2 - ddelta * e23))
- dz * (dz - zt * e23 * (w1 - ddelta * e13) + zt * e13 * (w2 - ddelta * e23))
+ dr * dr
+ (w1 - ddelta * e13)
* (
e21 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23))
+ e22 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23))
+ zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1)
+ zt
* e33
* (
w1 * z
+ zt * e33 * w1
+ ddelta * e11 * x
+ ddelta * e12 * y
+ zt * ddelta * e11 * e31
+ zt * ddelta * e12 * e32
)
+ zt * e31 * (x + zt * e31) * (w1 - ddelta * e13)
+ zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
)
- (w2 - ddelta * e23)
* (
e11 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23))
+ e12 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23))
+ zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1)
- zt
* e33
* (
w2 * z
+ zt * e33 * w2
+ ddelta * e21 * x
+ ddelta * e22 * y
+ zt * ddelta * e21 * e31
+ zt * ddelta * e22 * e32
)
- zt * e31 * (x + zt * e31) * (w2 - ddelta * e23)
- zt * e32 * (y + zt * e32) * (w2 - ddelta * e23)
),
]
)
dRexp = C.SXMatrix(3, 3)
dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23)
dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33)
dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13)
dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23)
dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33)
dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13)
dRexp[2, 0] = e23 * w3 - e33 * w2
dRexp[2, 1] = e33 * w1 - e13 * w3
dRexp[2, 2] = e13 * w2 - e23 * w1
c = (x + zt * e31) ** 2 / 2 + (y + zt * e32) ** 2 / 2 + (z + zt * e33) ** 2 / 2 - r ** 2 / 2
cdot = (
dx * (x + zt * e31)
+ dy * (y + zt * e32)
+ dz * (z + zt * e33)
+ zt * (w2 - ddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
- zt * (w1 - ddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
- r * dr
)
ddx = dae.z("ddx")
ddy = dae.z("ddy")
ddz = dae.z("ddz")
dw1 = dae.z("dw1")
dw2 = dae.z("dw2")
dddelta = 0
cddot = (
-(w1 - ddelta * e13)
* (
zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1)
+ zt
* e33
* (
w1 * z
+ zt * e33 * w1
+ ddelta * e11 * x
+ ddelta * e12 * y
+ zt * ddelta * e11 * e31
+ zt * ddelta * e12 * e32
)
+ zt * e21 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
+ zt * e22 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
+ zt * e31 * (x + zt * e31) * (w1 - ddelta * e13)
+ zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
)
+ (w2 - ddelta * e23)
* (
zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1)
- zt
* e33
* (
w2 * z
+ zt * e33 * w2
+ ddelta * e21 * x
+ ddelta * e22 * y
+ zt * ddelta * e21 * e31
+ zt * ddelta * e22 * e32
)
+ zt * e11 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
+ zt * e12 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
- zt * e31 * (x + zt * e31) * (w2 - ddelta * e23)
- zt * e32 * (y + zt * e32) * (w2 - ddelta * e23)
)
- ddr * r
+ (
zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
+ zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
)
* (w3 - ddelta * e33)
+ dx * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
+ dy * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
+ dz * (dz + zt * e13 * w2 - zt * e23 * w1)
+ ddx * (x + zt * e31)
+ ddy * (y + zt * e32)
+ ddz * (z + zt * e33)
- dr * dr
+ zt * (dw2 - dddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
- zt * (dw1 - dddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
- zt
* dddelta
* (
e11 * e23 * x
- e13 * e21 * x
+ e12 * e23 * y
- e13 * e22 * y
+ zt * e11 * e23 * e31
- zt * e13 * e21 * e31
+ zt * e12 * e23 * e32
- zt * e13 * e22 * e32
)
)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae.addOutput("c", c)
dae.addOutput("cdot", cdot)
dae.addOutput("cddot", cddot)
return (mm, rhs, dRexp)
def setupModel(dae, conf):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
mass matrix columns:
ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu
rhs:
forces/torques acting on : ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu
'''
# Parameters
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
jCarousel = conf['jCarousel']
cfric = conf['cfric']
zt = conf['zt']
rA = conf['rArm']
# Frames
# ==========================================
# World frame (n) NED North-East-Down
#
# Wind carrying frame (w)
# - Translates along the world frame's x axis with the wind speed
#
# Carousel frame (c)
# - Origin coincides with the world origin
# - Makes and angle delta
#
# Carousel tip frame (a)
# - Origin sits at tip of arm
# - e_x extends radially outwards
# - e_z points downwards
#
# Body frame (b)
# - attached to body of aircraft
# - e_x extends to the nose
# - e_z points downwards
#
# Vector naming convention
# =========================
# v_ab_c
# Velocity of point a, differentiated in frame b, expressed in frame c (Greg terminlogy)
# Velocity of point a w.r.t. frame b, expressed in frame c (Joris terminology)
# States
# Components that make up the rotation matrix from carousel frame to body frame R_c2b [-]
# e11 e12 e13
# e21 e22 e23
# e31 e32 e33
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x'] # Aircraft position in carousel tip frame coordinates [m]
y = dae['y']
z = dae['z']
dx = dae['dx'] # Time derivatives of x [m/s]
dy = dae['dy']
dz = dae['dz']
w1 = dae['w_bn_b_x'] # Body angular rate w.r.t world frame, expressed in body coordinates [rad/s^2]
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
ddelta = dae['ddelta'] # Carousel turning speed [rad/s]
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
tc = dae['motor_torque'] #Carousel motor torque
if 'xt' in conf:
t_xt = dae['tether_tension'] * conf['xt']
else:
t_xt = 0.0
R_g2c = C.blockcat([[dae['cos_delta'], -dae['sin_delta'], 0.0],
[dae['sin_delta'], dae['cos_delta'], 0.0],
[0.0, 0.0, 1.0]])
# wind model
def getWind():
if 'wind_model' not in conf:
return C.veccat([0, 0, 0])
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((altitude+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
# altitude is -z - altitude0, where altitude0 is a user parameter
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
altitude = -z - conf['wind_model']['altitude0']
wind = dae['w0']*C.log((altitude+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness)
dae['wind_at_altitude'] = wind
return C.mul([R_g2c, C.veccat([wind, 0, 0])])
elif conf['wind_model']['name'] in ['constant','hardcoded']:
# constant wind
dae['wind_at_altitude'] = dae['w0']
return C.mul([R_g2c, C.veccat([dae['w0'], 0, 0])])
elif conf['wind_model']['name'] == 'random_walk':
dae.addU('delta_wind_x')
dae.addU('delta_wind_y')
dae.addU('delta_wind_z')
wind_x = dae.addX('wind_x')
wind_y = dae.addX('wind_y')
wind_z = dae.addX('wind_z')
dae['wind_at_altitude'] = wind_x
return C.veccat([wind_x, wind_y, wind_z])
wind = getWind()
# Velocity of aircraft w.r.t wind carrying frame, expressed in carousel frame
v_bw_c = C.veccat( [ dx - ddelta*y
, dy + ddelta*(rA + x)
, dz
]) - wind
# Velocity of aircraft w.r.t wind carrying frame, expressed in body frame
# (needed to compute the aero forces and torques !)
v_bw_b = C.mul( dae['R_c2b'], v_bw_c )
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates
)
# f1..f3 expressed in carrousel coordinates
# t1..t3 expressed in body coordinates
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 - gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 - gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 - gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 - gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy)
if 'useVirtualForces' in conf:
_v = conf[ 'useVirtualForces' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
dae.addU('df1_disturbance')
f1 += dae['rho_sref_v2'] * dae.addX('f1_disturbance')
if _which[ 1 ]:
dae.addU('df2_disturbance')
f2 += dae['rho_sref_v2'] * dae.addX('f2_disturbance')
if _which[ 2 ]:
dae.addU('df3_disturbance')
f3 += dae['rho_sref_v2'] * dae.addX('f3_disturbance')
elif _type == 'parameter':
if _which[ 0 ]:
f1 += dae['rho_sref_v2'] * dae.addX('f1_disturbance')
if _which[ 1 ]:
f2 += dae['rho_sref_v2'] * dae.addX('f2_disturbance')
if _which[ 2 ]:
f3 += dae['rho_sref_v2'] * dae.addX('f3_disturbance')
elif _type == 'online_data':
if _which[ 0 ]:
f1 += dae['rho_sref_v2'] * dae.addP('f1_disturbance')
if _which[ 1 ]:
f2 += dae['rho_sref_v2'] * dae.addP('f2_disturbance')
if _which[ 2 ]:
f3 += dae['rho_sref_v2'] * dae.addP('f3_disturbance')
else:
raise ValueError("WTF?")
if 'useVirtualTorques' in conf:
_v = conf[ 'useVirtualTorques' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
dae.addU('dt1_disturbance')
t1 += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
if _which[ 1 ]:
dae.addU('dt2_disturbance')
t2 += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
if _which[ 2 ]:
dae.addU('dt3_disturbance')
t3 += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')
elif _type == 'parameter':
if _which[ 0 ]:
t1 += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
if _which[ 1 ]:
t2 += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
if _which[ 2 ]:
t3 += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')
elif _type == 'online_data':
if _which[ 0 ]:
t1 += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t1_disturbance')
if _which[ 1 ]:
t2 += dae['rho_sref_v2'] * conf['cref'] * dae.addP('t2_disturbance')
if _which[ 2 ]:
t3 += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t3_disturbance')
else:
raise ValueError("WTF?")
# mass matrix
mm = C.SX.zeros(8, 8)
mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x
mm[0,1] = -m*y
mm[0,2] = m*(rA + x)
mm[0,3] = 0
mm[0,4] = 0
mm[0,5] = 0
mm[0,6] = 0
mm[0,7] = 0
mm[1,0] = -m*y
mm[1,1] = m
mm[1,2] = 0
mm[1,3] = 0
mm[1,4] = 0
mm[1,5] = 0
mm[1,6] = 0
mm[1,7] = x + zt*e31
mm[2,0] = m*(rA + x)
mm[2,1] = 0
mm[2,2] = m
mm[2,3] = 0
mm[2,4] = 0
mm[2,5] = 0
mm[2,6] = 0
mm[2,7] = y + zt*e32
mm[3,0] = 0
mm[3,1] = 0
mm[3,2] = 0
mm[3,3] = m
mm[3,4] = 0
mm[3,5] = 0
mm[3,6] = 0
mm[3,7] = z + zt*e33
mm[4,0] = 0
mm[4,1] = 0
mm[4,2] = 0
mm[4,3] = 0
mm[4,4] = j1
mm[4,5] = 0
mm[4,6] = j31
mm[4,7] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[5,0] = 0
mm[5,1] = 0
mm[5,2] = 0
mm[5,3] = 0
mm[5,4] = 0
mm[5,5] = j2
mm[5,6] = 0
mm[5,7] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[6,0] = 0
mm[6,1] = 0
mm[6,2] = 0
mm[6,3] = 0
mm[6,4] = j31
mm[6,5] = 0
mm[6,6] = j3
mm[6,7] = 0
mm[7,0] = -zt*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
mm[7,1] = x + zt*e31
mm[7,2] = y + zt*e32
mm[7,3] = z + zt*e33
mm[7,4] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[7,5] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[7,6] = 0
mm[7,7] = 0
# right hand side
zt2 = zt*zt
rhs = C.veccat(
[ tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x)
, f1 + ddelta*m*(dy + ddelta*rA + ddelta*x) + ddelta*dy*m
, f2 - ddelta*m*(dx - ddelta*y) - ddelta*dx*m
, f3 + g*m
, t1 - w2*(j3*w3 + j31*w1) + j2*w2*w3
, t2 + w1*(j3*w3 + j31*w1) - w3*(j1*w1 + j31*w3) + t_xt
, t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2
, ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))
] )
dRexp = C.SX.zeros(3,3)
dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23)
dRexp[0,1] = e31*(w1 - ddelta*e13) - e11*(w3 - ddelta*e33)
dRexp[0,2] = e11*(w2 - ddelta*e23) - e21*(w1 - ddelta*e13)
dRexp[1,0] = e22*(w3 - ddelta*e33) - e32*(w2 - ddelta*e23)
dRexp[1,1] = e32*(w1 - ddelta*e13) - e12*(w3 - ddelta*e33)
dRexp[1,2] = e12*(w2 - ddelta*e23) - e22*(w1 - ddelta*e13)
dRexp[2,0] = e23*w3 - e33*w2
dRexp[2,1] = e33*w1 - e13*w3
dRexp[2,2] = e13*w2 - e23*w1
# The cable constraint
c =(x + zt*e31)**2/2 + (y + zt*e32)**2/2 + (z + zt*e33)**2/2 - r**2/2
cdot =dx*(x + zt*e31) + dy*(y + zt*e32) + dz*(z + zt*e33) + zt*(w2 - ddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(w1 - ddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - r*dr
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
# dddelta = dae['dddelta']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
dddelta = dae.ddt('ddelta')
cddot = -(w1-ddelta*e13)*(zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e21*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e22*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))+(w2-ddelta*e23)*(zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)+zt*e11*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e12*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))-ddr*r+(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)+dx*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+dy*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+dz*(dz+zt*e13*w2-zt*e23*w1)+ddx*(x+zt*e31)+ddy*(y+zt*e32)+ddz*(z+zt*e33)-dr*dr+zt*(dw2-dddelta*e23)*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)-zt*(dw1-dddelta*e13)*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33)-zt*dddelta*(e11*e23*x-e13*e21*x+e12*e23*y-e13*e22*y+zt*e11*e23*e31-zt*e13*e21*e31+zt*e12*e23*e32-zt*e13*e22*e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def setupModel(dae, conf):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
jCarousel = conf['jCarousel']
cfric = conf['cfric']
zt = conf['zt']
rA = conf['rArm']
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x']
y = dae['y']
z = dae['z']
dx = dae['dx']
dy = dae['dy']
dz = dae['dz']
w1 = dae['w_bn_b_x']
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
ddelta = dae['ddelta']
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
tc = dae['motor_torque'] #Carousel motor torque
# wind model
def getWind():
if 'wind_model' not in conf:
return 0
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((z+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
zsat = 0.5 * (-z + C.sqrt(z * z))
return dae['w0'] * C.log(
(zsat + zt_roughness + 2) / zt_roughness) / C.log(
z0 / zt_roughness)
elif conf['wind_model']['name'] == 'constant':
# constant wind
return dae['w0']
wind_x = getWind()
dae['wind_at_altitude'] = wind_x
dp_carousel_frame = C.veccat([
dx - ddelta * y, dy + ddelta * (rA + x), dz
]) - C.veccat([dae['cos_delta'] * wind_x, dae['sin_delta'] * wind_x, 0])
# Aircraft velocity w.r.t. inertial frame, given in its own reference frame
# (needed to compute the aero forces and torques !)
dpE = C.mul(dae['R_c2b'], dp_carousel_frame)
(f1, f2, f3, t1, t2,
t3) = aeroForcesTorques(dae, conf, dp_carousel_frame, dpE, dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']))
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 -
gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 -
gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 -
gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 -
gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 -
gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 -
gamma_homotopy)
# mass matrix
mm = C.SXMatrix(8, 8)
mm[0, 0] = jCarousel + m * rA * rA + m * x * x + m * y * y + 2 * m * rA * x
mm[0, 1] = -m * y
mm[0, 2] = m * (rA + x)
mm[0, 3] = 0
mm[0, 4] = 0
mm[0, 5] = 0
mm[0, 6] = 0
mm[0, 7] = 0
mm[1, 0] = -m * y
mm[1, 1] = m
mm[1, 2] = 0
mm[1, 3] = 0
mm[1, 4] = 0
mm[1, 5] = 0
mm[1, 6] = 0
mm[1, 7] = x + zt * e31
mm[2, 0] = m * (rA + x)
mm[2, 1] = 0
mm[2, 2] = m
mm[2, 3] = 0
mm[2, 4] = 0
mm[2, 5] = 0
mm[2, 6] = 0
mm[2, 7] = y + zt * e32
mm[3, 0] = 0
mm[3, 1] = 0
mm[3, 2] = 0
mm[3, 3] = m
mm[3, 4] = 0
mm[3, 5] = 0
mm[3, 6] = 0
mm[3, 7] = z + zt * e33
mm[4, 0] = 0
mm[4, 1] = 0
mm[4, 2] = 0
mm[4, 3] = 0
mm[4, 4] = j1
mm[4, 5] = 0
mm[4, 6] = j31
mm[4, 7] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)
mm[5, 0] = 0
mm[5, 1] = 0
mm[5, 2] = 0
mm[5, 3] = 0
mm[5, 4] = 0
mm[5, 5] = j2
mm[5, 6] = 0
mm[5, 7] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33)
mm[6, 0] = 0
mm[6, 1] = 0
mm[6, 2] = 0
mm[6, 3] = 0
mm[6, 4] = j31
mm[6, 5] = 0
mm[6, 6] = j3
mm[6, 7] = 0
mm[7,
0] = -zt * (e11 * e23 * x - e13 * e21 * x + e12 * e23 * y -
e13 * e22 * y + zt * e11 * e23 * e31 - zt * e13 * e21 * e31
+ zt * e12 * e23 * e32 - zt * e13 * e22 * e32)
mm[7, 1] = x + zt * e31
mm[7, 2] = y + zt * e32
mm[7, 3] = z + zt * e33
mm[7, 4] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)
mm[7, 5] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33)
mm[7, 6] = 0
mm[7, 7] = 0
# right hand side
zt2 = zt * zt
rhs = C.veccat([
tc - cfric * ddelta - f1 * y + f2 * (rA + x) + dy * m *
(dx - 2 * ddelta * y) - dx * m *
(dy + 2 * ddelta * rA + 2 * ddelta * x),
f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) + ddelta * dy * m,
f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m, f3 + g * m,
t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3,
t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3),
t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2,
ddr * r - (zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33) + zt * w2 *
(e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)) * (w3 - ddelta * e33) -
dx * (dx - zt * e21 * (w1 - ddelta * e13) + zt * e11 *
(w2 - ddelta * e23)) - dy * (dy - zt * e22 *
(w1 - ddelta * e13) + zt * e12 *
(w2 - ddelta * e23)) - dz *
(dz - zt * e23 * (w1 - ddelta * e13) + zt * e13 *
(w2 - ddelta * e23)) + dr * dr + (w1 - ddelta * e13) *
(e21 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 *
(w2 - ddelta * e23)) + e22 *
(zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 *
(w2 - ddelta * e23)) + zt * e23 *
(dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 *
(w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y +
zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32) + zt * e31 *
(x + zt * e31) * (w1 - ddelta * e13) + zt * e32 * (y + zt * e32) *
(w1 - ddelta * e13)) - (w2 - ddelta * e23) *
(e11 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 *
(w2 - ddelta * e23)) + e12 *
(zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 *
(w2 - ddelta * e23)) + zt * e13 *
(dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 *
(w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y +
zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32) - zt * e31 *
(x + zt * e31) * (w2 - ddelta * e23) - zt * e32 * (y + zt * e32) *
(w2 - ddelta * e23))
])
dRexp = C.SXMatrix(3, 3)
dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23)
dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33)
dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13)
dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23)
dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33)
dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13)
dRexp[2, 0] = e23 * w3 - e33 * w2
dRexp[2, 1] = e33 * w1 - e13 * w3
dRexp[2, 2] = e13 * w2 - e23 * w1
c = (x + zt * e31)**2 / 2 + (y + zt * e32)**2 / 2 + (
z + zt * e33)**2 / 2 - r**2 / 2
cdot = dx * (x + zt * e31) + dy * (y + zt * e32) + dz * (
z + zt * e33) + zt * (w2 - ddelta * e23) * (
e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 +
zt * e13 * e33) - zt * (w1 - ddelta * e13) * (
e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 +
zt * e23 * e33) - r * dr
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
# dddelta = dae['dddelta']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
dddelta = dae.ddt('ddelta')
cddot = -(w1 - ddelta * e13) * (
zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 *
(w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y +
zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32) + zt * e21 *
(dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + zt * e22 *
(dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 +
zt * ddelta * e13 * e22) + zt * e31 * (x + zt * e31) *
(w1 - ddelta * e13) + zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
) + (w2 - ddelta * e23) * (
zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 *
(w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y +
zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32) + zt * e11 *
(dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + zt * e12 *
(dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 +
zt * ddelta * e13 * e22) - zt * e31 * (x + zt * e31) *
(w2 - ddelta * e23) - zt * e32 * (y + zt * e32) *
(w2 - ddelta * e23)) - ddr * r + (
zt * w1 *
(e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 +
zt * e13 * e33) + zt * w2 *
(e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 +
zt * e23 * e33)) * (w3 - ddelta * e33) + dx * (
dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + dy * (
dy + zt * e12 * w2 - zt * e22 * w1 -
zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22
) + dz * (dz + zt * e13 * w2 - zt * e23 * w1) + ddx * (
x + zt * e31) + ddy * (y + zt * e32) + ddz * (
z +
zt * e33) - dr * dr + zt * (dw2 - dddelta * e23) * (
e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 +
zt * e13 * e33) - zt * (dw1 - dddelta * e13) * (
e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 +
zt * e23 * e33) - zt * dddelta * (
e11 * e23 * x - e13 * e21 * x +
e12 * e23 * y - e13 * e22 * y + zt * e11 *
e23 * e31 - zt * e13 * e21 * e31 + zt *
e12 * e23 * e32 - zt * e13 * e22 * e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def setupModel(dae, conf):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
mass matrix columns:
ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu
rhs:
forces/torques acting on : ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu
'''
# Parameters
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
jCarousel = conf['jCarousel']
cfric = conf['cfric']
zt = conf['zt']
rA = conf['rArm']
# Frames
# ==========================================
# World frame (n) NED North-East-Down
#
# Wind carrying frame (w)
# - Translates along the world frame's x axis with the wind speed
#
# Carousel frame (c)
# - Origin coincides with the world origin
# - Makes and angle delta
#
# Carousel tip frame (a)
# - Origin sits at tip of arm
# - e_x extends radially outwards
# - e_z points downwards
#
# Body frame (b)
# - attached to body of aircraft
# - e_x extends to the nose
# - e_z points downwards
#
# Vector naming convention
# =========================
# v_ab_c
# Velocity of point a, differentiated in frame b, expressed in frame c (Greg terminlogy)
# Velocity of point a w.r.t. frame b, expressed in frame c (Joris terminology)
# States
# Components that make up the rotation matrix from carousel frame to body frame R_c2b [-]
# e11 e12 e13
# e21 e22 e23
# e31 e32 e33
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x'] # Aircraft position in carousel tip frame coordinates [m]
y = dae['y']
z = dae['z']
dx = dae['dx'] # Time derivatives of x [m/s]
dy = dae['dy']
dz = dae['dz']
w1 = dae['w_bn_b_x'] # Body angular rate w.r.t world frame, expressed in body coordinates [rad/s^2]
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
ddelta = dae['ddelta'] # Carousel turning speed [rad/s]
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
tc = dae['motor_torque'] #Carousel motor torque
if 'xt' in conf:
t_xt = dae['tether_tension'] * conf['xt']
else:
t_xt = 0.0
R_g2c = C.blockcat([[dae['cos_delta'], -dae['sin_delta'], 0.0],
[dae['sin_delta'], dae['cos_delta'], 0.0],
[0.0, 0.0, 1.0]])
# wind model
def getWind():
if 'wind_model' not in conf:
return C.veccat([0, 0, 0])
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((altitude+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
# altitude is -z - altitude0, where altitude0 is a user parameter
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
altitude = -z - conf['wind_model']['altitude0']
wind = dae['w0']*C.log((altitude+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness)
dae['wind_at_altitude'] = wind
return C.mul([R_g2c, C.veccat([wind, 0, 0])])
elif conf['wind_model']['name'] in ['constant','hardcoded']:
# constant wind
dae['wind_at_altitude'] = dae['w0']
return C.mul([R_g2c, C.veccat([dae['w0'], 0, 0])])
elif conf['wind_model']['name'] == 'random_walk':
dae.addU('delta_wind_x')
dae.addU('delta_wind_y')
dae.addU('delta_wind_z')
wind_x = dae.addX('wind_x')
wind_y = dae.addX('wind_y')
wind_z = dae.addX('wind_z')
dae['wind_at_altitude'] = wind_x
return C.veccat([wind_x, wind_y, wind_z])
wind = getWind()
# Velocity of aircraft w.r.t wind carrying frame, expressed in carousel frame
v_bw_c = C.veccat( [ dx - ddelta*y
, dy + ddelta*(rA + x)
, dz
]) - wind
# Velocity of aircraft w.r.t wind carrying frame, expressed in body frame
# (needed to compute the aero forces and torques !)
v_bw_b = C.mul( dae['R_c2b'], v_bw_c )
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates
)
# f1..f3 expressed in carrousel coordinates
# t1..t3 expressed in body coordinates
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 - gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 - gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 - gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 - gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy)
fx_dist = 0
fy_dist = 0
fz_dist = 0
mx_dist = 0
my_dist = 0
mz_dist = 0
if 'useVirtualForces' in conf:
_v = conf[ 'useVirtualForces' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
dae.addU('df1_disturbance')
fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance')
if _which[ 1 ]:
dae.addU('df2_disturbance')
fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance')
if _which[ 2 ]:
dae.addU('df3_disturbance')
fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance')
elif _type == 'parameter':
if _which[ 0 ]:
fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance')
if _which[ 1 ]:
fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance')
if _which[ 2 ]:
fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance')
elif _type == 'online_data':
if _which[ 0 ]:
fx_dist += dae['rho_sref_v2'] * dae.addP('f1_disturbance')
if _which[ 1 ]:
fy_dist += dae['rho_sref_v2'] * dae.addP('f2_disturbance')
if _which[ 2 ]:
fz_dist += dae['rho_sref_v2'] * dae.addP('f3_disturbance')
else:
raise ValueError("WTF?")
f1 += fx_dist
f2 += fy_dist
f3 += fz_dist
dae["aero_fx_dist"] = fx_dist
dae["aero_fy_dist"] = fy_dist
dae["aero_fz_dist"] = fz_dist
if 'useVirtualTorques' in conf:
_v = conf[ 'useVirtualTorques' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
dae.addU('dt1_disturbance')
mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
if _which[ 1 ]:
dae.addU('dt2_disturbance')
my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
if _which[ 2 ]:
dae.addU('dt3_disturbance')
mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')
elif _type == 'parameter':
if _which[ 0 ]:
mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
if _which[ 1 ]:
my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
if _which[ 2 ]:
mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')
elif _type == 'online_data':
if _which[ 0 ]:
mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t1_disturbance')
if _which[ 1 ]:
my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addP('t2_disturbance')
if _which[ 2 ]:
mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t3_disturbance')
else:
raise ValueError("WTF?")
t1 += mx_dist
t2 += my_dist
t3 += mz_dist
dae["aero_mx_dist"] = mx_dist
dae["aero_my_dist"] = my_dist
dae["aero_mz_dist"] = mz_dist
# mass matrix
mm = C.SXMatrix(8, 8)
mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x
mm[0,1] = -m*y
mm[0,2] = m*(rA + x)
mm[0,3] = 0
mm[0,4] = 0
mm[0,5] = 0
mm[0,6] = 0
mm[0,7] = 0
mm[1,0] = -m*y
mm[1,1] = m
mm[1,2] = 0
mm[1,3] = 0
mm[1,4] = 0
mm[1,5] = 0
mm[1,6] = 0
mm[1,7] = x + zt*e31
mm[2,0] = m*(rA + x)
mm[2,1] = 0
mm[2,2] = m
mm[2,3] = 0
mm[2,4] = 0
mm[2,5] = 0
mm[2,6] = 0
mm[2,7] = y + zt*e32
mm[3,0] = 0
mm[3,1] = 0
mm[3,2] = 0
mm[3,3] = m
mm[3,4] = 0
mm[3,5] = 0
mm[3,6] = 0
mm[3,7] = z + zt*e33
mm[4,0] = 0
mm[4,1] = 0
mm[4,2] = 0
mm[4,3] = 0
mm[4,4] = j1
mm[4,5] = 0
mm[4,6] = j31
mm[4,7] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[5,0] = 0
mm[5,1] = 0
mm[5,2] = 0
mm[5,3] = 0
mm[5,4] = 0
mm[5,5] = j2
mm[5,6] = 0
mm[5,7] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[6,0] = 0
mm[6,1] = 0
mm[6,2] = 0
mm[6,3] = 0
mm[6,4] = j31
mm[6,5] = 0
mm[6,6] = j3
mm[6,7] = 0
mm[7,0] = -zt*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
mm[7,1] = x + zt*e31
mm[7,2] = y + zt*e32
mm[7,3] = z + zt*e33
mm[7,4] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[7,5] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[7,6] = 0
mm[7,7] = 0
# right hand side
zt2 = zt*zt
rhs = C.veccat(
[ tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x)
, f1 + ddelta*m*(dy + ddelta*rA + ddelta*x) + ddelta*dy*m
, f2 - ddelta*m*(dx - ddelta*y) - ddelta*dx*m
, f3 + g*m
, t1 - w2*(j3*w3 + j31*w1) + j2*w2*w3
, t2 + w1*(j3*w3 + j31*w1) - w3*(j1*w1 + j31*w3) + t_xt
, t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2
, ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))
] )
dRexp = C.SXMatrix(3,3)
dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23)
dRexp[0,1] = e31*(w1 - ddelta*e13) - e11*(w3 - ddelta*e33)
dRexp[0,2] = e11*(w2 - ddelta*e23) - e21*(w1 - ddelta*e13)
dRexp[1,0] = e22*(w3 - ddelta*e33) - e32*(w2 - ddelta*e23)
dRexp[1,1] = e32*(w1 - ddelta*e13) - e12*(w3 - ddelta*e33)
dRexp[1,2] = e12*(w2 - ddelta*e23) - e22*(w1 - ddelta*e13)
dRexp[2,0] = e23*w3 - e33*w2
dRexp[2,1] = e33*w1 - e13*w3
dRexp[2,2] = e13*w2 - e23*w1
# The cable constraint
c =(x + zt*e31)**2/2 + (y + zt*e32)**2/2 + (z + zt*e33)**2/2 - r**2/2
cdot =dx*(x + zt*e31) + dy*(y + zt*e32) + dz*(z + zt*e33) + zt*(w2 - ddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(w1 - ddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - r*dr
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
# dddelta = dae['dddelta']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
dddelta = dae.ddt('ddelta')
cddot = -(w1-ddelta*e13)*(zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e21*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e22*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))+(w2-ddelta*e23)*(zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)+zt*e11*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e12*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))-ddr*r+(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)+dx*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+dy*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+dz*(dz+zt*e13*w2-zt*e23*w1)+ddx*(x+zt*e31)+ddy*(y+zt*e32)+ddz*(z+zt*e33)-dr*dr+zt*(dw2-dddelta*e23)*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)-zt*(dw1-dddelta*e13)*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33)-zt*dddelta*(e11*e23*x-e13*e21*x+e12*e23*y-e13*e22*y+zt*e11*e23*e31-zt*e13*e21*e31+zt*e12*e23*e32-zt*e13*e22*e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def crosswind_model(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ('nu')
dae.addX([
'r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z', 'e11', 'e12', 'e13', 'e21',
'e22', 'e23', 'e31', 'e32', 'e33', 'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z',
'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z', 'aileron', 'elevator', 'rudder',
'flaps', 'prop_drag'
])
dae.addU(['daileron', 'delevator', 'drudder', 'dflaps', 'dprop_drag'])
dae.addP(['r', 'w0'])
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('v_bn_n_x')
dae['ddy'] = dae.ddt('v_bn_n_y')
dae['ddz'] = dae.ddt('v_bn_n_z')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat(
[dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron'] * 180 / C.pi
dae['elevator_deg'] = dae['elevator'] * 180 / C.pi
dae['rudder_deg'] = dae['rudder'] * 180 / C.pi
dae['daileron_deg_s'] = dae['daileron'] * 180 / C.pi
dae['delevator_deg_s'] = dae['delevator'] * 180 / C.pi
dae['drudder_deg_s'] = dae['drudder'] * 180 / C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r'] * dae['nu']
dae['R_n2b'] = C.vertcat([
C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
C.horzcat([dae['e31'], dae['e32'], dae['e33']])
])
# local wind
dae['wind_at_altitude'] = get_wind(dae, conf)
# wind velocity in NED
dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])
# body velocity in NED
dae['v_bn_n'] = C.veccat(
[dae['v_bn_n_x'], dae['v_bn_n_y'], dae['v_bn_n_z']])
# body velocity w.r.t. wind in NED
v_bw_n = dae['v_bn_n'] - dae['v_wn_n']
# body velocity w.r.t. wind in body frame
v_bw_b = C.mul(dae['R_n2b'], v_bw_n)
# compute aerodynamic forces and moments
(f1, f2, f3, t1, t2, t3) = \
aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']))
dae['prop_drag_vector'] = dae['prop_drag'] * v_bw_n / dae['airspeed']
dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n)
f1 -= dae['prop_drag_vector'][0]
f2 -= dae['prop_drag_vector'][1]
f3 -= dae['prop_drag_vector'][2]
# if we are running a homotopy,
# add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 -
gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 -
gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 -
gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 -
gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 -
gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 -
gamma_homotopy)
# derivative of dcm
dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b'])
ddt_R_n2b = C.vertcat(\
[C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]),
C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]),
C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])])
# get mass matrix, rhs
(mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3)
# set up the residual
ode = C.veccat([
C.veccat([
dae.ddt('r_n2b_n_' + name) - dae['v_bn_n_' + name]
for name in ['x', 'y', 'z']
]),
C.veccat([ddt_R_n2b - dRexp]),
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps'],
dae.ddt('prop_drag') - dae['dprop_drag']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2) / 9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2) / 9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \
C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle']) * 180.0 / C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2 / 27.0 * rho * S * w**3 * cL**3 / cD**2
loyds2 = 2 / 27.0 * rho * S * w**3 * (cL**2 / cD**2 + 1)**(1.5) * cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
addLoydsLimit()
psuedo_zvec = C.veccat([dae.ddt(name) for name in \
['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(mass_matrix, psuedo_zvec) - rhs
dae.setResidual([ode, alg])
return dae
def crosswind_model(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ('nu')
dae.addX( ['r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z',
'e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33',
'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z',
'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z',
'aileron', 'elevator', 'rudder', 'flaps', 'prop_drag'
])
dae.addU( [ 'daileron'
, 'delevator'
, 'drudder'
, 'dflaps'
, 'dprop_drag'
] )
dae.addP( ['r','w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('v_bn_n_x')
dae['ddy'] = dae.ddt('v_bn_n_y')
dae['ddz'] = dae.ddt('v_bn_n_z')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['rudder_deg'] = dae['rudder']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
dae['drudder_deg_s'] = dae['drudder']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
C.horzcat([dae['e31'], dae['e32'], dae['e33']])])
# local wind
dae['wind_at_altitude'] = get_wind(dae, conf)
# wind velocity in NED
dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])
# body velocity in NED
dae['v_bn_n'] = C.veccat( [ dae['v_bn_n_x'] , dae['v_bn_n_y'], dae['v_bn_n_z'] ] )
# body velocity w.r.t. wind in NED
v_bw_n = dae['v_bn_n'] - dae['v_wn_n']
# body velocity w.r.t. wind in body frame
v_bw_b = C.mul( dae['R_n2b'], v_bw_n )
# compute aerodynamic forces and moments
(f1, f2, f3, t1, t2, t3) = \
aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']))
dae['prop_drag_vector'] = dae['prop_drag']*v_bw_n/dae['airspeed']
dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n)
f1 -= dae['prop_drag_vector'][0]
f2 -= dae['prop_drag_vector'][1]
f3 -= dae['prop_drag_vector'][2]
# if we are running a homotopy,
# add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1*gamma_homotopy + dae.addZ('f1_homotopy')*(1 - gamma_homotopy)
f2 = f2*gamma_homotopy + dae.addZ('f2_homotopy')*(1 - gamma_homotopy)
f3 = f3*gamma_homotopy + dae.addZ('f3_homotopy')*(1 - gamma_homotopy)
t1 = t1*gamma_homotopy + dae.addZ('t1_homotopy')*(1 - gamma_homotopy)
t2 = t2*gamma_homotopy + dae.addZ('t2_homotopy')*(1 - gamma_homotopy)
t3 = t3*gamma_homotopy + dae.addZ('t3_homotopy')*(1 - gamma_homotopy)
# derivative of dcm
dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b'])
ddt_R_n2b = C.vertcat(\
[C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]),
C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]),
C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])])
# get mass matrix, rhs
(mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3)
# set up the residual
ode = C.veccat([
C.veccat([dae.ddt('r_n2b_n_'+name) - dae['v_bn_n_'+name] for name in ['x','y','z']]),
C.veccat([ddt_R_n2b - dRexp]),
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps'],
dae.ddt('prop_drag') - dae['dprop_drag']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \
C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
addLoydsLimit()
psuedo_zvec = C.veccat([dae.ddt(name) for name in \
['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(mass_matrix, psuedo_zvec) - rhs
dae.setResidual( [ode, alg] )
return dae
def setupModel(dae, conf):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
zt = conf['zt']
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x']
y = dae['y']
z = dae['z']
dx = dae['dx']
dy = dae['dy']
dz = dae['dz']
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
# wind model
def getWind():
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((z+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
return dae['w0']*C.log((-z+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness)
elif conf['wind_model']['name'] == 'constant':
# constant wind
return dae['w0']
dae['wind_at_altitude'] = getWind()
# wind velocity in NED
dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])
# body velocity in NED
dae['v_bn_n'] = C.veccat( [ dx , dy, dz ] )
# body velocity w.r.t. wind in NED
v_bw_n = dae['v_bn_n'] - dae['v_wn_n']
# body velocity w.r.t. wind in body frame
v_bw_b = C.mul( dae['R_n2b'], v_bw_n )
# compute aerodynamic forces and moments
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23'])
)
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 - gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 - gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 - gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 - gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy)
# mass matrix
mm = C.SXMatrix(7, 7)
mm[0,0] = m
mm[0,1] = 0
mm[0,2] = 0
mm[0,3] = 0
mm[0,4] = 0
mm[0,5] = 0
mm[0,6] = x + zt*e31
mm[1,0] = 0
mm[1,1] = m
mm[1,2] = 0
mm[1,3] = 0
mm[1,4] = 0
mm[1,5] = 0
mm[1,6] = y + zt*e32
mm[2,0] = 0
mm[2,1] = 0
mm[2,2] = m
mm[2,3] = 0
mm[2,4] = 0
mm[2,5] = 0
mm[2,6] = z + zt*e33
mm[3,0] = 0
mm[3,1] = 0
mm[3,2] = 0
mm[3,3] = j1
mm[3,4] = 0
mm[3,5] = j31
mm[3,6] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[4,0] = 0
mm[4,1] = 0
mm[4,2] = 0
mm[4,3] = 0
mm[4,4] = j2
mm[4,5] = 0
mm[4,6] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[5,0] = 0
mm[5,1] = 0
mm[5,2] = 0
mm[5,3] = j31
mm[5,4] = 0
mm[5,5] = j3
mm[5,6] = 0
mm[6,0] = x + zt*e31
mm[6,1] = y + zt*e32
mm[6,2] = z + zt*e33
mm[6,3] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[6,4] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[6,5] = 0
mm[6,6] = 0
# right hand side
w1 = dae['w_bn_b_x']
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
zt2 = zt*zt
ddelta = 0
rA = 0
rhs = C.veccat(
[ f1 + ddelta*m*(dy + ddelta*rA + ddelta*x) + ddelta*dy*m
, f2 - ddelta*m*(dx - ddelta*y) - ddelta*dx*m
, f3 + g*m
, t1 - w2*(j3*w3 + j31*w1) + j2*w2*w3
, t2 + w1*(j3*w3 + j31*w1) - w3*(j1*w1 + j31*w3)
, t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2
, ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))
] )
dRexp = C.SXMatrix(3,3)
dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23)
dRexp[0,1] = e31*(w1 - ddelta*e13) - e11*(w3 - ddelta*e33)
dRexp[0,2] = e11*(w2 - ddelta*e23) - e21*(w1 - ddelta*e13)
dRexp[1,0] = e22*(w3 - ddelta*e33) - e32*(w2 - ddelta*e23)
dRexp[1,1] = e32*(w1 - ddelta*e13) - e12*(w3 - ddelta*e33)
dRexp[1,2] = e12*(w2 - ddelta*e23) - e22*(w1 - ddelta*e13)
dRexp[2,0] = e23*w3 - e33*w2
dRexp[2,1] = e33*w1 - e13*w3
dRexp[2,2] = e13*w2 - e23*w1
c =(x + zt*e31)**2/2 + (y + zt*e32)**2/2 + (z + zt*e33)**2/2 - r**2/2
cdot =dx*(x + zt*e31) + dy*(y + zt*e32) + dz*(z + zt*e33) + zt*(w2 - ddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(w1 - ddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - r*dr
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
dddelta = 0
cddot = -(w1-ddelta*e13)*(zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e21*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e22*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))+(w2-ddelta*e23)*(zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)+zt*e11*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e12*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))-ddr*r+(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)+dx*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+dy*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+dz*(dz+zt*e13*w2-zt*e23*w1)+ddx*(x+zt*e31)+ddy*(y+zt*e32)+ddz*(z+zt*e33)-dr*dr+zt*(dw2-dddelta*e23)*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)-zt*(dw1-dddelta*e13)*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33)-zt*dddelta*(e11*e23*x-e13*e21*x+e12*e23*y-e13*e22*y+zt*e11*e23*e31-zt*e13*e21*e31+zt*e12*e23*e32-zt*e13*e22*e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def setupModel(dae, conf):
# PARAMETERS OF THE KITE :
# ##############
m = conf['mass'] # mass of the kite # [ kg ]
# PHYSICAL CONSTANTS :
# ##############
g = conf['g'] # gravitational constant # [ m /s^2]
# PARAMETERS OF THE CABLE :
# ##############
#INERTIA MATRIX (Kurt's direct measurements)
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
#Carousel Friction & inertia
jCarousel = conf['jCarousel']
cfric = conf['cfric']
zt = conf['zt']
rA = conf['rArm']
########### model integ ###################
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x']
y = dae['y']
z = dae['z']
dx = dae['dx']
dy = dae['dy']
dz = dae['dz']
w1 = dae['w1']
w2 = dae['w2']
w3 = dae['w3']
ddelta = dae['ddelta']
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
tc = dae['motor_torque'] #Carousel motor torque
# wind
if 'w0' in dae:
z0 = conf['z0']
zt_roughness = conf['zt_roughness']
zsat = 0.5*(z+C.sqrt(z*z))
wind_x = dae['w0']*C.log((zsat+zt_roughness+2)/zt_roughness)/C.log(z0/zt_roughness)
else:
wind_x = 0
dae['wind at altitude'] = wind_x
dp_carousel_frame = C.veccat( [ dx - ddelta*y
, dy + ddelta*(rA + x)
, dz
]) - C.veccat([dae['cos_delta']*wind_x, dae['sin_delta']*wind_x, 0])
R_c2b = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33']]
).reshape((3,3))
# Aircraft velocity w.r.t. inertial frame, given in its own reference frame
# (needed to compute the aero forces and torques !)
dpE = C.mul( R_c2b, dp_carousel_frame )
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, dp_carousel_frame, dpE,
(dae['w1'], dae['w2'], dae['w3']),
(dae['e21'], dae['e22'], dae['e23']),
(dae['aileron'],dae['elevator'])
)
# mass matrix
mm = C.SXMatrix(8, 8)
mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x
mm[0,1] = -m*y
mm[0,2] = m*(rA + x)
mm[0,3] = 0
mm[0,4] = 0
mm[0,5] = 0
mm[0,6] = 0
mm[0,7] = 0
mm[1,0] = -m*y
mm[1,1] = m
mm[1,2] = 0
mm[1,3] = 0
mm[1,4] = 0
mm[1,5] = 0
mm[1,6] = 0
mm[1,7] = x + zt*e31
mm[2,0] = m*(rA + x)
mm[2,1] = 0
mm[2,2] = m
mm[2,3] = 0
mm[2,4] = 0
mm[2,5] = 0
mm[2,6] = 0
mm[2,7] = y + zt*e32
mm[3,0] = 0
mm[3,1] = 0
mm[3,2] = 0
mm[3,3] = m
mm[3,4] = 0
mm[3,5] = 0
mm[3,6] = 0
mm[3,7] = z + zt*e33
mm[4,0] = 0
mm[4,1] = 0
mm[4,2] = 0
mm[4,3] = 0
mm[4,4] = j1
mm[4,5] = 0
mm[4,6] = j31
mm[4,7] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[5,0] = 0
mm[5,1] = 0
mm[5,2] = 0
mm[5,3] = 0
mm[5,4] = 0
mm[5,5] = j2
mm[5,6] = 0
mm[5,7] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[6,0] = 0
mm[6,1] = 0
mm[6,2] = 0
mm[6,3] = 0
mm[6,4] = j31
mm[6,5] = 0
mm[6,6] = j3
mm[6,7] = 0
mm[7,0] = -zt*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
mm[7,1] = x + zt*e31
mm[7,2] = y + zt*e32
mm[7,3] = z + zt*e33
mm[7,4] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[7,5] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[7,6] = 0
mm[7,7] = 0
# right hand side
zt2 = zt*zt
rhs = C.veccat(
[ tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x)
, f1 + ddelta*m*(dy + ddelta*rA + ddelta*x) + ddelta*dy*m
, f2 - ddelta*m*(dx - ddelta*y) - ddelta*dx*m
, f3 - g*m
, t1 - w2*(j3*w3 + j31*w1) + j2*w2*w3
, t2 + w1*(j3*w3 + j31*w1) - w3*(j1*w1 + j31*w3)
, t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2
, ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))
] )
dRexp = C.SXMatrix(3,3)
dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23)
dRexp[0,1] = e31*(w1 - ddelta*e13) - e11*(w3 - ddelta*e33)
dRexp[0,2] = e11*(w2 - ddelta*e23) - e21*(w1 - ddelta*e13)
dRexp[1,0] = e22*(w3 - ddelta*e33) - e32*(w2 - ddelta*e23)
dRexp[1,1] = e32*(w1 - ddelta*e13) - e12*(w3 - ddelta*e33)
dRexp[1,2] = e12*(w2 - ddelta*e23) - e22*(w1 - ddelta*e13)
dRexp[2,0] = e23*w3 - e33*w2
dRexp[2,1] = e33*w1 - e13*w3
dRexp[2,2] = e13*w2 - e23*w1
c =(x + zt*e31)**2/2 + (y + zt*e32)**2/2 + (z + zt*e33)**2/2 - r**2/2
cdot =dx*(x + zt*e31) + dy*(y + zt*e32) + dz*(z + zt*e33) + zt*(w2 - ddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(w1 - ddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - r*dr
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
# dddelta = dae['dddelta']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w1')
dw2 = dae.ddt('w2')
dddelta = dae.ddt('ddelta')
cddot = -(w1-ddelta*e13)*(zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e21*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e22*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))+(w2-ddelta*e23)*(zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)+zt*e11*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e12*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))-ddr*r+(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)+dx*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+dy*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+dz*(dz+zt*e13*w2-zt*e23*w1)+ddx*(x+zt*e31)+ddy*(y+zt*e32)+ddz*(z+zt*e33)-dr*dr+zt*(dw2-dddelta*e23)*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)-zt*(dw1-dddelta*e13)*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33)-zt*dddelta*(e11*e23*x-e13*e21*x+e12*e23*y-e13*e22*y+zt*e11*e23*e31-zt*e13*e21*e31+zt*e12*e23*e32-zt*e13*e22*e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def setupModel(dae, conf):
# PARAMETERS OF THE KITE :
# ##############
m = conf['mass'] # mass of the kite # [ kg ]
# PHYSICAL CONSTANTS :
# ##############
g = conf['g'] # gravitational constant # [ m /s^2]
# PARAMETERS OF THE CABLE :
# ##############
#INERTIA MATRIX (Kurt's direct measurements)
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
#Carousel Friction & inertia
zt = conf['zt']
########### model integ ###################
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x']
y = dae['y']
z = dae['z']
dx = dae['dx']
dy = dae['dy']
dz = dae['dz']
w1 = dae['w1']
w2 = dae['w2']
w3 = dae['w3']
r = dae['r']
# wind
def getWind():
z0 = conf['z0']
zt_roughness = conf['zt_roughness']
zsat = 0.5*(z+C.sqrt(z*z))
wind_x = dae['w0']*C.log((zsat+zt_roughness+2)/zt_roughness)/C.log(z0/zt_roughness)
# wind_x = dae['w0']
return wind_x
dae['wind_at_altitude'] = getWind()
v_bw_n = C.veccat( [ dx - dae['wind_at_altitude'], dy, dz ] )
R_n2b = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33']]
).reshape((3,3))
# Aircraft velocity w.r.t. inertial frame, given in its own reference frame
# (needed to compute the aero forces and torques !)
v_bw_b = C.mul( R_n2b, v_bw_n )
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
(dae['w1'], dae['w2'], dae['w3']),
(dae['e21'], dae['e22'], dae['e23']),
(dae['aileron'],dae['elevator'])
)
dae['prop_drag_vector'] = dae['prop_drag']*v_bw_n/dae['airspeed']
dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n)
f1 -= dae['prop_drag_vector'][0]
f2 -= dae['prop_drag_vector'][1]
f3 -= dae['prop_drag_vector'][2]
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 - gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 - gamma_homotopy)
f3 = f3 * gamma_homotopy + (dae.addZ('f3_homotopy') - g*m) * (1 - gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 - gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy)
# mass matrix
mm = C.SXMatrix(7, 7)
mm[0,0] = m
mm[0,1] = 0
mm[0,2] = 0
mm[0,3] = 0
mm[0,4] = 0
mm[0,5] = 0
mm[0,6] = x + zt*e31
mm[1,0] = 0
mm[1,1] = m
mm[1,2] = 0
mm[1,3] = 0
mm[1,4] = 0
mm[1,5] = 0
mm[1,6] = y + zt*e32
mm[2,0] = 0
mm[2,1] = 0
mm[2,2] = m
mm[2,3] = 0
mm[2,4] = 0
mm[2,5] = 0
mm[2,6] = z + zt*e33
mm[3,0] = 0
mm[3,1] = 0
mm[3,2] = 0
mm[3,3] = j1
mm[3,4] = 0
mm[3,5] = j31
mm[3,6] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[4,0] = 0
mm[4,1] = 0
mm[4,2] = 0
mm[4,3] = 0
mm[4,4] = j2
mm[4,5] = 0
mm[4,6] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[5,0] = 0
mm[5,1] = 0
mm[5,2] = 0
mm[5,3] = j31
mm[5,4] = 0
mm[5,5] = j3
mm[5,6] = 0
mm[6,0] = x + zt*e31
mm[6,1] = y + zt*e32
mm[6,2] = z + zt*e33
mm[6,3] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[6,4] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[6,5] = 0
mm[6,6] = 0
# right hand side
zt2 = zt*zt
ddelta = 0
rA = 0
dr = 0
ddr = 0
rhs = C.veccat(
[ f1 + ddelta*m*(dy + ddelta*rA + ddelta*x) + ddelta*dy*m
, f2 - ddelta*m*(dx - ddelta*y) - ddelta*dx*m
, f3 - g*m
, t1 - w2*(j3*w3 + j31*w1) + j2*w2*w3
, t2 + w1*(j3*w3 + j31*w1) - w3*(j1*w1 + j31*w3)
, t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2
, ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))
] )
dRexp = C.SXMatrix(3,3)
dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23)
dRexp[0,1] = e31*(w1 - ddelta*e13) - e11*(w3 - ddelta*e33)
dRexp[0,2] = e11*(w2 - ddelta*e23) - e21*(w1 - ddelta*e13)
dRexp[1,0] = e22*(w3 - ddelta*e33) - e32*(w2 - ddelta*e23)
dRexp[1,1] = e32*(w1 - ddelta*e13) - e12*(w3 - ddelta*e33)
dRexp[1,2] = e12*(w2 - ddelta*e23) - e22*(w1 - ddelta*e13)
dRexp[2,0] = e23*w3 - e33*w2
dRexp[2,1] = e33*w1 - e13*w3
dRexp[2,2] = e13*w2 - e23*w1
c =(x + zt*e31)**2/2 + (y + zt*e32)**2/2 + (z + zt*e33)**2/2 - r**2/2
cdot =dx*(x + zt*e31) + dy*(y + zt*e32) + dz*(z + zt*e33) + zt*(w2 - ddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(w1 - ddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - r*dr
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w1')
dw2 = dae.ddt('w2')
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
dddelta = 0
cddot = -(w1-ddelta*e13)*(zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e21*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e22*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))+(w2-ddelta*e23)*(zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)+zt*e11*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e12*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))-ddr*r+(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)+dx*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+dy*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+dz*(dz+zt*e13*w2-zt*e23*w1)+ddx*(x+zt*e31)+ddy*(y+zt*e32)+ddz*(z+zt*e33)-dr*dr+zt*(dw2-dddelta*e23)*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)-zt*(dw1-dddelta*e13)*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33)-zt*dddelta*(e11*e23*x-e13*e21*x+e12*e23*y-e13*e22*y+zt*e11*e23*e31-zt*e13*e21*e31+zt*e12*e23*e32-zt*e13*e22*e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def setupModel(dae, conf):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
jCarousel = conf['jCarousel']
cfric = conf['cfric']
zt = conf['zt']
rA = conf['rArm']
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x']
y = dae['y']
z = dae['z']
dx = dae['dx']
dy = dae['dy']
dz = dae['dz']
w1 = dae['w_bn_b_x']
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
ddelta = dae['ddelta']
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
tc = dae['motor_torque'] #Carousel motor torque
# wind model
def getWind():
if 'wind_model' not in conf:
return 0
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((z+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
zsat = 0.5*(-z+C.sqrt(z*z))
return dae['w0']*C.log((zsat+zt_roughness+2)/zt_roughness)/C.log(z0/zt_roughness)
elif conf['wind_model']['name'] == 'constant':
# constant wind
return dae['w0']
wind_x = getWind()
dae['wind_at_altitude'] = wind_x
dp_carousel_frame = C.veccat( [ dx - ddelta*y
, dy + ddelta*(rA + x)
, dz
]) - C.veccat([dae['cos_delta']*wind_x, dae['sin_delta']*wind_x, 0])
# Aircraft velocity w.r.t. inertial frame, given in its own reference frame
# (needed to compute the aero forces and torques !)
dpE = C.mul( dae['R_c2b'], dp_carousel_frame )
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, dp_carousel_frame, dpE,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23'])
)
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 - gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 - gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 - gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 - gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy)
# mass matrix
mm = C.SXMatrix(8, 8)
mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x
mm[0,1] = -m*y
mm[0,2] = m*(rA + x)
mm[0,3] = 0
mm[0,4] = 0
mm[0,5] = 0
mm[0,6] = 0
mm[0,7] = 0
mm[1,0] = -m*y
mm[1,1] = m
mm[1,2] = 0
mm[1,3] = 0
mm[1,4] = 0
mm[1,5] = 0
mm[1,6] = 0
mm[1,7] = x + zt*e31
mm[2,0] = m*(rA + x)
mm[2,1] = 0
mm[2,2] = m
mm[2,3] = 0
mm[2,4] = 0
mm[2,5] = 0
mm[2,6] = 0
mm[2,7] = y + zt*e32
mm[3,0] = 0
mm[3,1] = 0
mm[3,2] = 0
mm[3,3] = m
mm[3,4] = 0
mm[3,5] = 0
mm[3,6] = 0
mm[3,7] = z + zt*e33
mm[4,0] = 0
mm[4,1] = 0
mm[4,2] = 0
mm[4,3] = 0
mm[4,4] = j1
mm[4,5] = 0
mm[4,6] = j31
mm[4,7] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[5,0] = 0
mm[5,1] = 0
mm[5,2] = 0
mm[5,3] = 0
mm[5,4] = 0
mm[5,5] = j2
mm[5,6] = 0
mm[5,7] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[6,0] = 0
mm[6,1] = 0
mm[6,2] = 0
mm[6,3] = 0
mm[6,4] = j31
mm[6,5] = 0
mm[6,6] = j3
mm[6,7] = 0
mm[7,0] = -zt*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
mm[7,1] = x + zt*e31
mm[7,2] = y + zt*e32
mm[7,3] = z + zt*e33
mm[7,4] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[7,5] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[7,6] = 0
mm[7,7] = 0
# right hand side
zt2 = zt*zt
rhs = C.veccat(
[ tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x)
, f1 + ddelta*m*(dy + ddelta*rA + ddelta*x) + ddelta*dy*m
, f2 - ddelta*m*(dx - ddelta*y) - ddelta*dx*m
, f3 + g*m
, t1 - w2*(j3*w3 + j31*w1) + j2*w2*w3
, t2 + w1*(j3*w3 + j31*w1) - w3*(j1*w1 + j31*w3)
, t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2
, ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))
] )
dRexp = C.SXMatrix(3,3)
dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23)
dRexp[0,1] = e31*(w1 - ddelta*e13) - e11*(w3 - ddelta*e33)
dRexp[0,2] = e11*(w2 - ddelta*e23) - e21*(w1 - ddelta*e13)
dRexp[1,0] = e22*(w3 - ddelta*e33) - e32*(w2 - ddelta*e23)
dRexp[1,1] = e32*(w1 - ddelta*e13) - e12*(w3 - ddelta*e33)
dRexp[1,2] = e12*(w2 - ddelta*e23) - e22*(w1 - ddelta*e13)
dRexp[2,0] = e23*w3 - e33*w2
dRexp[2,1] = e33*w1 - e13*w3
dRexp[2,2] = e13*w2 - e23*w1
c =(x + zt*e31)**2/2 + (y + zt*e32)**2/2 + (z + zt*e33)**2/2 - r**2/2
cdot =dx*(x + zt*e31) + dy*(y + zt*e32) + dz*(z + zt*e33) + zt*(w2 - ddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(w1 - ddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - r*dr
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
# dddelta = dae['dddelta']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
dddelta = dae.ddt('ddelta')
cddot = -(w1-ddelta*e13)*(zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e21*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e22*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))+(w2-ddelta*e23)*(zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)+zt*e11*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e12*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))-ddr*r+(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)+dx*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+dy*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+dz*(dz+zt*e13*w2-zt*e23*w1)+ddx*(x+zt*e31)+ddy*(y+zt*e32)+ddz*(z+zt*e33)-dr*dr+zt*(dw2-dddelta*e23)*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)-zt*(dw1-dddelta*e13)*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33)-zt*dddelta*(e11*e23*x-e13*e21*x+e12*e23*y-e13*e22*y+zt*e11*e23*e31-zt*e13*e21*e31+zt*e12*e23*e32-zt*e13*e22*e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def setupModel(dae, conf):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
zt = conf['zt']
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x']
y = dae['y']
z = dae['z']
dx = dae['dx']
dy = dae['dy']
dz = dae['dz']
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
# wind model
def getWind():
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((z+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
return dae['w0'] * C.log(
(-z + zt_roughness) / zt_roughness) / C.log(z0 / zt_roughness)
elif conf['wind_model']['name'] == 'constant':
# constant wind
return dae['w0']
dae['wind_at_altitude'] = getWind()
# wind velocity in NED
dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])
# body velocity in NED
dae['v_bn_n'] = C.veccat([dx, dy, dz])
# body velocity w.r.t. wind in NED
v_bw_n = dae['v_bn_n'] - dae['v_wn_n']
# body velocity w.r.t. wind in body frame
v_bw_b = C.mul(dae['R_n2b'], v_bw_n)
# compute aerodynamic forces and moments
(f1, f2, f3, t1, t2,
t3) = aeroForcesTorques(dae, conf, v_bw_n, v_bw_b, dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']))
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 -
gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 -
gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 -
gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 -
gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 -
gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 -
gamma_homotopy)
# mass matrix
mm = C.SXMatrix(7, 7)
mm[0, 0] = m
mm[0, 1] = 0
mm[0, 2] = 0
mm[0, 3] = 0
mm[0, 4] = 0
mm[0, 5] = 0
mm[0, 6] = x + zt * e31
mm[1, 0] = 0
mm[1, 1] = m
mm[1, 2] = 0
mm[1, 3] = 0
mm[1, 4] = 0
mm[1, 5] = 0
mm[1, 6] = y + zt * e32
mm[2, 0] = 0
mm[2, 1] = 0
mm[2, 2] = m
mm[2, 3] = 0
mm[2, 4] = 0
mm[2, 5] = 0
mm[2, 6] = z + zt * e33
mm[3, 0] = 0
mm[3, 1] = 0
mm[3, 2] = 0
mm[3, 3] = j1
mm[3, 4] = 0
mm[3, 5] = j31
mm[3, 6] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)
mm[4, 0] = 0
mm[4, 1] = 0
mm[4, 2] = 0
mm[4, 3] = 0
mm[4, 4] = j2
mm[4, 5] = 0
mm[4, 6] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33)
mm[5, 0] = 0
mm[5, 1] = 0
mm[5, 2] = 0
mm[5, 3] = j31
mm[5, 4] = 0
mm[5, 5] = j3
mm[5, 6] = 0
mm[6, 0] = x + zt * e31
mm[6, 1] = y + zt * e32
mm[6, 2] = z + zt * e33
mm[6, 3] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)
mm[6, 4] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33)
mm[6, 5] = 0
mm[6, 6] = 0
# right hand side
w1 = dae['w_bn_b_x']
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
zt2 = zt * zt
ddelta = 0
rA = 0
rhs = C.veccat([
f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) + ddelta * dy * m,
f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m, f3 + g * m,
t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3,
t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3),
t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2,
ddr * r - (zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33) + zt * w2 *
(e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)) * (w3 - ddelta * e33) -
dx * (dx - zt * e21 * (w1 - ddelta * e13) + zt * e11 *
(w2 - ddelta * e23)) - dy * (dy - zt * e22 *
(w1 - ddelta * e13) + zt * e12 *
(w2 - ddelta * e23)) - dz *
(dz - zt * e23 * (w1 - ddelta * e13) + zt * e13 *
(w2 - ddelta * e23)) + dr * dr + (w1 - ddelta * e13) *
(e21 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 *
(w2 - ddelta * e23)) + e22 *
(zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 *
(w2 - ddelta * e23)) + zt * e23 *
(dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 *
(w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y +
zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32) + zt * e31 *
(x + zt * e31) * (w1 - ddelta * e13) + zt * e32 * (y + zt * e32) *
(w1 - ddelta * e13)) - (w2 - ddelta * e23) *
(e11 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 *
(w2 - ddelta * e23)) + e12 *
(zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 *
(w2 - ddelta * e23)) + zt * e13 *
(dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 *
(w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y +
zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32) - zt * e31 *
(x + zt * e31) * (w2 - ddelta * e23) - zt * e32 * (y + zt * e32) *
(w2 - ddelta * e23))
])
dRexp = C.SXMatrix(3, 3)
dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23)
dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33)
dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13)
dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23)
dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33)
dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13)
dRexp[2, 0] = e23 * w3 - e33 * w2
dRexp[2, 1] = e33 * w1 - e13 * w3
dRexp[2, 2] = e13 * w2 - e23 * w1
c = (x + zt * e31)**2 / 2 + (y + zt * e32)**2 / 2 + (
z + zt * e33)**2 / 2 - r**2 / 2
cdot = dx * (x + zt * e31) + dy * (y + zt * e32) + dz * (
z + zt * e33) + zt * (w2 - ddelta * e23) * (
e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 +
zt * e13 * e33) - zt * (w1 - ddelta * e13) * (
e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 +
zt * e23 * e33) - r * dr
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
dddelta = 0
cddot = -(w1 - ddelta * e13) * (
zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 *
(w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y +
zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32) + zt * e21 *
(dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + zt * e22 *
(dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 +
zt * ddelta * e13 * e22) + zt * e31 * (x + zt * e31) *
(w1 - ddelta * e13) + zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
) + (w2 - ddelta * e23) * (
zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 *
(w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y +
zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32) + zt * e11 *
(dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + zt * e12 *
(dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 +
zt * ddelta * e13 * e22) - zt * e31 * (x + zt * e31) *
(w2 - ddelta * e23) - zt * e32 * (y + zt * e32) *
(w2 - ddelta * e23)) - ddr * r + (
zt * w1 *
(e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 +
zt * e13 * e33) + zt * w2 *
(e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 +
zt * e23 * e33)) * (w3 - ddelta * e33) + dx * (
dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + dy * (
dy + zt * e12 * w2 - zt * e22 * w1 -
zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22
) + dz * (dz + zt * e13 * w2 - zt * e23 * w1) + ddx * (
x + zt * e31) + ddy * (y + zt * e32) + ddz * (
z +
zt * e33) - dr * dr + zt * (dw2 - dddelta * e23) * (
e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 +
zt * e13 * e33) - zt * (dw1 - dddelta * e13) * (
e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 +
zt * e23 * e33) - zt * dddelta * (
e11 * e23 * x - e13 * e21 * x +
e12 * e23 * y - e13 * e22 * y + zt * e11 *
e23 * e31 - zt * e13 * e21 * e31 + zt *
e12 * e23 * e32 - zt * e13 * e22 * e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
