def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
'''
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
'''
R_b2n = dae['R_n2b'].T
r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
r_b2bridle_b = C.veccat([0,0,conf['zt']])
v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])
r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)
mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
mm01 = C.SXMatrix(3,3)
mm10 = mm01.T
mm02 = r_n2bridle_n
mm20 = mm02.T
J = C.blockcat([[ conf['j1'], 0, conf['j31']],
[ 0, conf['j2'], 0],
[conf['j31'], 0, conf['j3']]])
mm11 = J
mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
mm21 = mm12.T
mm22 = C.SXMatrix(1,1)
mm = C.blockcat([[mm00,mm01,mm02],
[mm10,mm11,mm12],
[mm20,mm21,mm22]])
# right hand side
rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))
# last element of RHS
R_n2b = dae['R_n2b']
w_bn_b = dae['w_bn_b']
grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']
rhs = C.veccat([rhs0,rhs1,rhs2])
c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']
a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
dae['dr']**2 - dae['r']*dae['ddr']
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs)
def U_upper_bound(self, robot):
ones = c.DM.ones(robot.nu, self.T)
ub = c.blockcat([[robot.vel_max * ones[0, :]],
[robot.ang_vel_max * ones[1, :]]])
ub = c.reshape(ub, robot.nu * self.T, 1)
return ub
def kinematics(self, state, U, epsilon=0):
f,_,_ = self.proc_model()
w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0]))
w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1]))
w = c.DM([[w0],[w1]])
#state_n = state + self.dt*c.blockcat([[vx],[vy],[vtheta]])
state_n = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))
# state_n = c.MX(self.nx,1)
# state_n[0] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[0]
# state_n[1] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[1]
# state_n[2] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[2]
# state_n[3] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[3]
state_n[2] = c.atan2(c.sin(state_n[2]),c.cos(state_n[2]))
state_n[4] = c.atan2(c.sin(state_n[4]),c.cos(state_n[4]))
state_n[5] = c.atan2(c.sin(state_n[5]),c.cos(state_n[5]))
return state_n
def U_lower_bound(self, robot):
ones = c.DM.ones(self.N * self.T)
lb = c.blockcat([[-robot.vel_max * ones], [-robot.vel_max * ones],
[-robot.ang_vel_max * ones]])
lb = c.reshape(lb, robot.nu * self.N * self.T, 1)
return lb
def sqrt_correct(Rs, H, W):
"""
source: Fast Stable Kalman Filter Algorithms Utilising the Square Root, Steward 98
Rs: sqrt(R)
H: measurement matrix
W: sqrt(P)
@return:
Wp: sqrt(P+) = sqrt((I - KH)P)
K: Kalman gain
Ss: Innovation variance
"""
n_x = H.shape[1]
n_y = H.shape[0]
B = ca.sparsify(ca.blockcat(Rs, ca.mtimes(H, W), ca.SX.zeros(n_x, n_y), W))
# qr by default is upper triangular, so we transpose inputs and outputs
B_Q, B_R = ca.qr(B.T) # B_Q orthogonal, B_R, lower triangular
B_Q = B_Q.T
B_R = B_R.T
Wp = B_R[n_y:, n_y:]
Ss = B_R[:n_y, :n_y]
P_HT_SsInv = B_R[n_y:, :n_y]
K = ca.mtimes(P_HT_SsInv, ca.inv(Ss))
return Wp, K, Ss
def state_contraints(self, robot, U):
constraintVar = c.MX(
2 * (robot.nx - 1) * self.N,
self.T) #skipping orientation. 2* to include min and max
U = c.reshape(U, robot.nu * self.N, self.T)
X = c.MX(robot.nx * self.N, self.T + 1)
X[:, 0] = np.reshape(self.X0, (robot.nx * self.N, ))
for i in range(self.T):
for n in range(self.N):
X[robot.nx * n:robot.nx * (n + 1), i + 1] = robot.kinematics(
X[robot.nx * n:robot.nx * (n + 1), i], U[robot.nu * n, i],
U[robot.nu * n + 1, i], U[robot.nu * n + 2, i])
constraintVar[2 * (robot.nx - 1) * n:2 * (robot.nx - 1) *
(n + 1), i] = c.blockcat(
[[
self.Xmax - X[robot.nx * n:robot.nx *
(n + 1) - 1, i + 1]
],
[
X[robot.nx * n:robot.nx *
(n + 1) - 1, i + 1] - self.Xmin
]])
constraintVar = c.reshape(constraintVar, 1,
2 * (robot.nx - 1) * self.N * self.T)
return constraintVar
def U_upper_bound(self, robot):
ones = c.DM.ones(robot.nu, self.T)
ub = c.blockcat([[robot.thrust_max * ones[0, :]],
[robot.torque_max * ones[1, :]],
[robot.torque_max * ones[2, :]],
[robot.torque_max * ones[3, :]]])
ub = c.reshape(ub, robot.nu * self.T, 1)
return ub
def dynamics_rear(X_prev, S, v):
dt = 0.1
L = 2.33
X_new = X_prev + dt*casadi.blockcat([[casadi.mtimes(v/2.0,casadi.cos(X_prev[2] - S - 8*m.pi/180)+casadi.cos(X_prev[2] + (S + 8*m.pi/180)))],
[casadi.mtimes(v/2.0,casadi.sin(X_prev[2] - S - 8*m.pi/180) + casadi.sin(X_prev[2] + (S + 8*m.pi/180)))],
[casadi.mtimes(v,casadi.sin(-S - 8*m.pi/180)*1/L)]])
X_new[2] = casadi.atan2(casadi.sin(X_new[2]), casadi.cos(X_new[2]))
return X_new
def U_lower_bound(self, robot):
ones = c.DM.ones(robot.nu, self.T)
lb = c.blockcat([[robot.thrust_min * ones[0, :]],
[-robot.torque_max * ones[1, :]],
[-robot.torque_max * ones[2, :]],
[-robot.torque_max * ones[3, :]]])
lb = c.reshape(lb, robot.nu * self.T, 1)
return lb
def skew_mat(xyz):
'''
return the skew symmetrix matrix of a 3-vector
'''
x = xyz[0]
y = xyz[1]
z = xyz[2]
return C.blockcat([[ 0, -z, y],
[ z, 0, -x],
[-y, x, 0]])
def state_contraints(self, robot, U):
constraintVar = c.MX(
2 * 2, self.T
) #skipping orientation & steer angle. 2* to include min and max
U = c.reshape(U, robot.nu, self.T)
X = c.MX(robot.nx, self.T + 1)
X[:, 0] = self.X0
for i in range(self.T):
X[:, i + 1] = robot.kinematics(X[:, i], U[:, i])
constraintVar[:, i] = c.blockcat([[self.Xmax - X[0:2, i + 1]],
[X[0:2, i + 1] - self.Xmin]])
constraintVar = c.reshape(constraintVar, 1, 2 * 2 * self.T)
return constraintVar
def kinematics(self, state, U, epsilon=0):
f,_,_ = self.proc_model()
w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0]))
w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1]))
w2 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[2,2]))
w3 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[3,3]))
w = c.DM([[w0],[w1],[w2],[w3]])
state_n = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]],[U[2] + w[2]],[U[3] + w[3]]]))
state_n[6] = c.atan2(c.sin(state_n[6]),c.cos(state_n[6]))
state_n[7] = c.atan2(c.sin(state_n[7]),c.cos(state_n[7]))
state_n[8] = c.atan2(c.sin(state_n[8]),c.cos(state_n[8]))
return state_n
def kinematics(self, state, vx, vy, vtheta, epsilon=0):
f,_,_ = self.proc_model()
#vx = vx + epsilon*self.vel_max*np.random.normal(0,1)
#vy = vy + epsilon*self.vel_max*np.random.normal(0,1)
#vtheta = vtheta + epsilon*self.ang_vel_max*np.random.normal(0,1)
w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0]))
w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1]))
w2 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[2,2]))
w = c.DM([[w0],[w1],[w2]])
#state_n = state + self.dt*c.blockcat([[vx],[vy],[vtheta]])
state_n = f(state,c.blockcat([[vx],[vy],[vtheta]])) + c.mtimes(self.G,w)
state_n[2] = c.atan2(c.sin(state_n[2]),c.cos(state_n[2]))
return state_n
def update(X, U):
"""
X contains [x,y,theta] in a column
U contains [v,s] in a column
"""
x = X[0]
y = X[1]
theta = X[2]
v = U[0]
s = U[1]
dt = 0.1 #update time
L, _ = robot_params()
#kinematics
x_new = x + v * casadi.cos(theta) * dt
y_new = y + v * casadi.sin(theta) * dt
theta_new = theta + ((v * casadi.tan(s)) / L) * dt
#lets make it into a casadi thing
X_new = casadi.blockcat([[x_new], [y_new], [theta_new]])
return X_new
def skew( w ):
x = w[0]
y = w[1]
z = w[2]
return C.blockcat([[0,-z,y],[z,0,-x],[-y,x,0]])
custom_hess_u = W_u
J = horzcat(SX.eye(2), SX(2, 2))
print(DM(J.sparsity()))
# diagonal matrix with second order terms of outer loss function.
D = SX.sym('D', Sparsity.diag(2))
D[0, 0] = 1
[hess_tan, grad_tan] = hessian(tanh(theta)**2, theta)
D[1, 1] = if_else(theta == 0, hess_tan, grad_tan / theta)
custom_hess_x = J.T @ D @ J
zeros = SX(1, nx)
cost_expr_ext_cost_custom_hess = blockcat(custom_hess_u, zeros, zeros.T,
custom_hess_x)
cost_expr_ext_cost_custom_hess_e = custom_hess_x
ocp.model.cost_expr_ext_cost_custom_hess = cost_expr_ext_cost_custom_hess
ocp.model.cost_expr_ext_cost_custom_hess_e = cost_expr_ext_cost_custom_hess_e
# set constraints
Fmax = 35
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.idxbu = np.array([0])
x0 = np.array([0.0, np.pi, 0.0, 0.0])
xf = np.array([0.0, 0.0, 0.0, 0.0])
ocp.constraints.x0 = x0
# ocp.constraints.x0 = np.array([0.0, 0.001, 0.0, 0.0])
dae['L_over_D'] = cL / cD
cD = dae['cD'] + dae['cD_tether']
dae['L_over_D_with_tether'] = cL / cD
######## moment coefficients #######
# offset
dae['momentCoeffs0'] = C.DMatrix([0, conf['cm0'], 0])
# with roll rates
# non-dimensionalized angular velocity
w_bn_b_hat = C.veccat([conf['bref'], conf['cref'], conf['bref']
]) * 0.5 / dae['airspeed'] * w_bn_b
momentCoeffs_pqr = C.mul(
C.blockcat([[conf['cl_p'], conf['cl_q'], conf['cl_r']],
[conf['cm_p'], conf['cm_q'], conf['cm_r']],
[conf['cn_p'], conf['cn_q'], conf['cn_r']]]), w_bn_b_hat)
dae['momentCoeffs_pqr'] = momentCoeffs_pqr
# with alpha beta
momentCoeffs_AB = C.mul(
C.blockcat([[0, conf['cl_B'], conf['cl_AB']], [conf['cm_A'], 0, 0],
[0, conf['cn_B'], conf['cn_AB']]]),
C.vertcat([alpha, beta, alpha * beta]))
dae['momentCoeffs_AB'] = momentCoeffs_AB
# with control surfaces
momentCoeffs_surf = C.SXMatrix(3, 1, 0)
momentCoeffs_surf[0] += conf['cl_ail'] * dae['aileron']
momentCoeffs_surf[1] += conf['cm_elev'] * dae['elevator']
if 'flaps' in dae:
def compute_covariance_matrix(self):
r'''
This function computes the covariance matrix of the estimated
parameters from the inverse of the KKT matrix for the
parameter estimation problem. This allows then for statements on the
quality of the values of the estimated parameters.
For efficiency, only the inverse of the relevant part of the matrix
is computed using the Schur complement.
A more detailed description of this function will follow in future
versions.
'''
intro.pecas_intro()
print('\n' + 20 * '-' + \
' PECas covariance matrix computation ' + 21 * '-')
print('''
Computing the covariance matrix for the estimated parameters,
this might take some time ...
''')
self.tstart_cov_computation = time.time()
try:
N1 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
self.w.shape[0])
N2 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
self.Vars.shape[0] - self.w.shape[0])
hess = ca.blockcat([
[N2, N1],
[N1.T, ca.diag(self.w)],
])
# hess = hess + 1e-10 * ca.diag(self.Vars)
# J2 can be re-used from parameter estimation, right?
J2 = ca.jacobian(self.g, self.Vars)
kkt = ca.blockcat( \
[[hess, \
J2.T], \
[J2, \
ca.MX(self.g.size1(), self.g.size1())]] \
)
B1 = kkt[:self.pesetup.np, :self.pesetup.np]
E = kkt[self.pesetup.np:, :self.pesetup.np]
D = kkt[self.pesetup.np:, self.pesetup.np:]
Dinv = ca.solve(D, E, "csparse")
F11 = B1 - ca.mul([E.T, Dinv])
self.fbeta = ca.MXFunction("fbeta", [self.Vars],
[ca.mul([self.R.T, self.R]) / \
(self.yN.size + self.g.size1() - self.Vars.size())])
[self.beta] = self.fbeta([self.Varshat])
self.fcovp = ca.MXFunction("fcovp", [self.Vars], \
[self.beta * ca.solve(F11, ca.MX.eye(F11.size1()))])
[self.Covp] = self.fcovp([self.Varshat])
print( \
'''Covariance matrix computation finished, run show_results() to visualize.''')
except AttributeError as err:
errmsg = '''
You must execute run_parameter_estimation() first before the covariance
matrix for the estimated parameters can be computed.
'''
raise AttributeError(errmsg)
finally:
self.tend_cov_computation = time.time()
self.duration_cov_computation = self.tend_cov_computation - \
self.tstart_cov_computation
def skew(a, b, c):
" creates skew-symmetric matrix"
d = ca.blockcat([[0., -c, b], [c, 0., -a], [-b, a, 0.]])
return d
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',
'r', 'dr', 'ddr',
'aileron', 'elevator', 'rudder', 'flaps'
])
dae.addU( [ 'daileron'
, 'delevator'
, 'drudder'
, 'dflaps'
, 'dddr'
] )
dae.addP( ['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']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_n2b'] = C.blockcat([[dae['e11'], dae['e12'], dae['e13']],
[dae['e21'], dae['e22'], dae['e23']],
[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']))
# 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.blockcat(\
[[dae.ddt(name) for name in ['e11', 'e12', 'e13']],
[dae.ddt(name) for name in ['e21', 'e22', 'e23']],
[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('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('ddr') - dae['dddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps']
])
# 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
dae['neg_electrical_winch_power'] = -dae['electrical_winch_power']
dae['neg_mechanical_winch_power'] = -dae['mechanical_winch_power']
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
obj = [x_end - ydata[0, :].T]
for k in range(int(N)):
x_end = rk4(x0=x_end, p=ca.vertcat(udata[k], EPS_U[k], P))["xf"]
obj.append(x_end - ydata_noise[k + 1, :].T)
r = ca.vertcat(ca.vertcat(*obj), EPS_U)
wv = (1.0 / sigma_y**2) * pl.ones(ydata.shape)
weps_u = (1.0 / sigma_u**2) * pl.ones(udata.shape)
Sigma_y_inv = ca.diag(ca.vec(wv))
Sigma_u_inv = ca.diag(weps_u)
Sigma = ca.blockcat(Sigma_y_inv, ca.DM(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))),\
ca.DM(pl.zeros((Sigma_u_inv.shape[0], Sigma_y_inv.shape[1]))), Sigma_u_inv)
nlp = {"x": V, "f": ca.mtimes([r.T, Sigma, r])}
nlpsolver = ca.nlpsol("nlpsolver", "ipopt", nlp)
V0 = ca.vertcat(
pl.ones(3), \
pl.zeros(N), \
ydata_noise[0,:].T
)
sol = nlpsolver(x0=V0)
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 blockcat(a, b, c, d):
return ca.blockcat(a, b, c, d)
def carouselModel(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( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
, "motor_torque"
, "ddr"
] )
if 'cn_rudder' in conf:
dae.addX('rudder')
dae.addU('drudder')
if 'cL_flaps' in conf:
dae.addX('flaps')
dae.addU('dflaps')
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta']
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
norm = dae['cos_delta']**2 + dae['sin_delta']**2
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
pole_delta = 0.5
else:
pole_delta = 0.0
cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
else:
raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
if "parametrizeInputsAsOnlineData" in conf and conf[ "parametrizeInputsAsOnlineData" ] is True:
dae.addP( [ "daileron",
"delevator",
"dmotor_torque",
'dddr' ] )
else:
dae.addU( [ "daileron",
"delevator",
"dmotor_torque",
'dddr' ] )
# add wind parameter if wind shear is in configuration
if 'wind_model' in conf:
if conf['wind_model']['name'] == 'hardcoded':
dae['w0'] = conf['wind_model']['hardcoded_value']
elif conf['wind_model']['name'] != 'random_walk':
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
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 for plotting
dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*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']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
[dae['e21'],dae['e22'],dae['e23']],
[dae['e31'],dae['e32'],dae['e33']]])
dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
RotPole = 0.5
else:
RotPole = 0.0
Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('motor_torque') - dae['dmotor_torque'],
dae.ddt('ddr') - dae['dddr']
])
if 'rudder' in dae:
ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
if 'flaps' in dae:
ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
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 ]:
ode = C.veccat([ode,
dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('t1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('t2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('t3_disturbance')])
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 is 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('f1_disturbance') - dae['df1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('f2_disturbance') - dae['df2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('f3_disturbance') - dae['df3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('f1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('f2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('f3_disturbance')])
if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
tau_wind = conf['wind_model']['tau_wind']
ode = C.veccat([ode,
dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
, dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
, dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
])
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
cPole = 0.5
else:
cPole = 0.0
rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
x_end = X0
obj = [x_end - ydata_noise[0,:].T]
for k in range(N):
x_end = rk4(x0 = x_end, p = ca.vertcat(udata[k], EPS_U[k], P))["xf"]
obj.append(x_end - ydata_noise[k+1, :].T)
r = ca.vertcat(ca.vertcat(*obj), EPS_U)
Sigma_y_inv = ca.diag(ca.vec(wv))
Sigma_u_inv = ca.diag(weps_u)
Z = ca.DM(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1])))
Sigma = ca.blockcat(Sigma_y_inv, Z, Z.T, Sigma_u_inv)
nlp = {"x": V, "f": 0.5 * ca.mtimes([r.T, Sigma, r])}
nlpsolver = ca.nlpsol("nlpsolver", "ipopt", nlp)
V0 = ca.vertcat(
pl.ones(3), \
pl.zeros(N), \
ydata[0,:].T
)
sol = nlpsolver(x0 = V0)
dae['cD_tether'] = 0.25*dae['r']*0.001/sref
dae['L_over_D'] = cL/cD
cD = dae['cD'] + dae['cD_tether']
dae['L_over_D_with_tether'] = cL/cD
######## moment coefficients #######
# offset
dae['momentCoeffs0'] = C.DMatrix([0, conf['cm0'], 0])
# with roll rates
# non-dimensionalized angular velocity
w_bn_b_hat = C.veccat([conf['bref'],conf['cref'],conf['bref']])*0.5/dae['airspeed']*w_bn_b
momentCoeffs_pqr = C.mul(C.blockcat([[conf['cl_p'], conf['cl_q'], conf['cl_r']],
[conf['cm_p'], conf['cm_q'], conf['cm_r']],
[conf['cn_p'], conf['cn_q'], conf['cn_r']]]),
w_bn_b_hat)
dae['momentCoeffs_pqr'] = momentCoeffs_pqr
# with alpha beta
momentCoeffs_AB = C.mul(C.blockcat([[ 0, conf['cl_B'], conf['cl_AB']],
[conf['cm_A'], 0, 0],
[ 0, conf['cn_B'], conf['cn_AB']]]),
C.vertcat([alpha, beta, alpha*beta]))
dae['momentCoeffs_AB'] = momentCoeffs_AB
# with control surfaces
momentCoeffs_surf = C.SXMatrix(3, 1, 0)
momentCoeffs_surf[0] += conf['cl_ail']*dae['aileron']
momentCoeffs_surf[1] += conf['cm_elev']*dae['elevator']
obj = [x_end - ydata[0,:].T]
for k in range(int(N)):
x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"]
obj.append(x_end - ydata_noise[k+1, :].T)
r = ca.vertcat([ca.vertcat(obj), EPS_U])
wv = (1.0 / sigma_y**2) * pl.ones(ydata.shape)
weps_u = (1.0 / sigma_u**2) * pl.ones(udata.shape)
Sigma_y_inv = ca.diag(ca.vec(wv))
Sigma_u_inv = ca.diag(weps_u)
Sigma = ca.blockcat(Sigma_y_inv, ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))),\
ca.DMatrix(pl.zeros((Sigma_u_inv.shape[0], Sigma_y_inv.shape[1]))), Sigma_u_inv)
nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), ca.nlpOut(f = ca.mul([r.T, Sigma, r])))
nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp)
V0 = ca.vertcat([
pl.ones(3), \
pl.zeros(N), \
ydata_noise[0,:].T
])
sol = nlpsolver(x0 = V0)
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 __init__(self,
horizon,
model,
gp=None,
Q=None,
P=None,
R=None,
S=None,
lam=None,
lam_state=None,
ulb=None,
uub=None,
xlb=None,
xub=None,
terminal_constraint=None,
feedback=True,
percentile=None,
gp_method='TA',
costFunc='quad',
solver_opts=None,
discrete_method='gp',
inequality_constraints=None,
num_con_par=0,
hybrid=None,
Bd=None,
Bf=None):
""" Initialize and build the MPC solver
# Arguments:
horizon: Prediction horizon with control inputs
model: System model
# Optional Argumants:
gp: GP model
Q: State penalty matrix, default=diag(1,...,1)
P: Termial penalty matrix, default=diag(1,...,1)
if feedback is True, then P is the solution of the DARE,
discarding this option.
R: Input penalty matrix, default=diag(1,...,1)*0.01
S: Input rate of change penalty matrix, default=diag(1,...,1)*0.1
lam: Slack variable penalty for constraints, defalt=1000
lam_state: Slack variable penalty for violation of upper/lower
state boundy, defalt=None
ulb: Lower boundry input
uub: Upper boundry input
xlb: Lower boundry state
xub: Upper boundry state
terminal_constraint: Terminal condition on the state
* if None: No terminal constraint is used
* if zero: Terminal state is equal to zero
* if nonzero: Terminal state is bounded within +/- the constraint
* if not None and feedback is True, then the expected value of
the Lyapunov function E{x^TPx} < terminal_constraint
is used as a terminal constraint.
feedback: If true, use an LQR feedback function u= Kx + v
percentile: Measure how far from the contrain that is allowed,
P(X in constrained set) > percentile,
percentile= 1 - probability of violation,
default=0.95
gp_method: Method of propagating the uncertainty
Possible options:
'TA': Second order Taylor approximation
'ME': Mean equivalent approximation
costFunc: Cost function to use in the objective
'quad': Expected valaue of Quadratic Cost
'sat': Expected value of Saturating cost
solver_opts: Additional options to pass to the NLP solver
e.g.: solver_opts['print_time'] = False
solver_opts['ipopt.tol'] = 1e-8
discrete_method: 'gp' - Gaussian process model
'rk4' - Runga-Kutta 4 Integrator
'exact' - CVODES or IDEAS (for ODEs or DEAs)
'hybrid' - GP model for dynamic equations, and RK4
for kinematic equations
'd_hybrid' - Same as above, without uncertainty
'f_hybrid' - GP estimating modelling errors, with
RK4 computing the the actual model
num_con_par: Number of parameters to pass to the inequality function
inequality_constraints: Additional inequality constraints
Use a function with inputs (x, covar, u, eps) and
that returns a dictionary with inequality constraints and limits.
e.g. cons = dict(con_ineq=con_ineq_array,
con_ineq_lb=con_ineq_lb_array,
con_ineq_ub=con_ineq_ub_array
)
# NOTES:
* Differentiation of Sundails integrators is not supported with SX graph,
meaning that the solver option 'extend_graph' must be set to False
to use MX graph instead when using the 'exact' discrete method.
* At the moment the f_hybrid option is not finished implemented...
"""
build_solver_time = -time.time()
dt = model.sampling_time()
Ny, Nu, Np = model.size()
Nx = Nu + Ny
Nt = int(horizon / dt)
self.__dt = dt
self.__Nt = Nt
self.__Ny = Ny
self.__Nx = Nx
self.__Nu = Nu
self.__num_con_par = num_con_par
self.__model = model
self.__hybrid = hybrid
self.__gp = gp
self.__feedback = feedback
self.__discrete_method = discrete_method
""" Default penalty values """
if P is None:
P = np.eye(Ny)
if Q is None:
Q = np.eye(Ny)
if R is None:
R = np.eye(Nu) * 0.01
if S is None:
S = np.eye(Nu) * 0.1
if lam is None:
lam = 1000
self.__Q = Q
self.__P = P
self.__R = R
self.__S = S
self.__Bd = Bd
self.__Bf = Bf
if xub is None:
xub = np.full((Ny), np.inf)
if xlb is None:
xlb = np.full((Ny), -np.inf)
if uub is None:
uub = np.full((Nu), np.inf)
if ulb is None:
ulb = np.full((Nu), -np.inf)
""" Default percentile probability """
if percentile is None:
percentile = 0.95
quantile_x = np.ones(Ny) * norm.ppf(percentile)
quantile_u = np.ones(Nu) * norm.ppf(percentile)
Hx = ca.MX.eye(Ny)
Hu = ca.MX.eye(Nu)
""" Create parameter symbols """
mean_0_s = ca.MX.sym('mean_0', Ny)
mean_ref_s = ca.MX.sym('mean_ref', Ny)
u_0_s = ca.MX.sym('u_0', Nu)
covariance_0_s = ca.MX.sym('covariance_0', Ny * Ny)
K_s = ca.MX.sym('K', Nu * Ny)
P_s = ca.MX.sym('P', Ny * Ny)
con_par = ca.MX.sym('con_par', num_con_par)
param_s = ca.vertcat(mean_0_s, mean_ref_s, covariance_0_s, u_0_s, K_s,
P_s, con_par)
""" Select wich GP function to use """
if discrete_method is 'gp':
self.__gp.set_method(gp_method)
#TODO:Fix
if solver_opts['expand'] is not False and discrete_method is 'exact':
raise TypeError(
"Can't use exact discrete system with expanded graph")
""" Initialize state variance with the GP noise variance """
if gp is not None:
#TODO: Cannot use gp variance with hybrid model
self.__variance_0 = np.full((Ny), 1e-10) #gp.noise_variance()
else:
self.__variance_0 = np.full((Ny), 1e-10)
""" Define which cost function to use """
self.__set_cost_function(costFunc, mean_ref_s, P_s.reshape((Ny, Ny)))
""" Feedback function """
mean_s = ca.MX.sym('mean', Ny)
v_s = ca.MX.sym('v', Nu)
if feedback:
u_func = ca.Function(
'u', [mean_s, mean_ref_s, v_s, K_s],
[v_s + ca.mtimes(K_s.reshape((Nu, Ny)), mean_s - mean_ref_s)])
else:
u_func = ca.Function('u', [mean_s, mean_ref_s, v_s, K_s], [v_s])
self.__u_func = u_func
""" Create variables struct """
var = ctools.struct_symMX([(
ctools.entry('mean', shape=(Ny, ), repeat=Nt + 1),
ctools.entry('L',
shape=(int((Ny**2 - Ny) / 2 + Ny), ),
repeat=Nt + 1),
ctools.entry('v', shape=(Nu, ), repeat=Nt),
ctools.entry('eps', shape=(3, ), repeat=Nt + 1),
ctools.entry('eps_state', shape=(Ny, ), repeat=Nt + 1),
)])
num_slack = 3 #TODO: Make this a little more dynamic...
num_state_slack = Ny
self.__var = var
self.__num_var = var.size
# Decision variable boundries
self.__varlb = var(-np.inf)
self.__varub = var(np.inf)
""" Adjust hard boundries """
for t in range(Nt + 1):
j = Ny
k = 0
for i in range(Ny):
# Lower boundry of diagonal
self.__varlb['L', t, k] = 0
k += j
j -= 1
self.__varlb['eps', t] = 0
self.__varlb['eps_state', t] = 0
if xub is not None:
self.__varub['mean', t] = xub
if xlb is not None:
self.__varlb['mean', t] = xlb
if lam_state is None:
self.__varub['eps_state'] = 0
""" Input covariance matrix """
if discrete_method is 'hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
Nz_gp = Ny_gp + Nu_gp
covar_d_sx = ca.SX.sym('cov_d', Ny_gp, Ny_gp)
K_sx = ca.SX.sym('K', Nu, Ny)
covar_u_func = ca.Function(
'cov_u',
[covar_d_sx, K_sx],
# [K_sx @ covar_d_sx @ K_sx.T])
[ca.SX(Nu, Nu)])
covar_s = ca.SX(Nz_gp, Nz_gp)
covar_s[:Ny_gp, :Ny_gp] = covar_d_sx
# covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u)
covar_func = ca.Function('covar', [covar_d_sx], [covar_s])
elif discrete_method is 'f_hybrid':
#TODO: Fix this...
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
Nz_gp = Ny_gp + Nu_gp
# covar_x_s = ca.MX.sym('covar_x', Ny_gp, Ny_gp)
covar_d_sx = ca.SX.sym('cov_d', Ny_gp, Ny_gp)
K_sx = ca.SX.sym('K', Nu, Ny)
#
covar_u_func = ca.Function(
'cov_u',
[covar_d_sx, K_sx],
# [K_sx @ covar_d_sx @ K_sx.T])
[ca.SX(Nu, Nu)])
# cov_xu_func = ca.Function('cov_xu', [covar_x_sx, K_sx],
# [covar_x_sx @ K_sx.T])
# cov_xu = cov_xu_func(covar_x_s, K_s.reshape((Nu, Ny)))
# cov_u = covar_u_func(covar_x_s, K_s.reshape((Nu, Ny)))
covar_s = ca.SX(Nz_gp, Nz_gp)
covar_s[:Ny_gp, :Ny_gp] = covar_d_sx
# covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u)
covar_func = ca.Function('covar', [covar_d_sx], [covar_s])
else:
covar_x_s = ca.MX.sym('covar_x', Ny, Ny)
covar_x_sx = ca.SX.sym('cov_x', Ny, Ny)
K_sx = ca.SX.sym('K', Nu, Ny)
covar_u_func = ca.Function('cov_u', [covar_x_sx, K_sx],
[K_sx @ covar_x_sx @ K_sx.T])
cov_xu_func = ca.Function('cov_xu', [covar_x_sx, K_sx],
[covar_x_sx @ K_sx.T])
cov_xu = cov_xu_func(covar_x_s, K_s.reshape((Nu, Ny)))
cov_u = covar_u_func(covar_x_s, K_s.reshape((Nu, Ny)))
covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u)
covar_func = ca.Function('covar', [covar_x_s], [covar_s])
""" Hybrid output covariance matrix """
if discrete_method is 'hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
covar_d_sx = ca.SX.sym('covar_d', Ny_gp, Ny_gp)
covar_x_sx = ca.SX.sym('covar_x', Ny, Ny)
u_s = ca.SX.sym('u', Nu)
cov_x_next_s = ca.SX(Ny, Ny)
cov_x_next_s[:Ny_gp, :Ny_gp] = covar_d_sx
#TODO: Missing kinematic states
covar_x_next_func = ca.Function(
'cov',
#[mean_s, u_s, covar_d_sx, covar_x_sx],
[covar_d_sx],
[cov_x_next_s])
""" f_hybrid output covariance matrix """
elif discrete_method is 'f_hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
# Nz_gp = Ny_gp + Nu_gp
covar_d_sx = ca.SX.sym('covar_d', Ny_gp, Ny_gp)
covar_x_sx = ca.SX.sym('covar_x', Ny, Ny)
# L_x = ca.SX.sym('L', ca.Sparsity.lower(Ny))
# L_d = ca.SX.sym('L', ca.Sparsity.lower(3))
mean_s = ca.SX.sym('mean', Ny)
u_s = ca.SX.sym('u', Nu)
# A_f = hybrid.rk4_jacobian_x(mean_s[Ny_gp:], mean_s[:Ny_gp])
# B_f = hybrid.rk4_jacobian_u(mean_s[Ny_gp:], mean_s[:Ny_gp])
# C = ca.horzcat(A_f, B_f)
# cov = ca.blocksplit(covar_x_s, Ny_gp, Ny_gp)
# cov[-1][-1] = covar_d_sx
# cov_i = ca.blockcat(cov)
# cov_f = C @ cov_i @ C.T
# cov[0][0] = cov_f
cov_x_next_s = ca.SX(Ny, Ny)
cov_x_next_s[:Ny_gp, :Ny_gp] = covar_d_sx
# cov_x_next_s[Ny_gp:, Ny_gp:] =
#TODO: Pre-solve the GP jacobian using the initial condition in the solve iteration
# jac_mean = ca.SX(Ny_gp, Ny)
# jac_mean = self.__gp.jacobian(mean_s[:Ny_gp], u_s, 0)
# A = ca.horzcat(jac_f, Bd)
# jac = Bf @ jac_f @ Bf.T + Bd @ jac_mean @ Bd.T
# temp = jac_mean @ covar_x_s
# temp = jac_mean @ L_s
# cov_i = ca.SX(Ny + 3, Ny + 3)
# cov_i[:Ny,:Ny] = covar_x_s
# cov_i[Ny:, Ny:] = covar_d_s
# cov_i[Ny:, :Ny] = temp
# cov_i[:Ny, Ny:] = temp.T
#TODO: This is just a new TA implementation... CLEAN UP...
covar_x_next_func = ca.Function(
'cov',
[mean_s, u_s, covar_d_sx, covar_x_sx],
#TODO: Clean up
# [A @ cov_i @ A.T])
# [Bd @ covar_d_s @ Bd.T + jac @ covar_x_s @ jac.T])
# [ca.blockcat(cov)])
[cov_x_next_s])
# Cholesky factorization of covariance function
# S_x_next_func = ca.Function( 'S_x', [mean_s, u_s, covar_d_s, covar_x_s],
# [Bd @ covar_d_s + jac @ covar_x_s])
L_s = ca.SX.sym('L', ca.Sparsity.lower(Ny))
L_to_cov_func = ca.Function('cov', [L_s], [L_s @ L_s.T])
covar_x_sx = ca.SX.sym('cov_x', Ny, Ny)
cholesky = ca.Function('cholesky', [covar_x_sx],
[ca.chol(covar_x_sx).T])
""" Set initial values """
obj = ca.MX(0)
con_eq = []
con_ineq = []
con_ineq_lb = []
con_ineq_ub = []
con_eq.append(var['mean', 0] - mean_0_s)
L_0_s = ca.MX(ca.Sparsity.lower(Ny), var['L', 0])
L_init = cholesky(covariance_0_s.reshape((Ny, Ny)))
con_eq.append(L_0_s.nz[:] - L_init.nz[:])
u_past = u_0_s
""" Build constraints """
for t in range(Nt):
# Input to GP
mean_t = var['mean', t]
u_t = u_func(mean_t, mean_ref_s, var['v', t], K_s)
L_x = ca.MX(ca.Sparsity.lower(Ny), var['L', t])
covar_x_t = L_to_cov_func(L_x)
if discrete_method is 'hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
covar_t = covar_func(covar_x_t[:Ny_gp, :Ny_gp])
elif discrete_method is 'd_hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
covar_t = ca.MX(Ny_gp + Nu_gp, Ny_gp + Nu_gp)
elif discrete_method is 'gp':
covar_t = covar_func(covar_x_t)
else:
covar_t = ca.MX(Nx, Nx)
""" Select the chosen integrator """
if discrete_method is 'rk4':
mean_next_pred = model.rk4(mean_t, u_t, [])
covar_x_next_pred = ca.MX(Ny, Ny)
elif discrete_method is 'exact':
mean_next_pred = model.Integrator(x0=mean_t, p=u_t)['xf']
covar_x_next_pred = ca.MX(Ny, Ny)
elif discrete_method is 'd_hybrid':
# Deterministic hybrid GP model
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t,
covar_t)
mean_next_pred = ca.vertcat(
mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], []))
covar_x_next_pred = ca.MX(Ny, Ny)
elif discrete_method is 'hybrid':
# Hybrid GP model
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t,
covar_t)
mean_next_pred = ca.vertcat(
mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], []))
#covar_x_next_pred = covar_x_next_func(mean_t, u_t, covar_d,
# covar_x_t)
covar_x_next_pred = covar_x_next_func(covar_d)
elif discrete_method is 'f_hybrid':
#TODO: Hybrid GP model estimating model error
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t,
covar_t)
mean_next_pred = ca.vertcat(
mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], []))
covar_x_next_pred = covar_x_next_func(mean_t, u_t, covar_d,
covar_x_t)
else: # Use GP as default
mean_next_pred, covar_x_next_pred = self.__gp.predict(
mean_t, u_t, covar_t)
""" Continuity constraints """
mean_next = var['mean', t + 1]
con_eq.append(mean_next_pred - mean_next)
L_x_next = ca.MX(ca.Sparsity.lower(Ny), var['L', t + 1])
covar_x_next = L_to_cov_func(L_x_next).reshape((Ny * Ny, 1))
L_x_next_pred = cholesky(covar_x_next_pred)
con_eq.append(L_x_next_pred.nz[:] - L_x_next.nz[:])
""" Chance state constraints """
cons = self.__constraint(mean_next, L_x_next, Hx, quantile_x, xub,
xlb, var['eps_state', t])
con_ineq.extend(cons['con'])
con_ineq_lb.extend(cons['con_lb'])
con_ineq_ub.extend(cons['con_ub'])
""" Input constraints """
# cov_u = covar_u_func(covar_x_t, K_s.reshape((Nu, Ny)))
cov_u = ca.MX(Nu, Nu)
# cons = self.__constraint(u_t, cov_u, Hu, quantile_u, uub, ulb)
# con_ineq.extend(cons['con'])
# con_ineq_lb.extend(cons['con_lb'])
# con_ineq_ub.extend(cons['con_ub'])
if uub is not None:
con_ineq.append(u_t)
con_ineq_ub.extend(uub)
con_ineq_lb.append(np.full((Nu, ), -ca.inf))
if ulb is not None:
con_ineq.append(u_t)
con_ineq_ub.append(np.full((Nu, ), ca.inf))
con_ineq_lb.append(ulb)
""" Add extra constraints """
if inequality_constraints is not None:
cons = inequality_constraints(var['mean', t + 1], covar_x_next,
u_t, var['eps', t], con_par)
con_ineq.extend(cons['con_ineq'])
con_ineq_lb.extend(cons['con_ineq_lb'])
con_ineq_ub.extend(cons['con_ineq_ub'])
""" Objective function """
u_delta = u_t - u_past
obj += self.__l_func(var['mean', t], covar_x_t, u_t, cov_u, u_delta) \
+ np.full((1, num_slack),lam) @ var['eps', t]
if lam_state is not None:
obj += np.full(
(1, num_state_slack), lam_state) @ var['eps_state', t]
u_t = u_past
L_x = ca.MX(ca.Sparsity.lower(Ny), var['L', Nt])
covar_x_t = L_to_cov_func(L_x)
obj += self.__lf_func(var['mean', Nt], covar_x_t, P_s.reshape((Ny, Ny))) \
+ np.full((1, num_slack),lam) @ var['eps', Nt]
if lam_state is not None:
obj += np.full(
(1, num_state_slack), lam_state) @ var['eps_state', Nt]
num_eq_con = ca.vertcat(*con_eq).size1()
num_ineq_con = ca.vertcat(*con_ineq).size1()
con_eq_lb = np.zeros((num_eq_con, ))
con_eq_ub = np.zeros((num_eq_con, ))
""" Terminal contraint """
if terminal_constraint is not None and not feedback:
con_ineq.append(var['mean', Nt] - mean_ref_s)
num_ineq_con += Ny
con_ineq_lb.append(np.full((Ny, ), -terminal_constraint))
con_ineq_ub.append(np.full((Ny, ), terminal_constraint))
elif terminal_constraint is not None and feedback:
con_ineq.append(
self.__lf_func(var['mean', Nt], covar_x_t, P_s.reshape(
(Ny, Ny))))
num_ineq_con += 1
con_ineq_lb.append(0)
con_ineq_ub.append(terminal_constraint)
con = ca.vertcat(*con_eq, *con_ineq)
self.__conlb = ca.vertcat(con_eq_lb, *con_ineq_lb)
self.__conub = ca.vertcat(con_eq_ub, *con_ineq_ub)
""" Build solver object """
nlp = dict(x=var, f=obj, g=con, p=param_s)
options = {
'ipopt.print_level': 0,
'ipopt.mu_init': 0.01,
'ipopt.tol': 1e-8,
'ipopt.warm_start_init_point': 'yes',
'ipopt.warm_start_bound_push': 1e-9,
'ipopt.warm_start_bound_frac': 1e-9,
'ipopt.warm_start_slack_bound_frac': 1e-9,
'ipopt.warm_start_slack_bound_push': 1e-9,
'ipopt.warm_start_mult_bound_push': 1e-9,
'ipopt.mu_strategy': 'adaptive',
'print_time': False,
'verbose': False,
'expand': True
}
if solver_opts is not None:
options.update(solver_opts)
self.__solver = ca.nlpsol('mpc_solver', 'ipopt', nlp, options)
# First prediction used in the NLP, used in plot later
self.__var_prediction = np.zeros((Nt + 1, Ny))
self.__mean_prediction = np.zeros((Nt + 1, Ny))
self.__mean = None
build_solver_time += time.time()
print('\n________________________________________')
print('# Time to build mpc solver: %f sec' % build_solver_time)
print('# Number of variables: %d' % self.__num_var)
print('# Number of equality constraints: %d' % num_eq_con)
print('# Number of inequality constraints: %d' % num_ineq_con)
print('----------------------------------------')
cD = dae["cD"] + dae["cD_tether"]
dae["L_over_D_with_tether"] = cL / cD
######## moment coefficients #######
# offset
dae["momentCoeffs0"] = C.DMatrix([0, conf["cm0"], 0])
# with roll rates
# non-dimensionalized angular velocity
w_bn_b_hat = C.veccat([conf["bref"], conf["cref"], conf["bref"]]) * 0.5 / dae["airspeed"] * w_bn_b
momentCoeffs_pqr = C.mul(
C.blockcat(
[
[conf["cl_p"], conf["cl_q"], conf["cl_r"]],
[conf["cm_p"], conf["cm_q"], conf["cm_r"]],
[conf["cn_p"], conf["cn_q"], conf["cn_r"]],
]
),
w_bn_b_hat,
)
dae["momentCoeffs_pqr"] = momentCoeffs_pqr
# with alpha beta
momentCoeffs_AB = C.mul(
C.blockcat([[0, conf["cl_B"], conf["cl_AB"]], [conf["cm_A"], 0, 0], [0, conf["cn_B"], conf["cn_AB"]]]),
C.vertcat([alpha, beta, alpha * beta]),
)
dae["momentCoeffs_AB"] = momentCoeffs_AB
# with control surfaces
x_end = X0
obj = [x_end - ydata_noise[0,:].T]
for k in range(N):
x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"]
obj.append(x_end - ydata_noise[k+1, :].T)
r = ca.vertcat([ca.vertcat(obj), EPS_U])
Sigma_y_inv = ca.diag(ca.vec(wv))
Sigma_u_inv = ca.diag(weps_u)
Z = ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1])))
Sigma = ca.blockcat(Sigma_y_inv, Z, Z.T, Sigma_u_inv)
nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), \
ca.nlpOut(f = 0.5 * ca.mul([r.T, Sigma, r])))
nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp)
V0 = ca.vertcat([
pl.ones(3), \
pl.zeros(N), \
ydata[0,:].T
])
sol = nlpsolver(x0 = V0)
def blockcat(a, b, c, d):
return ca.blockcat(a, b, c, d)
def compute_covariance_matrix(self):
r'''
This function computes the covariance matrix of the estimated
parameters from the inverse of the KKT matrix for the
parameter estimation problem. This allows then for statements on the
quality of the values of the estimated parameters.
For efficiency, only the inverse of the relevant part of the matrix
is computed using the Schur complement.
A more detailed description of this function will follow in future
versions.
'''
intro.pecas_intro()
print('\n' + 20 * '-' + \
' PECas covariance matrix computation ' + 21 * '-')
print('''
Computing the covariance matrix for the estimated parameters,
this might take some time ...
''')
self.tstart_cov_computation = time.time()
try:
N1 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
self.w.shape[0])
N2 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
self.Vars.shape[0] - self.w.shape[0])
hess = ca.blockcat([[N2, N1], [N1.T, ca.diag(self.w)],])
# hess = hess + 1e-10 * ca.diag(self.Vars)
# J2 can be re-used from parameter estimation, right?
J2 = ca.jacobian(self.g, self.Vars)
kkt = ca.blockcat( \
[[hess, \
J2.T], \
[J2, \
ca.MX(self.g.size1(), self.g.size1())]] \
)
B1 = kkt[:self.pesetup.np, :self.pesetup.np]
E = kkt[self.pesetup.np:, :self.pesetup.np]
D = kkt[self.pesetup.np:, self.pesetup.np:]
Dinv = ca.solve(D, E, "csparse")
F11 = B1 - ca.mul([E.T, Dinv])
self.fbeta = ca.MXFunction("fbeta", [self.Vars],
[ca.mul([self.R.T, self.R]) / \
(self.yN.size + self.g.size1() - self.Vars.size())])
[self.beta] = self.fbeta([self.Varshat])
self.fcovp = ca.MXFunction("fcovp", [self.Vars], \
[self.beta * ca.solve(F11, ca.MX.eye(F11.size1()))])
[self.Covp] = self.fcovp([self.Varshat])
print( \
'''Covariance matrix computation finished, run show_results() to visualize.''')
except AttributeError as err:
errmsg = '''
You must execute run_parameter_estimation() first before the covariance
matrix for the estimated parameters can be computed.
'''
raise AttributeError(errmsg)
finally:
self.tend_cov_computation = time.time()
self.duration_cov_computation = self.tend_cov_computation - \
self.tstart_cov_computation
def carouselModel(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( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
, "motor_torque"
, "ddr"
] )
if 'cn_rudder' in conf:
dae.addX('rudder')
dae.addU('drudder')
if 'cL_flaps' in conf:
dae.addX('flaps')
dae.addU('dflaps')
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta']
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
norm = dae['cos_delta']**2 + dae['sin_delta']**2
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
pole_delta = 0.5
else:
pole_delta = 0.0
cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
else:
raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
dae.addU( [ "daileron"
, "delevator"
, "dmotor_torque"
, 'dddr'
] )
# add wind parameter if wind shear is in configuration
if 'wind_model' in conf:
if conf['wind_model']['name'] == 'hardcoded':
dae['w0'] = conf['wind_model']['hardcoded_value']
elif conf['wind_model']['name'] != 'random_walk':
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
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 for plotting
dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*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']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
[dae['e21'],dae['e22'],dae['e23']],
[dae['e31'],dae['e32'],dae['e33']]])
dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
RotPole = 0.5
else:
RotPole = 0.0
Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('motor_torque') - dae['dmotor_torque'],
dae.ddt('ddr') - dae['dddr']
])
if 'rudder' in dae:
ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
if 'flaps' in dae:
ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
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 ]:
ode = C.veccat([ode,
dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('t1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('t2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('t3_disturbance')])
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 is 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('f1_disturbance') - dae['df1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('f2_disturbance') - dae['df2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('f3_disturbance') - dae['df3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('f1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('f2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('f3_disturbance')])
if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
tau_wind = conf['wind_model']['tau_wind']
ode = C.veccat([ode,
dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
, dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
, dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
])
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
cPole = 0.5
else:
cPole = 0.0
rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
def tr(x, y, z):
return c.blockcat([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])
