def __cost_saturation_l(self, x, x_ref, covar_x, u, covar_u, delta_u, Q, R,
S):
""" Stage Cost function: Expected Value of Saturating Cost
"""
Nx = ca.MX.size1(Q)
Nu = ca.MX.size1(R)
# Create symbols
Q_s = ca.SX.sym('Q', Nx, Nx)
R_s = ca.SX.sym('Q', Nu, Nu)
x_s = ca.SX.sym('x', Nx)
u_s = ca.SX.sym('x', Nu)
covar_x_s = ca.SX.sym('covar_z', Nx, Nx)
covar_u_s = ca.SX.sym('covar_u', ca.MX.size(R))
Z_x = ca.SX.eye(Nx) + 2 * covar_x_s @ Q_s
Z_u = ca.SX.eye(Nu) + 2 * covar_u_s @ R_s
cost_x = ca.Function('cost_x', [x_s, Q_s, covar_x_s], [
1 - ca.exp(-(x_s.T @ ca.solve(Z_x.T, Q_s.T).T @ x_s)) /
ca.sqrt(ca.det(Z_x))
])
cost_u = ca.Function('cost_u', [u_s, R_s, covar_u_s], [
1 - ca.exp(-(u_s.T @ ca.solve(Z_u.T, R_s.T).T @ u_s)) /
ca.sqrt(ca.det(Z_u))
])
return cost_x(x - x_ref, Q, covar_x) + cost_u(u, R, covar_u)
def pinv(self, J):
if self.options["pinv_method"] == "standard":
pJ = cs.pinv(J)
elif self.options["pinv_method"] == "damped":
dmpng_fctr = self.options["damping_factor"]
if J.size2() >= J.size1():
inner = cs.mtimes(J, J.T)
inner += dmpng_fctr * cs.DM.eye(J.size1())
pJ = cs.solve(inner, J).T
else:
inner = cs.mtimes(J.T, J)
inner += dmpng_fctr * cs.DM.eye(J.size2())
pJ = cs.solve(inner, J.T)
return pJ
def get_forward_dynamics_crba(self, root, tip, gravity=None, f_ext=None):
"""Returns the forward dynamics as a casadi function by
solving the Lagrangian eq. of motion. OBS! Not appropriate
for robots with a high number of dof -> use
get_forward_dynamics_aba().
"""
if self.robot_desc is None:
raise ValueError('Robot description not loaded from urdf')
n_joints = self.get_n_joints(root, tip)
q = cs.SX.sym("q", n_joints)
q_dot = cs.SX.sym("q_dot", n_joints)
tau = cs.SX.sym("tau", n_joints)
q_ddot = cs.SX.zeros(n_joints)
i_X_p, Si, Ic = self._model_calculation(root, tip, q)
M = self._get_M(Ic, i_X_p, Si, n_joints, q)
M_inv = cs.solve(M, cs.SX.eye(M.size1()))
C = self._get_C(i_X_p, Si, Ic, q, q_dot, n_joints, gravity, f_ext)
q_ddot = cs.mtimes(M_inv, (tau - C))
q_ddot = cs.Function("q_ddot", [q, q_dot, tau], [q_ddot],
self.func_opts)
return q_ddot
def create_LSpoly(d, x, y):
"creates polynomial by least squares fitting of order d. Builds Vandermonde matrix manually"
# x = np.append(x,0) # add (0,0) as data point
# y = np.append(y,0)
a = d + 1 # number of parameters including a0
M = ca.SX.sym('M', 2 * d + 1) # all the exponents from 1 to 2k
sizex = x.shape[0] # number of data points x
M[0] = sizex # first entry in matrix
for k in range(1, M.shape[0]): # collect all matrix entries
M[k] = sum([x[o]**k for o in range(0, sizex)])
sumM = ca.SX(a, a) # create actual matrix
for j in range(0, a):
for k in range(0, a):
sumM[j, k] = M[k + j]
B = ca.SX(a, 1) # create B vector (sum of y)
for k in range(0, a):
B[k] = sum([y[o] * x[o]**k for o in range(0, sizex)])
X = ca.solve(sumM, B) # parameters order: low to high power
xvar = ca.SX.sym('xvar')
poly = X[0]
for k in range(1, X.shape[0]):
poly += X[k] * xvar**(k) # create polynomial
pfun = ca.Function('poly', [xvar], [poly])
return pfun, X, poly
def buildDynamicalSystem(self):
# q1 is first link q2 is second link
q1 = ca.MX.sym('q1')
dq1 = ca.MX.sym('dq1')
q2 = ca.MX.sym('q2')
dq2 = ca.MX.sym('dq2')
u = ca.MX.sym('u')
theta = ca.MX.sym("theta", 7, 1)
if self.trainMotor:
Rm = ca.MX.sym("Rm")
km = ca.MX.sym("km")
params = ca.vertcat(ca.MX.sym("params", 7, 1), Rm, km)
else:
Rm = self.Rm
km = self.km
params = theta
states = ca.vertcat(q1, q2, dq1, dq2)
controls = u # km * (u - km * dq1) / Rm;
if self.hackOn:
# convert actions which are given as torques to voltage to be consistent with the controller inputs of Lukas
u = u * Rm / km
controls_torque = km * (u - km * dq1) / Rm
# build matrix for mass matrix
phi_1_1 = ca.MX(2, 7)
phi_1_1[0, 0] = ca.MX.ones(1)
phi_1_1[0, 1] = ca.sin(q2) * ca.sin(q2)
phi_1_1[1, 2] = ca.cos(q2)
phi_1_2 = ca.MX(2, 7)
phi_1_2[0, 2] = -ca.cos(q2)
phi_1_2[1, 3] = ca.MX.ones(1)
mass_matrix = ca.horzcat(ca.mtimes(phi_1_1, params),
ca.mtimes(phi_1_2, params))
phi_2 = ca.MX(2, 7)
phi_2[0, 1] = ca.sin(2 * q2) * dq1 * dq2
phi_2[0, 2] = ca.sin(q2) * dq2 * dq2
phi_2[0, 5] = dq1
phi_2[1, 1] = -1 / 2 * ca.sin(2 * q2) * dq1 * dq1
phi_2[1, 4] = self.g * ca.sin(q2)
phi_2[1, 6] = dq2
forces = ca.mtimes(phi_2, params)
actions = ca.vertcat(controls_torque, ca.MX.zeros(1))
b = actions - forces
states_dd = ca.solve(mass_matrix, b)
states_d = ca.vertcat(dq1, dq2, states_dd[0], states_dd[1])
return states, states_d, controls, params
def setup_oed(self, outputs, parameters, sigma, time_points, design="A"):
"""
Transforms an Optimization Problem into an Optimal Experimental Design problem.
Parameters::
outputs --
List of names for outputs.
Type: [string]
parameters --
List of names for parameters to estimate.
Type: [string]
sigma --
Experiment variance matrix.
Type: [[float]]
time_points --
List of measurement time points.
Type: [float]
design --
Design criterion.
Possible values: "A", "T"
"""
# Augment sensitivities and add timed variables
self.augment_sensitivities(parameters)
timed_sens = self.create_timed_sensitivities(outputs, parameters, time_points)
# Create sensitivity and Fisher matrices
Q = []
for j in xrange(len(outputs)):
Q.append(casadi.vertcat([casadi.horzcat([s.getVar() for s in timed_sens[i][j]])
for i in xrange(len(time_points))]))
Fisher = sum([sigma[i, j] * casadi.mul(Q[i].T, Q[j]) for i in xrange(len(outputs))
for j in xrange(len(outputs))])
# Define the objective
if design == "A":
b = casadi.MX.sym("b", Fisher.shape[1], 1)
Fisher_inv = casadi.jacobian(casadi.solve(Fisher, b), b)
obj = casadi.trace(Fisher_inv)
elif design == "T":
obj = -casadi.trace(Fisher)
elif design == "D":
obj = -casadi.det(Fisher)
raise NotImplementedError("D-optimal design is not supported.")
else:
raise ValueError("Invalid design %s." % design)
old_obj = self.getObjective()
self.setObjective(old_obj + obj)
def construct_upd_z(self, problem=None, lineq_updz=True):
if problem is not None:
self.problem_upd_z = problem
self._lineq_updz = lineq_updz
return 0.
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
raise ValueError('For now, only equality constrained QP ' +
'z-updates are allowed!')
x_i = struct_symMX(self.q_i_struct)
x_j = struct_symMX(self.q_ij_struct)
l_i = struct_symMX(self.q_i_struct)
l_ij = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
par = struct_symMX(self.par_global_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t / T
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A(par.cat)
b = b(par.cat)
# build KKT system and solve it via schur complement method
l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
f = -(l + rho * x)
G = -(1 / rho) * mtimes(A, A.T)
h = b + (1 / rho) * mtimes(A, f)
mu = solve(G, h)
z = -(1 / rho) * (mtimes(A.T, mu) + f)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi + l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, buildtime = create_function('upd_z_' + str(self._index), inp,
out, self.options)
self.problem_upd_z = prob
return buildtime
def nllh_casf(self, grad=True, hess=False):
"""
Creates a casadi function computing the negative log likelihood (without const terms) of the data
dependent on hyperparameters v.
:param grad: Whether the function should compute the gradient, too
:param hess: Whether the function should compute the hessian, too
:return: A casadi function taking a value of v as input and returning the neg log likelihood, gradient,
and hessian is desired
"""
vshape = np.atleast_2d(self.v).shape
v = cas.MX.sym("v", vshape[0], vshape[1])
phi_x = self.phi_cas(self.x, v)
sinv = self.sinv0 + self.beta * cas.mtimes(phi_x.T, phi_x)
mean = cas.solve(
sinv,
cas.mtimes(self.sinv0, self.mean0) +
self.beta * cas.mtimes(phi_x.T, self.y))
y_pred = cas.mtimes(mean.T, phi_x.T).T
sigma = 1 / self.beta + cas.sum2(phi_x * cas.solve(sinv, phi_x.T).T)
y_true = self.y
y_diff = y_true - y_pred
llht = cas.sum2(y_diff * y_diff) / sigma
llh = cas.sum1(llht)
if hess:
H, llh_grad = cas.hessian(llh, v)
elif grad:
llh_grad = cas.gradient(llh, v)
res = [llh]
if grad:
res += [llh_grad]
if hess:
res += [H]
f = cas.Function("f_mu", [v], res)
return f
def construct_upd_z(self, problem=None, lineq_updz=True):
if problem is not None:
self.problem_upd_z = problem
self._lineq_updz = lineq_updz
return 0.
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
raise ValueError('For now, only equality constrained QP ' +
'z-updates are allowed!')
x_i = struct_symMX(self.q_i_struct)
x_j = struct_symMX(self.q_ij_struct)
l_i = struct_symMX(self.q_i_struct)
l_ij = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
par = struct_symMX(self.par_global_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t/T
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A(par.cat)
b = b(par.cat)
# build KKT system and solve it via schur complement method
l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
f = -(l + rho*x)
G = -(1/rho)*mtimes(A, A.T)
h = b + (1/rho)*mtimes(A, f)
mu = solve(G, h)
z = -(1/rho)*(mtimes(A.T, mu)+f)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options)
self.problem_upd_z = prob
return buildtime
def solve(A, b): # TODO get this working
"""
Solve the linear system Ax=b for x.
Args:
A: A square matrix.
b: A vector representing the RHS of the linear system.
Returns: The solution vector x.
"""
try:
return onp.linalg.solve(A, b)
except Exception:
return cas.solve(A, b)
def solve(A, b): # TODO get this working
"""
Solve the linear system Ax=b for x.
Args:
A: A square matrix.
b: A vector representing the RHS of the linear system.
Returns: The solution vector x.
"""
if not is_casadi_type([A, b]):
return _onp.linalg.solve(A, b)
else:
return _cas.solve(A, b)
def construct_upd_z(self):
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
self._construct_upd_z_nlp()
x_i = struct_symSX(self.q_i_struct)
x_j = struct_symSX(self.q_ij_struct)
l_i = struct_symSX(self.q_i_struct)
l_ij = struct_symSX(self.q_ij_struct)
t = SX.sym('t')
T = SX.sym('T')
rho = SX.sym('rho')
par = struct_symSX(self.par_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t/T
# put symbols in SX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A([par])[0]
b = b([par])[0]
# build KKT system
E = rho*SX.eye(A.shape[1])
l, x = vertcat([l_i.cat, l_ij.cat]), vertcat([x_i.cat, x_j.cat])
f = -(l + rho*x)
G = vertcat([horzcat([E, A.T]),
horzcat([A, SX.zeros(A.shape[0], A.shape[0])])])
h = vertcat([-f, b])
z = solve(G, h)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, compile_time = self.father.create_function(
'upd_z', inp, out, self.options)
self.problem_upd_z = prob
def __cost_saturation_lf(self, x, x_ref, covar_x, P):
""" Terminal Cost function: Expected Value of Saturating Cost
"""
Nx = ca.MX.size1(P)
# Create symbols
P_s = ca.SX.sym('P', Nx, Nx)
x_s = ca.SX.sym('x', Nx)
covar_x_s = ca.SX.sym('covar_z', Nx, Nx)
Z_x = ca.SX.eye(Nx) + 2 * covar_x_s @ P_s
cost_x = ca.Function('cost_x', [x_s, P_s, covar_x_s], [
1 - ca.exp(-(x_s.T @ ca.solve(Z_x.T, P_s.T).T @ x_s)) /
ca.sqrt(ca.det(Z_x))
])
return cost_x(x - x_ref, P, covar_x)
def buildDynamicalSystem(self):
g = 9.8
x = ca.MX.sym('x')
dx = ca.MX.sym('dx')
theta = ca.MX.sym('theta')
dtheta = ca.MX.sym('dtheta')
u = ca.MX.sym('u')
states = ca.vertcat(x, dx, theta, dtheta);
controls = u;
param1 = ca.MX.sym('param_1')
param2 = ca.MX.sym('param_2')
param3 = ca.MX.sym('param_3')
params = ca.vertcat(param1, param2, param3)
print("params_shape", params.shape)
# build matrix for mass matrix
phi_1_1 = ca.MX(2, 3)
phi_1_1[0, 0] = ca.MX.ones(1)
phi_1_1[1, 1] = ca.cos(theta)
phi_1_2 = ca.MX(2, 3)
phi_1_2[0, 1] = ca.cos(theta)
phi_1_2[1, 2] = 4/3*ca.MX.ones(1)
mass_matrix = ca.horzcat(ca.mtimes(phi_1_1, params), ca.mtimes(phi_1_2, params))
phi_2 = ca.MX(2, 3)
phi_2[0, 1] = -1 * ca.sin(theta) * dtheta * dtheta
phi_2[1, 1] = -g * ca.sin(theta)
forces = ca.mtimes(phi_2, params)
actions = ca.vertcat(controls, ca.MX.zeros(1))
b = actions - forces
states_dd = ca.solve(mass_matrix, b)
states_d = ca.vertcat(dx, states_dd[0], dtheta, states_dd[1])
return states, states_d, controls, params
def integral(self, dy):
"""Calculate integral from derivative values
"""
y0 = cs.MX.zeros(dy.shape[0])
y = []
k = 0
for b in self._basis:
N = b.numPoints - 1
# Calculate y the equation
# dy = cs.mtimes(y[:, 1 :], D[: -1, 1 :].T)
#invD = cs.inv(D[: -1, 1 :])
#y.append(cs.transpose(cs.mtimes(invD, cs.transpose(dy[:, k : k + N]))))
y.append(cs.transpose(cs.solve(b.D[: -1, 1 :], cs.transpose(dy[:, k : k + N]))) + cs.repmat(y0, 1, N))
k += N
y0 = y[-1][:, -1]
return cs.horzcat(*y)
def tangent_approx(f: SYM, x: SYM, a: SYM = None, assert_linear: bool = False) -> Dict[str, SYM]:
"""
Create a tangent approximation of a non-linear function f(x) about point a
using a block lower triangular solver
0 = f(x) = f(a) + J*x # taylor series about a (if f(x) linear in x, then globally valid)
J*x = -f(a) # solve for x
x = -J^{-1}f(a) # but inverse is slow, so we use solve
where J = df/dx
"""
# find f(a)
if a is None:
a = ca.DM.zeros(x.numel(), 1)
f_a = ca.substitute(f, x, a) # f(a)
J = ca.jacobian(f, x)
if assert_linear and ca.depends_on(J, x):
raise AssertionError('not linear')
# solve is smart enough to to convert to blt if necessary
return ca.solve(J, -f_a)
def sqrt_covariance_predict(W, F, Q):
"""
Finds a sqrt factorization of the continuous time covariance
propagation equations. Requires solving a linear system of equations
to keep the sqrt lower triangular.
'A Square Root Formulation of the Kalman Covariance Equations', Andrews 68
W: sqrt P, symbolic, with sparsity lower triangulr
F: dynamics matrix
Q: process noise matrix
returns:
W_dot_sol: sqrt of P deriative, lower triangular
"""
n_x = F.shape[0]
XL = ca.SX.sym('X', ca.Sparsity_lower(n_x))
X = (XL - XL.T)
for i in range(n_x):
X[i, i] = 0
W_dot = ca.mtimes(F, W) + ca.mtimes(Q / 2 + X, ca.inv(W).T)
# solve for XI that keeps W dot lower triangular
y = ca.vertcat(*ca.triu(W_dot, False).nonzeros())
x_dep = []
for i, xi in enumerate(XL.nonzeros()):
if ca.depends_on(y, xi):
x_dep += [xi]
x_dep = ca.vertcat(*x_dep)
A = ca.jacobian(y, x_dep)
for i, xi in enumerate(XL.nonzeros()):
assert not ca.depends_on(A, xi)
b = -ca.substitute(y, x_dep, 0)
x_sol = ca.solve(A, b)
X_sol = ca.SX(X)
for i in range(x_dep.shape[0]):
X_sol = ca.substitute(X_sol, x_dep[i], x_sol[i])
X_sol = ca.sparsify(X_sol)
W_dot_sol = ca.mtimes(F, W) + ca.mtimes(Q / 2 + X_sol, ca.inv(W).T)
return W_dot_sol
def getRobustSteadyStateNlpFunctions(self, dae, ref_dict = {}):
xDotSol, zSol = dae.solveForXDotAndZ()
ginv = Constraints()
def constrainInvariantErrs():
R_c2b = dae['R_c2b']
self.makeOrthonormal(ginv, R_c2b)
ginv.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
ginv.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
di = dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1
ginv.add(di, '==', 0, tag = ('delta invariant', None))
ginv.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
("Rotational velocities", None))
constrainInvariantErrs()
invariants = ginv.getG()
J = C.jacobian(invariants,dae.xVec())
# make steady state model
g = Constraints()
xds = C.vertcat([xDotSol[ name ] for name in dae.xNames()])
jInv = C.mul(J.T,C.solve(C.mul(J,J.T),invariants))
g.add(xds - jInv - dae.xDotVec(), '==', 0, tag = ('dae residual', None))
for name in ['alpha_deg', 'beta_deg', 'cL']:
if name in ref_dict:
g.addBnds(dae[name], ref_dict[name], tag = (name, None))
dvs = C.veccat([dae.xVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
obj = 0
for name in dae.uNames() + ['aileron', 'elevator']:
if name in dae:
obj += dae[ name ] ** 2
return dvs, obj, g.getG(), g.getLb(), g.getUb(), zSol
def tangent_approx(f: SYM,
x: SYM,
a: SYM = None,
assert_linear: bool = False) -> Dict[str, SYM]:
"""
Create a tangent approximation of a non-linear function f(x) about point a
using a block lower triangular solver
0 = f(x) = f(a) + J*x # taylor series about a (if f(x) linear in x, then globally valid)
J*x = -f(a) # solve for x
x = -J^{-1}f(a) # but inverse is slow, so we use solve
where J = df/dx
"""
# find f(a)
if a is None:
a = ca.DM.zeros(x.numel(), 1)
f_a = ca.substitute(f, x, a) # f(a)
J = ca.jacobian(f, x)
if assert_linear and ca.depends_on(J, x):
raise AssertionError('not linear')
# solve is smart enough to to convert to blt if necessary
return ca.solve(J, -f_a)
def solveForXDotAndZ(self):
'''
returns (xDotDict,zDict) where these dictionaries contain symbolic
xdot and z which are only a function of x,u,p
'''
# get the residual fg(xdot,x,z)
fg = self.getResidual()
# take the jacobian w.r.t. xdot,z
jac = C.jacobian(fg,C.veccat([self.xDotVec(), self.zVec()]))
# make sure that it was linear in {xdot,z}, i.e. the jacobian is not a function of {xdot,z}
testJac = C.SXFunction([self.xVec(),self.uVec(),self.pVec()], [jac])
testJac.init()
assert len(testJac.getFree()) == 0, \
"can't convert dae to ode, residual jacobian is a function of {xdot,z}"
# get the constant term
fg_fun = C.SXFunction([self.xVec(),self.zVec(),self.uVec(),self.pVec(),self.xDotVec()], [fg])
fg_fun.init()
[fg_zero] = fg_fun([self.xVec(),0*self.zVec(),self.uVec(),self.pVec(),0*self.xDotVec()])
testFun = C.SXFunction([self.xVec(),self.uVec(),self.pVec()], [fg_zero])
testFun.init()
assert len(testFun.getFree()) == 0, \
"the \"impossible\" happened in solveForXDotAndZ"
xDotAndZ = C.solve(jac, -fg_zero)
xDot = xDotAndZ[0:len(self.xNames())]
z = xDotAndZ[len(self.xNames()):]
xDotDict = {}
for k,name in enumerate(self.xNames()):
xDotDict[name] = xDot[k]
zDict = {}
for k,name in enumerate(self.zNames()):
zDict[name] = z[k]
return (xDotDict, zDict)
def __init__(self, inertial_frame_id='world'):
Vehicle.__init__(self, inertial_frame_id)
# Declaring state variables
## Generalized position vector
self.eta = casadi.SX.sym('eta', 6)
## Generalized velocity vector
self.nu = casadi.SX.sym('nu', 6)
# Build the Coriolis matrix
self.CMatrix = casadi.SX.zeros(6, 6)
S_12 = - cross_product_operator(
casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6]))
S_22 = - cross_product_operator(
casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6]))
self.CMatrix[0:3, 3:6] = S_12
self.CMatrix[3:6, 0:3] = S_12
self.CMatrix[3:6, 3:6] = S_22
# Build the damping matrix (linear and nonlinear elements)
self.DMatrix = - casadi.diag(self._linear_damping)
self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
self.DMatrix -= casadi.diag(self._quad_damping * self.nu)
# Build the restoring forces vectors wrt the BODY frame
Rx = np.array([[1, 0, 0],
[0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
Ry = np.array([[casadi.cos(self.eta[4]), 0, casadi.sin(self.eta[4])],
[0, 1, 0],
[-1 * casadi.sin(self.eta[4]), 0, casadi.cos(self.eta[4])]])
Rz = np.array([[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
[casadi.sin(self.eta[5]), casadi.cos(self.eta[5]), 0],
[0, 0, 1]])
R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))
if inertial_frame_id == 'world_ned':
Fg = casadi.SX([0, 0, -self.mass * self.gravity])
Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
else:
Fg = casadi.SX([0, 0, self.mass * self.gravity])
Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])
self.gVec = casadi.SX.zeros(6)
self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)
self.gVec[3:6] = -1 * casadi.mtimes(
R_n_to_b, casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
# Build Jacobian
T = 1 / casadi.cos(self.eta[4]) * np.array(
[[0, casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]), casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])],
[0, casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]), -casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
self.eta_dot = casadi.vertcat(
casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]),
casadi.mtimes(T, self.nu[3::]))
self.u = casadi.SX.sym('u', 6)
self.nu_dot = casadi.solve(
self._Mtotal,
self.u - casadi.mtimes(self.CMatrix, self.nu) - casadi.mtimes(self.DMatrix, self.nu) - self.gVec)
def freeModel(endTimeSteps=None):
stateNames = [
"x" # state 0
,
"y" # state 1
,
"z" # state 2
,
"e11" # state 3
,
"e12" # state 4
,
"e13" # state 5
,
"e21" # state 6
,
"e22" # state 7
,
"e23" # state 8
,
"e31" # state 9
,
"e32" # state 10
,
"e33" # state 11
,
"dx" # state 12
,
"dy" # state 13
,
"dz" # state 14
,
"w1" # state 15
,
"w2" # state 16
,
"w3" # state 17
]
uNames = [
"tc", "aileron", "elevator", "rudder"
# , 'ddr'
]
pNames = ["wind_x"]
uSyms = [C.ssym(name) for name in uNames]
uDict = dict(zip(uNames, uSyms))
uVec = C.veccat(uSyms)
pSyms = [C.ssym(name) for name in pNames]
pDict = dict(zip(pNames, pSyms))
pVec = C.veccat(pSyms)
stateSyms = [C.ssym(name) for name in stateNames]
stateDict = dict(zip(stateNames, stateSyms))
stateVec = C.veccat(stateSyms)
dx = stateDict['dx']
dy = stateDict['dy']
dz = stateDict['dz']
outputs = {}
outputs['aileron(deg)'] = uDict['aileron'] * 180 / C.pi
outputs['elevator(deg)'] = uDict['elevator'] * 180 / C.pi
outputs['rudder(deg)'] = uDict['rudder'] * 180 / C.pi
(massMatrix, rhs, dRexp) = modelInteg(stateDict, uDict, pDict, outputs)
zVec = C.solve(massMatrix, rhs)
ddX = zVec[0:3]
dw = zVec[3:6]
ode = C.veccat(
[C.veccat([dx, dy, dz]),
dRexp.trans().reshape([9, 1]), ddX, dw])
if endTimeSteps is not None:
endTime, nSteps = endTimeSteps
pNames.append("endTime")
pDict["endTime"] = endTime
pVec = C.veccat([pVec, endTime])
ode = ode #/(endTime/(nSteps-1))
dae = C.SXFunction(C.daeIn(x=stateVec, p=C.veccat([uVec, pVec])),
C.daeOut(ode=ode))
return (dae, {
'xVec': stateVec,
'xDict': stateDict,
'xNames': stateNames,
'uVec': uVec,
'uNames': uNames,
'pVec': pVec,
'pNames': pNames
}, outputs)
def calc_NLL(hyper, X, Y, squaredist, meanFunc='zero', prior=None):
""" Objective function
Calculate the negative log likelihood function using Casadi SX symbols.
# Arguments:
hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn], where Nx is the
number of inputs to the GP.
X: Training data matrix with inputs of size (N x Nx).
Y: Training data matrix with outpyts of size (N x Ny),
with Ny number of outputs.
# Returns:
NLL: The negative log likelihood function (scalar)
"""
N, Nx = ca.MX.size(X)
ell = hyper[:Nx]
sf2 = hyper[Nx]**2
sn2 = hyper[Nx + 1]**2
m = get_mean_function(hyper, X.T, func=meanFunc)
# Calculate covariance matrix
K_s = ca.SX.sym('K_s', N, N)
sqdist = ca.SX.sym('sqd', N, N)
elli = ca.SX.sym('elli')
ki = ca.Function('ki', [sqdist, elli, K_s], [sqdist / elli**2 + K_s])
K1 = ca.MX(N, N)
for i in range(Nx):
K1 = ki(squaredist[:, (i * N):(i + 1) * N], ell[i], K1)
sf2_s = ca.SX.sym('sf2')
exponent = ca.SX.sym('exp', N, N)
K_exp = ca.Function('K', [exponent, sf2_s],
[sf2_s * ca.SX.exp(-.5 * exponent)])
K2 = K_exp(K1, sf2)
K = K2 + sn2 * ca.MX.eye(N)
K = (K + K.T) * 0.5 # Make sure matrix is symmentric
A = ca.SX.sym('A', ca.MX.size(K))
cholesky = ca.Function('cholesky', [A], [ca.chol(A).T])
L = cholesky(K)
B = 2 * ca.sum1(ca.SX.log(ca.diag(A)))
log_determinant = ca.Function('log_det', [A], [B])
log_detK = log_determinant(L)
Y_s = ca.SX.sym('Y', ca.MX.size(Y))
L_s = ca.SX.sym('L', ca.Sparsity.lower(N))
sol = ca.Function('sol', [L_s, Y_s], [ca.solve(L_s, Y_s)])
invLy = sol(L, Y - m(X.T))
invLy_s = ca.SX.sym('invLy', ca.MX.size(invLy))
sol2 = ca.Function('sol2', [L_s, invLy_s], [ca.solve(L_s.T, invLy_s)])
alpha = sol2(L, invLy)
alph = ca.SX.sym('alph', ca.MX.size(alpha))
detK = ca.SX.sym('det')
# Calculate hyperpriors
theta = ca.SX.sym('theta')
mu = ca.SX.sym('mu')
s2 = ca.SX.sym('s2')
prior_gauss = ca.Function(
'hyp_prior', [theta, mu, s2],
[-(theta - mu)**2 / (2 * s2) - 0.5 * ca.log(2 * ca.pi * s2)])
log_prior = 0
if prior is not None:
for i in range(Nx):
log_prior += prior_gauss(ell[i], prior['ell_mean'],
prior['ell_std']**2)
log_prior += prior_gauss(sf2, prior['sf_mean'], prior['sf_std']**2)
log_prior += prior_gauss(sn2, prior['sn_mean'], prior['sn_std']**2)
NLL = ca.Function('NLL', [Y_s, alph, detK],
[0.5 * ca.mtimes(Y_s.T, alph) + 0.5 * detK])
return NLL(Y - m(X.T), alpha, log_detK) + log_prior
def gp_exact_moment(invK, X, Y, hyper, inputmean, inputcov):
""" Gaussian Process with Exact Moment Matching
Copyright (c) 2018, Eric Bradford, Helge-André Langåker
The first and second moments are used to compute the mean and covariance of the
posterior distribution with a stochastic input distribution. This assumes a
zero prior mean function and the squared exponential kernel.
# Arguments
invK: Array with the inverse covariance matrices of size (Ny x N x N),
with Ny number of outputs from the GP and N number of training points.
X: Training data matrix with inputs of size NxNx, with Nx number of
inputs to the GP.
Y: Training data matrix with outpyts of size (N x Ny).
hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn].
inputmean: Mean from the last GP iteration of size (1 x Nx)
inputcov: Covariance matrix from the last GP iteration of size (Nx x Nx)
# Returns
mean: Array of the output mean of size (Ny x 1).
covariance: Covariance matrix of size (Ny x Ny).
"""
hyper = ca.log(hyper)
Ny = len(invK)
N, Nx = ca.MX.size(X)
mean = ca.MX.zeros(Ny, 1)
beta = ca.MX.zeros(N, Ny)
log_k = ca.MX.zeros(N, Ny)
v = X - ca.repmat(inputmean, N, 1)
covariance = ca.MX.zeros(Ny, Ny)
#TODO: Fix that LinsolQr don't work with the extended graph?
A = ca.SX.sym('A', inputcov.shape)
[Q, R2] = ca.qr(A)
determinant = ca.Function('determinant', [A], [ca.exp(ca.trace(ca.log(R2)))])
for a in range(Ny):
beta[:, a] = ca.mtimes(invK[a], Y[:, a])
iLambda = ca.diag(ca.exp(-2 * hyper[a, :Nx]))
R = inputcov + ca.diag(ca.exp(2 * hyper[a, :Nx]))
iR = ca.mtimes(iLambda, (ca.MX.eye(Nx) - ca.solve((ca.MX.eye(Nx)
+ ca.mtimes(inputcov, iLambda)), (ca.mtimes(inputcov, iLambda)))))
T = ca.mtimes(v, iR)
c = ca.exp(2 * hyper[a, Nx]) / ca.sqrt(determinant(R)) \
* ca.exp(ca.sum2(hyper[a, :Nx]))
q2 = c * ca.exp(-ca.sum2(T * v) * 0.5)
qb = q2 * beta[:, a]
mean[a] = ca.sum1(qb)
t = ca.repmat(ca.exp(hyper[a, :Nx]), N, 1)
v1 = v / t
log_k[:, a] = 2 * hyper[a, Nx] - ca.sum2(v1 * v1) * 0.5
# covariance with noisy input
for a in range(Ny):
ii = v / ca.repmat(ca.exp(2 * hyper[a, :Nx]), N, 1)
for b in range(a + 1):
R = ca.mtimes(inputcov, ca.diag(ca.exp(-2 * hyper[a, :Nx])
+ ca.exp(-2 * hyper[b, :Nx]))) + ca.MX.eye(Nx)
t = 1.0 / ca.sqrt(determinant(R))
ij = v / ca.repmat(ca.exp(2 * hyper[b, :Nx]), N, 1)
Q = ca.exp(ca.repmat(log_k[:, a], 1, N)
+ ca.repmat(ca.transpose(log_k[:, b]), N, 1)
+ maha(ii, -ij, ca.solve(R, inputcov * 0.5), N))
A = ca.mtimes(beta[:, a], ca.transpose(beta[:, b]))
if b == a:
A = A - invK[a]
A = A * Q
covariance[a, b] = t * ca.sum2(ca.sum1(A))
covariance[b, a] = covariance[a, b]
covariance[a, a] = covariance[a, a] + ca.exp(2 * hyper[a, Nx])
covariance = covariance - ca.mtimes(mean, ca.transpose(mean))
return [mean, covariance]
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 solve_chain_mass_nmpc_qp(num_masses, num_intervals):
time_step = 0.1
u, y, dot_y = get_chain_mass_ode(num_masses)
init_state = get_equilibrium_states(num_masses, y, dot_y, [0, 1, 0])
ref_state = get_equilibrium_states(num_masses, y, dot_y, [1, 0, 0])
# NMPC formulation:
num_states = y.shape[0]
num_controls = 3
weights_states = numpy.ones(y.shape)
weights_states[0:3 * num_masses] = 1e2
weights_states[3 * num_masses:6 * num_masses] = 1e0
weights_states[-3:] = 1e2
weights_controls = numpy.ones((num_controls, 1)) * 1e-2
# states 1..N
states = casadi.SX.sym('states', num_states, num_intervals)
# controls 0..N-1
controls = casadi.SX.sym('controls', num_controls, num_intervals)
weights_states_sqrt = numpy.sqrt(weights_states)
weights_controls_sqrt = numpy.sqrt(weights_controls)
objective_residuals = []
ref_control = numpy.zeros(u.shape)
for node in range(num_intervals):
objective_residuals.append(
(states[:, node] - ref_state) * weights_states_sqrt)
objective_residuals.append(
(controls[:, node] - ref_control) * weights_controls_sqrt)
objective_residuals = casadi.veccat(*objective_residuals)
ode = casadi.Function('ode', [u, y], [dot_y])
rk4_k1 = ode(u, y)
rk4_k2 = ode(u, y + time_step / 2.0 * rk4_k1)
rk4_k3 = ode(u, y + time_step / 2.0 * rk4_k2)
rk4_k4 = ode(u, y + time_step * rk4_k3)
final_y = y + time_step / 6.0 * (rk4_k1 + 2 * rk4_k2 + 2 * rk4_k3 + rk4_k4)
integrate = casadi.Function('integrate', [u, y], [final_y])
states_for_integration = casadi.horzcat(init_state, states[:, :-1])
integrated_states = integrate.map(num_intervals)(controls,
states_for_integration)
equality_constraints = states - integrated_states
equality_constraints = casadi.veccat(equality_constraints)
# Prepare and condense the underlying QP.
states_vec = casadi.veccat(states)
controls_vec = casadi.veccat(controls)
jac_obj_residuals_wrt_states = casadi.jacobian(objective_residuals,
states_vec)
jac_obj_residuals_wrt_controls = casadi.jacobian(objective_residuals,
controls_vec)
jac_eq_constraints_wrt_states = casadi.jacobian(equality_constraints,
states_vec)
jac_eq_constraints_wrt_controls = casadi.jacobian(equality_constraints,
controls_vec)
qp_h = jac_obj_residuals_wrt_controls.T @ jac_obj_residuals_wrt_controls
delta_x_contrib = casadi.solve(jac_eq_constraints_wrt_states,
jac_eq_constraints_wrt_controls)
delta_x_qp_contrib = -jac_obj_residuals_wrt_states @ delta_x_contrib
qp_h += delta_x_qp_contrib.T @ delta_x_qp_contrib
qp_g = (jac_obj_residuals_wrt_controls +
delta_x_qp_contrib).T @ objective_residuals
states_lbx = numpy.zeros(y.shape)
states_ubx = numpy.zeros(y.shape)
states_lbx.fill(-numpy.inf)
for m in range(num_masses):
states_lbx[3 * m + 1] = -0.01
states_ubx.fill(numpy.inf)
qp_lb_a = []
qp_ub_a = []
for _ in range(num_intervals):
qp_lb_a.append(states_lbx)
qp_ub_a.append(states_ubx)
qp_lb_a = numpy.concatenate(qp_lb_a, axis=0)
qp_ub_a = numpy.concatenate(qp_ub_a, axis=0)
init_states = numpy.concatenate([init_state for _ in range(num_intervals)],
axis=0)
delta_x_bound_contrib = casadi.solve(jac_eq_constraints_wrt_states,
equality_constraints) + init_states
qp_lb_a -= delta_x_bound_contrib
qp_ub_a -= delta_x_bound_contrib
qp_a = -delta_x_contrib
qp_fcn = casadi.Function('qp_h_fcn', [controls_vec, states_vec],
[qp_h, qp_g, qp_a, qp_lb_a, qp_ub_a])
init_controls = numpy.zeros(controls_vec.shape)
qp_h_eval, qp_g_eval, qp_a_eval, qp_lb_a_eval, qp_ub_a_eval = qp_fcn(
init_controls, init_states)
qp_lbx = -numpy.ones(qp_g.shape)
qp_ubx = numpy.ones(qp_g.shape)
# Reduce the number of rows of the A-matrix to the minimum.
qp_a_indices = []
for el in range(qp_lb_a.shape[0]):
if qp_lb_a_eval[el] <= -numpy.inf and qp_ub_a_eval[el] >= numpy.inf:
continue
qp_a_indices.append(el)
qp_lb_a_eval = qp_lb_a_eval[qp_a_indices]
qp_ub_a_eval = qp_ub_a_eval[qp_a_indices]
qp_a_eval = qp_a_eval[qp_a_indices, :]
qp_x = casadi.SX.sym('qp_x', *qp_g.shape)
qp = {
'x': qp_x,
'f': 0.5 * qp_x.T @ qp_h_eval @ qp_x + qp_x.T @ qp_g_eval,
'g': casadi.densify(qp_a_eval @ qp_x)
}
qp_solver = casadi.qpsol('qp_solver', 'qpoases', qp)
sol = qp_solver(lbx=qp_lbx, ubx=qp_ubx, lbg=qp_lb_a_eval, ubg=qp_ub_a_eval)
x_opt = numpy.asarray(sol['x']).flatten()
y_opt = numpy.asarray(-casadi.vertcat(sol['lam_x'], sol['lam_g']))
f_opt = sol['f']
num_variables = qp_x.shape[0]
num_constraints = qp_a_eval.shape[0]
qp_h_flat = numpy.asarray(qp_h_eval).flatten()
qp_g_flat = numpy.asarray(qp_g_eval).flatten()
qp_a_flat = numpy.asarray(qp_a_eval).flatten()
qp_lb_a_flat = numpy.asarray(qp_lb_a_eval).flatten()
qp_ub_a_flat = numpy.asarray(qp_ub_a_eval).flatten()
return (num_variables, num_constraints, qp_h_flat, qp_g_flat, qp_a_flat,
qp_lbx, qp_ubx, qp_lb_a_flat, qp_ub_a_flat, x_opt, y_opt, f_opt)
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 freeModel(endTimeSteps=None):
stateNames = [ "x" # state 0
, "y" # state 1
, "z" # state 2
, "e11" # state 3
, "e12" # state 4
, "e13" # state 5
, "e21" # state 6
, "e22" # state 7
, "e23" # state 8
, "e31" # state 9
, "e32" # state 10
, "e33" # state 11
, "dx" # state 12
, "dy" # state 13
, "dz" # state 14
, "w1" # state 15
, "w2" # state 16
, "w3" # state 17
]
uNames = [ "tc"
, "aileron"
, "elevator"
, "rudder"
# , 'ddr'
]
pNames = [ "wind_x"
]
uSyms = [C.ssym(name) for name in uNames]
uDict = dict(zip(uNames,uSyms))
uVec = C.veccat( uSyms )
pSyms = [C.ssym(name) for name in pNames]
pDict = dict(zip(pNames,pSyms))
pVec = C.veccat( pSyms )
stateSyms = [C.ssym(name) for name in stateNames]
stateDict = dict(zip(stateNames,stateSyms))
stateVec = C.veccat( stateSyms )
dx = stateDict['dx']
dy = stateDict['dy']
dz = stateDict['dz']
outputs = {}
outputs['aileron(deg)']=uDict['aileron']*180/C.pi
outputs['elevator(deg)']=uDict['elevator']*180/C.pi
outputs['rudder(deg)']=uDict['rudder']*180/C.pi
(massMatrix, rhs, dRexp) = modelInteg(stateDict, uDict, pDict, outputs)
zVec = C.solve(massMatrix, rhs)
ddX = zVec[0:3]
dw = zVec[3:6]
ode = C.veccat( [ C.veccat([dx,dy,dz])
, dRexp.trans().reshape([9,1])
, ddX
, dw
] )
if endTimeSteps is not None:
endTime,nSteps = endTimeSteps
pNames.append("endTime")
pDict["endTime"] = endTime
pVec = C.veccat([pVec,endTime])
ode = ode#/(endTime/(nSteps-1))
dae = C.SXFunction( C.daeIn( x=stateVec, p=C.veccat([uVec,pVec])),
C.daeOut( ode=ode))
return (dae, {'xVec':stateVec,'xDict':stateDict,'xNames':stateNames,
'uVec':uVec,'uNames':uNames,
'pVec':pVec,'pNames':pNames}, outputs)
# Dynamics are satisfied when this vector expression is zero. DAE_RHS = LHS-RHS #################### # Derive the ODE form of the equations of motion #################### # c + M * ddq - dLddq = generalized_forces c_ = mul(J_temp[:,:nq],dq).reshape(dLddq.shape) c = jacobianTimesVector(dLddq.T, q, dq).reshape(dLddq.shape) M = J_temp[:,nq:] # Mass matrix, i.e. the term that got multiplied by ddq # M*ddq = RHS RHS = f_gen + dLdq.T - c.T RHS2 = solve(M,RHS) # ddq = RHS2 Symbolic system solve. ODE_RHS = vertcat([dq,RHS2]) # d[q;dq]/dt = [dq; RHS2] #################### # Function for setpoint computation. #################### #################### # Linearize the system about a setpoint and determine stability #################### #################### # Debugging and testing stuff... ####################
pl.ones(3), \
pl.zeros(N), \
ydata_noise[0,:].T
])
sol = nlpsolver(x0 = V0)
p_est_single_shooting = sol["x"][:3]
tstart_Sigma_p = time()
J_s = ca.jacobian(r, V)
F_s = ca.mul(J_s.T, J_s)
beta = (ca.mul(r.T, r) / (r.size() - V.size()))
Sigma_p_s = beta * ca.solve(F_s, ca.MX.eye(F_s.shape[0]), "csparse")
beta_fcn = ca.MXFunction("beta_fcn", [V], [beta])
print beta_fcn([sol["x"]])[0]
Sigma_p_s_fcn = ca.MXFunction("Sigma_p_s_fcn", \
[V] , [Sigma_p_s])
Cov_p = Sigma_p_s_fcn([sol["x"]])[0][:3, :3]
tend_Sigma_p = time()
time_covariance_matrix_evlaution.append(tend_Sigma_p - tstart_Sigma_p)
def __init__(self,
m1=1.0,
m2=0.1,
l=0.5,
g=9.81,
u_max=10.0,
d_max=2.0,
tf=2.0,
N=5,
Q=np.array([100, 150, 5, 5]),
Qf=np.array([10, 50, 5, 5]),
R=5,
verbose=0):
"""
Initializes a casadi optimization model that solves a trajectory optimization
problem using direct hermite-simpson collocation in separated form, where the
objective is finite horizon target state tracking using LQR costs.
The mathematical expressions for collocation are based on Matthew Kelly 2017,
"An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation"
Note: In hermite simpson collocation in separated form, a finite element consists
of three collocation points: left endpoint, midpoint, and right endpoint.
Each finite element is indexed using the odd indices of the collocation points,
as these correspond to the midpoints of the finite elements
"""
self.nx = 4 #dimension of state vector
self.nu = 1 #dimension of input vector
self.m1 = m1 #cart mass
self.m2 = m2 #pendulum mass
self.l = l #pendulum length
self.g = g #gravity
self.u_max = u_max #maximum actuator force
self.d_max = d_max #extent of rail that cart travels on
self.tf = tf #optimization horizon
self.N = N #number of finite elements (number of collocation points = 2*N+1)
self.Q = Q #state error cost weight
self.Qf = Qf #final state error cost weight
self.R = R #input regulation cost weight
# integer from 0 to 12 indicating how much IPOPT output to print
self.verbose = verbose
# initialize optimization solution memory
self.reset_sol_prev()
# casadi optimization model
self.opti = ca.Opti()
# parameters
self.x_init = self.opti.parameter(self.nx) # initial state
self.x_des = self.opti.parameter(self.nx,
2 * self.N + 1) # desired states
# decision variables
self.X = self.opti.variable(self.nx, 2 * self.N +
1) # state at each collocation point
self.U = self.opti.variable(self.nu, 2 * self.N +
1) # input at each collocation point
# objective
# simpson quadrature coefficients, to be used to compute integrals
simp = np.empty((1, 2 * N + 1))
simp[0, ::2] = 2
simp[0, 1::2] = 4
simp[0, 0], simp[0, -1] = 1, 1
# cost = finite horizon LQR cost computed with simpson quadrature
J = 0.0
J += ca.dot(simp, self.U[:, :] * self.U[:, :])
for j in range(self.nx):
if j == 1:
# angle error must be computed using trig functions to account for
# periodicity
J += self.Q[j] * ca.dot(simp,(ca.cos(self.X[j,:])-ca.cos(self.x_des[j,:])) \
*(ca.cos(self.X[j,:])-ca.cos(self.x_des[j,:])))
J += self.Q[j] * ca.dot(simp,(ca.sin(self.X[j,:])-ca.sin(self.x_des[j,:])) \
*(ca.sin(self.X[j,:])-ca.sin(self.x_des[j,:])))
J += self.Qf[j] * (ca.cos(self.X[j,-1])-ca.cos(self.x_des[j,-1])) \
*(ca.cos(self.X[j,-1])-ca.cos(self.x_des[j,-1]))
J += self.Qf[j] * (ca.sin(self.X[j,-1])-ca.sin(self.x_des[j,-1])) \
*(ca.sin(self.X[j,-1])-ca.sin(self.x_des[j,-1]))
else:
J += self.Q[j] * ca.dot(simp, (self.X[j,:]-self.x_des[j,:]) \
*(self.X[j,:]-self.x_des[j,:]))
J += self.Qf[j] * (self.X[j,-1]-self.x_des[j,-1]) \
*(self.X[j,-1]-self.x_des[j,-1])
self.opti.minimize(J)
# position bound constraint
self.opti.subject_to(self.opti.bounded( \
np.full(self.X[0,:].shape, -self.d_max),
self.X[0,:],
np.full(self.X[0,:].shape, self.d_max)
))
# control bound constraint
self.opti.subject_to(self.opti.bounded( \
np.full(self.U[:,:].shape, -self.u_max),
self.U[:,:],
np.full(self.U[:,:].shape, self.u_max)
))
# initial condition constraint
self.opti.subject_to(self.X[:, 0] == self.x_init)
# dynamics
# symbolic variables used to derive equations of motion
x = ca.SX.sym('x', self.nx) #state
u = ca.SX.sym('u', self.nu) #control
m1, m2, l, g = self.m1, self.m2, self.l, self.g
# equations of motion taken from Russ Tedrake underactuated robotics ch.3
M = ca.SX(np.array([ \
[m1 + m2, m2*l*ca.cos(x[1])],
[m2*l*ca.cos(x[1]), m2*l**2]
]))
C = ca.SX(np.array([ \
[0.0, -m2*l*x[3]*ca.sin(x[1])],
[0.0, 0.0]
]))
tau_g = ca.SX(np.array([ \
[0.0],
[-m2*g*l*ca.sin(x[1])]
]))
B = ca.SX(np.array([ \
[1.0],
[0.0]
]))
xdot = ca.vertcat( \
x[2:], # d/dt(q) = qdot
ca.solve(M, -C@x[2:]+tau_g+B@u) # M@qddot = (-C@qdot+tau_g+B@u)
)
f = ca.Function('f', [x, u], [xdot]) # xdot = f(x,u)
for i in range(2 * self.N + 1):
if i % 2 != 0:
# for each finite element:
x_left, x_mid, x_right = self.X[:, i -
1], self.X[:, i], self.X[:,
i + 1]
u_left, u_mid, u_right = self.U[:, i -
1], self.U[:, i], self.U[:,
i + 1]
f_left, f_mid, f_right = f(x_left, u_left), f(x_mid, u_mid), f(
x_right, u_right)
# interpolation constraints
self.opti.subject_to( \
# equation (6.11) in Kelly 2017
x_mid == (x_left+x_right)/2.0 + self.tf/self.N*(f_left-f_right)/8.0)
# collocation constraints
self.opti.subject_to( \
# equation (6.12) in Kelly 2017
self.tf/self.N*(f_left+4*f_mid+f_right)/6.0 == x_right-x_left)
def __init__(self,NR=4,debug=False,quatnorm=False):
"""
Keyword arguments:
NR -- the number of rotors
debug -- wether to print out debug info
quatnorm -- add the quaternion norm to the DAE rhs
"""
# ----------- system states and their derivatives ----
pos = struct_symSX(["x","y","z"]) # rigid body centre of mass position [m] {0}
v = struct_symSX(["vx","vy","vz"]) # rigid body centre of mass position velocity [m/s] {0}
NR = 4 # Number of rotors
states = struct_symSX([
entry("p",struct=pos),
entry("v",struct=v),
entry("q",shape=4), # quaternions {0} -> {1}
entry("w",shape=3), # rigid body angular velocity w_101 [rad/s] {1}
entry("r",shape=NR) # spin speed of rotor, wrt to platform. [rad/s] Should be positive!
# The signs are such that positive means lift generating, regardless of spin direction.
])
pos, v, q, w, r = states[...]
# ------------------------------------------------
dist = struct_symSX([
entry("Faer",shape=NR), # Disturbance on aerodynamic forcing [N]
entry("Caer",shape=NR) # Disturbance on aerodynamic torques [Nm]
])
# ----------------- Controls ---------------------
controls = struct_symSX([
entry("CR",shape=NR) # [Nm]
# Torques of the motors that drive the rotors, acting from platform on propeller
# The torque signs are always positive when putting energy in the propellor,
# regardless of spin direction.
#
])
CR = controls["CR"]
# ------------------------------------------------
# ---------------- Temporary symbols --------------
F = ssym("F",3) # Forces acting on the platform in {1} [N]
C = ssym("C",3) # Torques acting on the platform in {1} [Nm]
rotors_Faer = [ssym("Faer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic force acting on propeller {1} [N]
rotors_Caer = [ssym("Caer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic torques acting on propeller {1} [Nm]
# ---------------------------------------------------
# ----------------- Parameters ---------------------
rotor_model = struct_symSX([
"c", # c Cord length [m]
"R", # R Radius of propeller [m]
"CL_alpha", # CL_alpha Lift coefficient [-]
"alpha_0", # alpha_0
"CD_alpha", # CD_alpha Drag coefficient [-]
"CD_i", # CD_i Induced drag coefficient [-]
])
p = struct_symSX([
entry("rotors_model",repeat=NR,struct=rotor_model), # Parameters that describe the rotor model
entry("rotors_I",repeat=NR,shape=sp_diag(3)), # Inertias of rotors [kg.m^2]
entry("rotors_spin",repeat=NR), # Direction of spin from each rotor. 1 means rotation around positive z.
entry("rotors_p",repeat=NR,shape=3), # position of rotors in {1} [m],
entry("I",sym=casadi.diag(ssym("[Ix,Iy,Iz]"))), # Inertia of rigid body [kg.m^2]
"m", # Mass of the whole system [kg]
"g", # gravity [m/s^2]
"rho", # Air density [kg/m^3]
])
I,m,g,rho = p[["I","m","g","rho"]]
# --------------------------------------------------
# ----------------- Parameters fillin's ---------------------
p_ = p()
p_["rotors_spin"] = [1,-1,1,-1]
p_["rotors_model",:,{}] = { "c": 0.01, "R" : 0.127, "CL_alpha": 6.0, "alpha_0": 0.15, "CD_alpha": 0.02, "CD_i": 0.05} # c Cord length [m]
p_["m"] = 0.5 # [kg]
p_["g"] = 9.81 # [N/kg]
p_["rho"] = 1.225 # [kg/m^3]
L = 0.25
I_max = p_["m"] * L**2 # Inertia of a point mass at a distance L
I_ref = I_max/5
p_["I"] = casadi.diag([I_ref/2,I_ref/2,I_ref]) # [N.m^2]
p_["rotors_p",0] = DM([L,0,0])
p_["rotors_p",1] = DM([0,L,0])
p_["rotors_p",2] = DM([-L,0,0])
p_["rotors_p",3] = DM([0,-L,0])
for i in range(NR):
R_ = p_["rotors_model",i,"R"] # Radius of propeller [m]
m_ = 0.01 # Mass of a propeller [kg]
I_max = m_ * R_**2 # Inertia of a point mass
I_ref = I_max/5
p_["rotors_I",i] = casadi.diag([I_ref/2,I_ref/2,I_ref])
if debug:
print p.vecNZcat()
dist_ = dist(0)
# ----------------- Scaling ---------------------
scaling_states = states(1)
scaling_controls = controls(1)
scaling_states["r"] = 500
scaling_controls["CR"] = 0.005
scaling_dist = dist()
scaling_dist["Faer"] = float(p_["m"]*p_["g"]/NR)
scaling_dist["Caer"] = 0.0026
# ----------- Frames ------------------
T_10 = mul(tr(*pos),Tquat(*q))
T_01 = kin_inv(T_10)
R_10 = T2R(T_10)
R_01 = R_10.T
# -------------------------------------
dstates = struct_symSX(states)
dp,dv,dq,dw,dr = dstates[...]
res = struct_SX(states) # DAE residual hand side
# ----------- Dynamics of the body ----
res["p"] = v - dp
# Newton, but transform the force F from {1} to {0}
res["v"] = mul(R_10,F) - m*dv
# Kinematics of the quaterion.
res["q"] = mul(quatDynamics(*q),w)-dq
# This is a trick by Sebastien Gros to stabilize the quaternion evolution equation
res["q"] += -q*(sumAll(q**2)-1)
# Agular impulse H_1011
H = mul(p["I"],w) # Due to platform
for i in range(NR):
H+= mul(p["rotors_I",i], w + vertcat([0,0,p["rotors_spin",i]*r[i]])) # Due to rotor i
dH = mul(jacobian(H,w),dw) + mul(jacobian(H,q),dq) + mul(jacobian(H,r),dr) + casadi.cross(w,H)
res["w"] = C - dH
for i in range(NR):
res["r",i] = CR[i] + p["rotors_spin",i]*rotors_Caer[i][2] - p["rotors_I",i][2]*(dr[i]+dw[2]) # Dynamics of rotor i
# ---------------------------------
# Make a vector of f ?
#if quatnorm:
# f = vertcat(f+[sumAll(q**2)-1])
#else:
# f = vertcat(f)
# ------------ Force model ------------
Fg = mul(R_01,vertcat([0,0,-g*m]))
F_total = Fg + sum(rotors_Faer) # Total force acting on the platform
C_total = SX([0,0,0]) # Total torque acting on the platform
for i in range(NR):
C_total[:2] += rotors_Caer[i][:2] # The x and y components propagate
C_total[2] -= p["rotors_spin",i]*CR[i] # the z compent moves through a serparate system
C_total += casadi.cross(p["rotors_p",i],rotors_Faer[i]) # Torques due to thrust
res = substitute(res,F,F_total)
res = substitute(res,C,C_total)
subs_before = []
subs_after = []
v_global = mul(R_01,v)
u_z = SX([0,0,1])
# Now fill in the aerodynamic forces
for i in range(NR):
c,R,CL_alpha,alpha_0, CD_alpha, CD_i = p["rotors_model",i,...]
#Bristeau P-jean, Martin P, Salaun E, Petit N. The role of propeller aerodynamics in the model of a quadrotor UAV. In: Proceedings of the European Control Conference 2009.; 2009:683-688.
v_local = v_global + (casadi.cross(w,p["rotors_p",i])) # Velocity at rotor i
rotors_Faer_physics = (rho*c*R**3*r[i]**2*CL_alpha*(alpha_0/3.0-v_local[2]/(2.0*R*r[i]))) * u_z
subs_before.append(rotors_Faer[i])
subs_after.append(rotors_Faer_physics + dist["Faer",i])
rotors_Caer_physics = -p["rotors_spin",i]*rho*c*R**4*r[i]**2*(CD_alpha/4.0+CD_i*alpha_0**2*(alpha_0/4.0-2.0*v_local[2]/(3.0*r[i]*R))-CL_alpha*v_local[2]/(r[i]*R)*(alpha_0/3.0-v_local[2]/(2.0*r[i]*R))) * u_z
subs_before.append(rotors_Caer[i])
subs_after.append(rotors_Caer_physics + dist["Caer",i])
res = substitute(res,veccat(subs_before),veccat(subs_after))
# Make an explicit ode
rhs = - casadi.solve(jacobian(res,dstates),substitute(res,dstates,0))
# --------------------------------------
self.res_w = res
self.res = substitute(res,dist,dist_)
self.res_ = substitute(self.res,p,p_)
resf = SXFunction([dstates, states, controls ],[self.res_])
resf.init()
self.resf = resf
self.rhs_w = rhs
self.rhs = substitute(rhs,dist,dist_)
self.rhs_ = substitute(self.rhs,p,p_)
t = SX("t")
# We end up with a DAE that captures the system dynamics
dae = SXFunction(daeIn(t=t,x=states,p=controls),daeOut(ode=self.rhs_))
dae.init()
self.dae = dae
cdae = SXFunction(controldaeIn(t=t, x=states, u= controls,p=p),daeOut(ode=self.rhs))
cdae.init()
self.cdae = cdae
self.states = states
self.dstates = dstates
self.p = p
self.p_ = p_
self.controls = controls
self.NR = NR
self.w = dist
self.w_ = dist_
self.t = t
self.states_ = states()
self.dstates_ = states()
self.controls_ = controls()
self.scaling_states = scaling_states
self.scaling_controls = scaling_controls
self.scaling_dist = scaling_dist
def __create_reference(self, wref, tuning, lam_g_ref):
""" Create periodic reference and tuning data.
"""
# period of reference
self.__Nref = len(wref['u'])
# create reference and tuning sequence
# for each starting point in period
ref_pr = []
ref_du = []
ref_du_struct = []
H = []
q = []
for k in range(self.__Nref):
# reference primal solution
refk = []
for j in range(self.__N):
refk += [
wref['x', (k + j) % self.__Nref],
wref['u', (k + j) % self.__Nref]
]
if 'us' in self.__vars:
refk += [wref['us', (k + j) % self.__Nref]]
refk.append(wref['x', (k + self.__N) % self.__Nref])
# reference dual solution
lamgk = self.__g(0.0)
lamgk['init'] = -lam_g_ref['dyn', (k - 1) % self.__Nref]
for j in range(self.__N):
lamgk['dyn', j] = lam_g_ref['dyn', (k + j) % self.__Nref]
if 'g' in list(lamgk.keys()):
lamgk['g', j] = lam_g_ref['g', (k + j) % self.__Nref]
if 'h' in list(lamgk.keys()):
lam_h = [lam_g_ref['h', (k + j) % self.__Nref]]
if 'usc' in self.__vars:
lam_h += [-self.__scost] # TODO not entirely correct
lamgk['h', j] = ct.vertcat(*lam_h)
lamgk['term'] = self.__p_operator(
lam_g_ref['dyn', (k + self.__N - 1) % self.__Nref])
# adjust dual solution of terminal constraint is projected
if self.__nx_term != self.__nx:
# find new terminal multiplier
A_m = []
b_m = []
A_factor = ca.DM.eye(self.__nx)
for j in range(self.__N):
A_m.append(
ct.mtimes(
ct.mtimes(
self.__S['B'][(self.__N - j - 1) %
self.__Nref].T, A_factor),
self.__jac_p_operator(ca.DM.ones(self.__nx, 1)).T))
b_m.append(
ct.mtimes(
ct.mtimes(
self.__S['B'][(self.__N - j - 1) %
self.__Nref].T, A_factor),
lam_g_ref['dyn',
(k + self.__N - 1) % self.__Nref]))
A_factor = ct.mtimes(
self.__S['A'][(self.__N - j - 1) % self.__Nref].T,
A_factor)
A_m = ct.vertcat(*A_m)
b_m = ct.vertcat(*b_m)
LI_indeces = [
] # indeces of first full rank number linearly independent rows
R0 = 0
for i in range(A_m.shape[0]):
R = np.linalg.matrix_rank(A_m[LI_indeces + [i], :])
if R > R0:
LI_indeces.append(i)
R0 = R
lamgk['term'] = ca.solve(A_m[LI_indeces, :],
b_m[LI_indeces, :])
# recursively update dynamics multipliers
delta_lam = -lam_g_ref['dyn', (k + self.__N - 1) %
self.__Nref] + ct.mtimes(
self.__jac_p_operator(
ca.DM.ones(self.__nx, 1)).T,
lamgk['term'])
lamgk['dyn', self.__N - 1] += delta_lam
for j in range(1, self.__N + 1):
delta_lam = ct.mtimes(
self.__S['A'][(self.__N - j) % self.__Nref].T,
delta_lam)
if j < self.__N:
lamgk['dyn', self.__N - 1 - j] += delta_lam
else:
lamgk['init'] += -delta_lam
ref_pr.append(ct.vertcat(*refk))
ref_du.append(lamgk.cat)
ref_du_struct.append(lamgk)
if tuning is not None:
H.append([
tuning['H'][(k + j) % self.__Nref] for j in range(self.__N)
])
q.append([
tuning['q'][(k + j) % self.__Nref] for j in range(self.__N)
])
self.__ref = ref_pr
self.__ref_du = ref_du
self.__ref_du_struct = ref_du_struct
self.__Href = H
self.__qref = q
return None
def solve(a, b, solver):
return ca.solve(a, b, solver)
def inverse_symbolic(x): assert type(x)==casadi.SXMatrix xinv = casadi.solve(x, casadi.SXMatrix.eye(x.size1())) return xinv
def _priori_update(self, x_mean, x_cov, u, p, theta):
x_samples = self._get_sampled_states(x_mean, x_cov)
# Perform predictions via simulation
simulation_results = []
x_cal_x_k_at_k_minus_1 = []
y_alg_cal_x_k_at_k_minus_1 = []
for s in range(self.n_samples):
x_0_i = DM(x_samples[s])
simulation_results_i = self.model.simulate(x_0=x_0_i,
t_0=self.t,
t_f=self.t + self.t_s,
u=u,
p=p,
theta=theta,
y_0=self.y_guess)
simulation_results.append(simulation_results_i)
x_cal_x_k_at_k_minus_1.append(
simulation_results_i.final_condition()[0])
y_alg_cal_x_k_at_k_minus_1.append(
simulation_results[s].final_condition()[1])
# fit the polynomial for x
a_x = []
x_hat_k_minus = []
for i in range(self.model.n_x):
x_i_vector = vertcat(
*[x_cal_x_k_at_k_minus_1[s][i] for s in range(self.n_samples)])
a_x.append(mtimes(self._ls_factor, x_i_vector))
# get the mean for x
for i in range(self.model.n_x):
x_hat_k_minus.append(a_x[i][0])
x_hat_k_minus = vertcat(*x_hat_k_minus)
# get the covariance for x
p_k_minus = DM.zeros(self.model.n_x, self.model.n_x)
for i in range(self.model.n_x):
for j in range(self.model.n_x):
p_k_minus[i, j] = sum(
[a_x[i][k] * a_x[j][k]
for k in range(1, self.n_samples)]) + self.r_v[i, j]
# calculate the measurement for each sample
y_cal_k_at_k_minus_1 = []
for s in range(self.n_samples):
y_cal_k_at_k_minus_1.append(
self._get_measurement_from_prediction(
x_cal_x_k_at_k_minus_1[s], y_alg_cal_x_k_at_k_minus_1[s],
u))
n_meas = y_cal_k_at_k_minus_1[0].numel()
# find the measurements estimate
a_meas = []
y_meas_hat_k_minus = []
for i in range(n_meas):
y_meas_i_vector = vertcat(
*[y_cal_k_at_k_minus_1[s][i] for s in range(self.n_samples)])
a_meas.append(mtimes(self._ls_factor, y_meas_i_vector))
# get the mean for the measurement
for i in range(n_meas):
y_meas_hat_k_minus.append(a_meas[i][0])
y_meas_hat_k_minus = vertcat(*y_meas_hat_k_minus)
# get the covariance for the meas
p_yk_yk = DM.zeros(n_meas, n_meas)
for i in range(n_meas):
for j in range(n_meas):
p_yk_yk[i, j] = sum([
a_meas[i][k] * a_meas[j][k]
for k in range(1, self.n_samples)
])
p_yk_yk = p_yk_yk + self.r_n
# get cross-covariance
p_xk_yk = DM.zeros(self.model.n_x, n_meas)
for i in range(self.model.n_x):
for j in range(n_meas):
p_xk_yk[i, j] = sum([
a_x[i][k] * a_meas[j][k] for k in range(1, self.n_samples)
])
# k_gain = mtimes(p_xk_yk, inv(p_yk_yk))
k_gain = solve(p_yk_yk.T, p_xk_yk.T).T
return x_hat_k_minus, p_k_minus, y_meas_hat_k_minus, p_yk_yk, k_gain
def solve(a, b, solver):
return ca.solve(a, b, solver)
def __init__(self, inertial_frame_id='world'):
Vehicle.__init__(self, inertial_frame_id)
# Declaring state variables
## Generalized position vector
self.eta = casadi.SX.sym('eta', 6)
## Generalized velocity vector
self.nu = casadi.SX.sym('nu', 6)
# Build the Coriolis matrix
self.CMatrix = casadi.SX.zeros(6, 6)
S_12 = -cross_product_operator(
casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6]))
S_22 = -cross_product_operator(
casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6]))
self.CMatrix[0:3, 3:6] = S_12
self.CMatrix[3:6, 0:3] = S_12
self.CMatrix[3:6, 3:6] = S_22
# Build the damping matrix (linear and nonlinear elements)
self.DMatrix = -casadi.diag(self._linear_damping)
self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
self.DMatrix -= casadi.diag(self._quad_damping * self.nu)
# Build the restoring forces vectors wrt the BODY frame
Rx = np.array(
[[1, 0, 0],
[0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]),
casadi.cos(self.eta[3])]])
Ry = np.array(
[[casadi.cos(self.eta[4]), 0,
casadi.sin(self.eta[4])], [0, 1, 0],
[-1 * casadi.sin(self.eta[4]), 0,
casadi.cos(self.eta[4])]])
Rz = np.array(
[[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
[casadi.sin(self.eta[5]),
casadi.cos(self.eta[5]), 0], [0, 0, 1]])
R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))
if inertial_frame_id == 'world_ned':
Fg = casadi.SX([0, 0, -self.mass * self.gravity])
Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
else:
Fg = casadi.SX([0, 0, self.mass * self.gravity])
Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])
self.gVec = casadi.SX.zeros(6)
self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)
self.gVec[3:6] = -1 * casadi.mtimes(
R_n_to_b,
casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
# Build Jacobian
T = 1 / casadi.cos(self.eta[4]) * np.array(
[[
0,
casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]),
casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])
],
[
0,
casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]),
-casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])
], [0, casadi.sin(self.eta[3]),
casadi.cos(self.eta[3])]])
self.eta_dot = casadi.vertcat(
casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]),
casadi.mtimes(T, self.nu[3::]))
self.u = casadi.SX.sym('u', 6)
self.nu_dot = casadi.solve(
self._Mtotal, self.u - casadi.mtimes(self.CMatrix, self.nu) -
casadi.mtimes(self.DMatrix, self.nu) - self.gVec)
def _create_augmented_model_and_p_update(self, model):
"""
:type model: SystemModel
"""
aug_model = SystemModel("aug_linearized_EKF_model")
aug_model.include_state(self.model.x_sym)
aug_model.include_state(self.model.y_sym)
aug_model.include_control(self.model.u_sym)
aug_model.include_parameter(self.model.p_sym)
aug_model.include_theta(self.model.theta_sym)
# remove u_par (self.model.u_par model.u_par)
all_sym = list(self.model.all_sym)
# Mean
a_func = Function("A_matrix", all_sym,
[jacobian(self.model.ode, self.model.x_sym)])
b_func = Function("B_matrix", all_sym,
[jacobian(self.model.ode, self.model.y_sym)])
c_func = Function("C_matrix", all_sym,
[jacobian(self.model.alg, self.model.x_sym)])
d_func = Function("D_matrix", all_sym,
[jacobian(self.model.alg, self.model.y_sym)])
x_lin = aug_model.create_parameter("x_lin", self.model.n_x)
y_lin = aug_model.create_parameter("y_lin", self.model.n_y)
all_sym[1] = x_lin
all_sym[2] = y_lin
a_matrix = a_func(*all_sym)
b_matrix = b_func(*all_sym)
c_matrix = c_func(*all_sym)
d_matrix = d_func(*all_sym)
a_aug_matrix = vertcat(
horzcat(a_matrix, b_matrix),
horzcat(
mtimes(-solve(d_matrix, c_matrix), a_matrix),
mtimes(-solve(d_matrix, c_matrix), b_matrix),
),
)
x_aug = vertcat(model.x_sym, model.y_sym)
aug_model.include_equations(ode=[mtimes(a_aug_matrix, x_aug)])
# Covariance
gamma = vertcat(DM.eye(self.model.n_x), -solve(d_matrix, c_matrix))
trans_matrix_sym = SX.sym("trans_matrix", a_aug_matrix.shape)
p_matrix_sym = SX.sym("p_matrix",
(model.n_x + model.n_y, model.n_x + model.n_y))
gamma_matrix_sym = SX.sym("gamma_matrix", gamma.shape)
q_matrix_sym = SX.sym("q_matrix", (self.model.n_x, self.model.n_x))
p_kpp = mtimes(trans_matrix_sym,
mtimes(p_matrix_sym, trans_matrix_sym.T)) + mtimes(
gamma_matrix_sym,
mtimes(q_matrix_sym, gamma_matrix_sym.T))
a_aug_matrix_func = Function("trans_matrix", all_sym, [a_aug_matrix])
gamma_func = Function("gamma", all_sym, [gamma])
p_update_func = Function(
"p_update",
[trans_matrix_sym, p_matrix_sym, gamma_matrix_sym, q_matrix_sym],
[p_kpp],
)
return aug_model, a_aug_matrix_func, gamma_func, p_update_func
def _estimate_standard_ukf(self, t_k, y_k, u_k):
if not self._checked:
self._check()
self._checked = True
if not self._types_fixed:
self._fix_types()
self._types_fixed = True
x_mean = self.x_mean
x_cov = self.p_k
# obtain the weights
sigma_points, weights_m, weights_c = self._get_sigma_points_and_weights(
x_mean, x_cov)
# Obtain the unscented transformation points via simulation
simulation_results = []
x_ut_list = []
y_ut_list = []
x_aug_ut_list = []
meas_ut_list = []
for i in range(self.n_sigma_points):
x_0_i = sigma_points[i]
simulation_results_i = self.model.simulate(x_0=x_0_i,
t_0=self.t,
t_f=self.t + self.t_s,
u=u_k,
p=self.p,
theta=self.theta,
y_0=self.y_guess)
simulation_results.append(simulation_results_i)
x_ut_list.append(simulation_results_i.final_condition()[0])
y_ut_list.append(simulation_results[i].final_condition()[1])
x_aug_ut_list.append(vertcat(x_ut_list[-1], y_ut_list[-1]))
meas_ut_list.append(
self._get_measurement_from_prediction(x_ut_list[i],
y_ut_list[i], u_k))
# Obtain the means
x_aug_pred = sum([
weights_m[i] * x_aug_ut_list[i] for i in range(self.n_sigma_points)
])
x_pred = x_aug_pred[:self.model.n_x]
meas_pred = sum([
weights_m[i] * meas_ut_list[i] for i in range(self.n_sigma_points)
])
# Compute the covariances
cov_x_aug_pred = sum([
weights_c[i] * mtimes((x_aug_ut_list[i] - x_aug_pred),
(x_aug_ut_list[i] - x_aug_pred).T)
for i in range(self.n_sigma_points)
])
cov_x_aug_pred[:self.model.n_x, :self.model.n_x] += self.r_v
cov_meas_pred = sum([
weights_c[i] * mtimes((meas_ut_list[i] - meas_pred),
(meas_ut_list[i] - meas_pred).T)
for i in range(self.n_sigma_points)
]) + self.r_n
cov_xmeas_pred = sum([
weights_c[i] * mtimes((x_aug_ut_list[i] - x_aug_pred),
(meas_ut_list[i] - meas_pred).T)
for i in range(self.n_sigma_points)
])
# Calculate the gain
k_gain = solve(cov_meas_pred.T, cov_xmeas_pred.T).T
# Correct prediction of the state estimation
x_mean = x_aug_pred + mtimes(k_gain, (y_k - meas_pred))
meas_corr = self._get_measurement_from_prediction(
x_mean[:self.model.n_x], x_mean[self.model.n_x:], u_k)
print('Predicted state: {}'.format(x_pred))
print('Prediction error: {}'.format(y_k - meas_pred))
print('Correction: {}'.format(mtimes(k_gain, (y_k - meas_pred))))
print('Corrected state: {}'.format(x_mean))
print('Measurement: {}'.format(y_k))
print('Corrected Meas.: {}'.format(meas_corr))
# Correct covariance prediction
cov_x_aug = cov_x_aug_pred - mtimes(k_gain,
mtimes(cov_meas_pred, k_gain.T))
self.x_mean = x_mean
self.p_k = cov_x_aug
# Save variables in local object
self._x_aug = x_mean
self.x_mean = x_mean[:self.model.n_x]
self._p_k_aug = cov_x_aug_pred
self.p_k = cov_x_aug[:self.model.n_x, :self.model.n_x]
# Save in the data set
self.dataset.insert_data('x', t_k, self.x_mean)
self.dataset.insert_data('y', t_k, x_mean[self.model.n_x:])
self.dataset.insert_data('meas', t_k, meas_corr)
self.dataset.insert_data('P', t_k, vec(self.p_k))
self.dataset.insert_data('P_y', t_k, cov_x_aug[self.model.n_x:,
self.model.n_x:])
return x_mean, cov_x_aug
def __init__(self, NR=4, debug=False, quatnorm=False):
"""
Keyword arguments:
NR -- the number of rotors
debug -- wether to print out debug info
quatnorm -- add the quaternion norm to the DAE rhs
"""
# ----------- system states and their derivatives ----
pos = struct_symSX(["x", "y", "z"
]) # rigid body centre of mass position [m] {0}
v = struct_symSX(
["vx", "vy",
"vz"]) # rigid body centre of mass position velocity [m/s] {0}
NR = 4 # Number of rotors
states = struct_symSX([
entry("p", struct=pos),
entry("v", struct=v),
entry("q", shape=4), # quaternions {0} -> {1}
entry("w",
shape=3), # rigid body angular velocity w_101 [rad/s] {1}
entry(
"r", shape=NR
) # spin speed of rotor, wrt to platform. [rad/s] Should be positive!
# The signs are such that positive means lift generating, regardless of spin direction.
])
pos, v, q, w, r = states[...]
# ------------------------------------------------
dist = struct_symSX([
entry("Faer", shape=NR), # Disturbance on aerodynamic forcing [N]
entry("Caer", shape=NR) # Disturbance on aerodynamic torques [Nm]
])
# ----------------- Controls ---------------------
controls = struct_symSX([
entry("CR", shape=NR) # [Nm]
# Torques of the motors that drive the rotors, acting from platform on propeller
# The torque signs are always positive when putting energy in the propellor,
# regardless of spin direction.
#
])
CR = controls["CR"]
# ------------------------------------------------
# ---------------- Temporary symbols --------------
F = ssym("F", 3) # Forces acting on the platform in {1} [N]
C = ssym("C", 3) # Torques acting on the platform in {1} [Nm]
rotors_Faer = [
ssym("Faer_%d" % i, 3, 1) for i in range(NR)
] # Placeholder for aerodynamic force acting on propeller {1} [N]
rotors_Caer = [
ssym("Caer_%d" % i, 3, 1) for i in range(NR)
] # Placeholder for aerodynamic torques acting on propeller {1} [Nm]
# ---------------------------------------------------
# ----------------- Parameters ---------------------
rotor_model = struct_symSX([
"c", # c Cord length [m]
"R", # R Radius of propeller [m]
"CL_alpha", # CL_alpha Lift coefficient [-]
"alpha_0", # alpha_0
"CD_alpha", # CD_alpha Drag coefficient [-]
"CD_i", # CD_i Induced drag coefficient [-]
])
p = struct_symSX([
entry("rotors_model", repeat=NR, struct=rotor_model
), # Parameters that describe the rotor model
entry("rotors_I", repeat=NR,
shape=sp_diag(3)), # Inertias of rotors [kg.m^2]
entry(
"rotors_spin", repeat=NR
), # Direction of spin from each rotor. 1 means rotation around positive z.
entry("rotors_p", repeat=NR,
shape=3), # position of rotors in {1} [m],
entry("I", sym=casadi.diag(
ssym("[Ix,Iy,Iz]"))), # Inertia of rigid body [kg.m^2]
"m", # Mass of the whole system [kg]
"g", # gravity [m/s^2]
"rho", # Air density [kg/m^3]
])
I, m, g, rho = p[["I", "m", "g", "rho"]]
# --------------------------------------------------
# ----------------- Parameters fillin's ---------------------
p_ = p()
p_["rotors_spin"] = [1, -1, 1, -1]
p_["rotors_model", :, {}] = {
"c": 0.01,
"R": 0.127,
"CL_alpha": 6.0,
"alpha_0": 0.15,
"CD_alpha": 0.02,
"CD_i": 0.05
} # c Cord length [m]
p_["m"] = 0.5 # [kg]
p_["g"] = 9.81 # [N/kg]
p_["rho"] = 1.225 # [kg/m^3]
L = 0.25
I_max = p_["m"] * L**2 # Inertia of a point mass at a distance L
I_ref = I_max / 5
p_["I"] = casadi.diag([I_ref / 2, I_ref / 2, I_ref]) # [N.m^2]
p_["rotors_p", 0] = DMatrix([L, 0, 0])
p_["rotors_p", 1] = DMatrix([0, L, 0])
p_["rotors_p", 2] = DMatrix([-L, 0, 0])
p_["rotors_p", 3] = DMatrix([0, -L, 0])
for i in range(NR):
R_ = p_["rotors_model", i, "R"] # Radius of propeller [m]
m_ = 0.01 # Mass of a propeller [kg]
I_max = m_ * R_**2 # Inertia of a point mass
I_ref = I_max / 5
p_["rotors_I", i] = casadi.diag([I_ref / 2, I_ref / 2, I_ref])
if debug:
print p.vecNZcat()
dist_ = dist(0)
# ----------------- Scaling ---------------------
scaling_states = states(1)
scaling_controls = controls(1)
scaling_states["r"] = 500
scaling_controls["CR"] = 0.005
scaling_dist = dist()
scaling_dist["Faer"] = float(p_["m"] * p_["g"] / NR)
scaling_dist["Caer"] = 0.0026
# ----------- Frames ------------------
T_10 = mul(tr(*pos), Tquat(*q))
T_01 = kin_inv(T_10)
R_10 = T2R(T_10)
R_01 = R_10.T
# -------------------------------------
dstates = struct_symSX(states)
dp, dv, dq, dw, dr = dstates[...]
res = struct_SX(states) # DAE residual hand side
# ----------- Dynamics of the body ----
res["p"] = v - dp
# Newton, but transform the force F from {1} to {0}
res["v"] = mul(R_10, F) - m * dv
# Kinematics of the quaterion.
res["q"] = mul(quatDynamics(*q), w) - dq
# This is a trick by Sebastien Gros to stabilize the quaternion evolution equation
res["q"] += -q * (sumAll(q**2) - 1)
# Agular impulse H_1011
H = mul(p["I"], w) # Due to platform
for i in range(NR):
H += mul(
p["rotors_I", i], w +
vertcat([0, 0, p["rotors_spin", i] * r[i]])) # Due to rotor i
dH = mul(jacobian(H, w), dw) + mul(jacobian(H, q), dq) + mul(
jacobian(H, r), dr) + casadi.cross(w, H)
res["w"] = C - dH
for i in range(NR):
res["r", i] = CR[i] + p["rotors_spin", i] * rotors_Caer[i][2] - p[
"rotors_I", i][2] * (dr[i] + dw[2]) # Dynamics of rotor i
# ---------------------------------
# Make a vector of f ?
#if quatnorm:
# f = vertcat(f+[sumAll(q**2)-1])
#else:
# f = vertcat(f)
# ------------ Force model ------------
Fg = mul(R_01, vertcat([0, 0, -g * m]))
F_total = Fg + sum(rotors_Faer) # Total force acting on the platform
C_total = SX([0, 0, 0]) # Total torque acting on the platform
for i in range(NR):
C_total[:2] += rotors_Caer[
i][:2] # The x and y components propagate
C_total[2] -= p["rotors_spin", i] * CR[
i] # the z compent moves through a serparate system
C_total += casadi.cross(p["rotors_p", i],
rotors_Faer[i]) # Torques due to thrust
res = substitute(res, F, F_total)
res = substitute(res, C, C_total)
subs_before = []
subs_after = []
v_global = mul(R_01, v)
u_z = SX([0, 0, 1])
# Now fill in the aerodynamic forces
for i in range(NR):
c, R, CL_alpha, alpha_0, CD_alpha, CD_i = p["rotors_model", i, ...]
#Bristeau P-jean, Martin P, Salaun E, Petit N. The role of propeller aerodynamics in the model of a quadrotor UAV. In: Proceedings of the European Control Conference 2009.; 2009:683-688.
v_local = v_global + (casadi.cross(w, p["rotors_p", i])
) # Velocity at rotor i
rotors_Faer_physics = (rho * c * R**3 * r[i]**2 * CL_alpha *
(alpha_0 / 3.0 - v_local[2] /
(2.0 * R * r[i]))) * u_z
subs_before.append(rotors_Faer[i])
subs_after.append(rotors_Faer_physics + dist["Faer", i])
rotors_Caer_physics = -p["rotors_spin", i] * rho * c * R**4 * r[
i]**2 * (CD_alpha / 4.0 + CD_i * alpha_0**2 *
(alpha_0 / 4.0 - 2.0 * v_local[2] /
(3.0 * r[i] * R)) - CL_alpha * v_local[2] /
(r[i] * R) * (alpha_0 / 3.0 - v_local[2] /
(2.0 * r[i] * R))) * u_z
subs_before.append(rotors_Caer[i])
subs_after.append(rotors_Caer_physics + dist["Caer", i])
res = substitute(res, veccat(subs_before), veccat(subs_after))
# Make an explicit ode
rhs = -casadi.solve(jacobian(res, dstates), substitute(
res, dstates, 0))
# --------------------------------------
self.res_w = res
self.res = substitute(res, dist, dist_)
self.res_ = substitute(self.res, p, p_)
resf = SXFunction([dstates, states, controls], [self.res_])
resf.init()
self.resf = resf
self.rhs_w = rhs
self.rhs = substitute(rhs, dist, dist_)
self.rhs_ = substitute(self.rhs, p, p_)
t = SX("t")
# We end up with a DAE that captures the system dynamics
dae = SXFunction(daeIn(t=t, x=states, p=controls),
daeOut(ode=self.rhs_))
dae.init()
self.dae = dae
cdae = SXFunction(controldaeIn(t=t, x=states, u=controls, p=p),
daeOut(ode=self.rhs))
cdae.init()
self.cdae = cdae
self.states = states
self.dstates = dstates
self.p = p
self.p_ = p_
self.controls = controls
self.NR = NR
self.w = dist
self.w_ = dist_
self.t = t
self.states_ = states()
self.dstates_ = states()
self.controls_ = controls()
self.scaling_states = scaling_states
self.scaling_controls = scaling_controls
self.scaling_dist = scaling_dist
def build_gp(invK, X, hyper, alpha, chol, meanFunc='zero'):
""" Build Gaussian Process function
Copyright (c) 2018, Helge-André Langåker
# Arguments
invK: Array with the inverse covariance matrices of size (Ny x N x N),
with Ny number of outputs from the GP and N number of training points.
X: Training data matrix with inputs of size (N x Nx), with Nx number of
inputs to the GP.
alpha: Training data matrix with invK time outputs of size (Ny x N).
hyper: Array with hyperparame|ters [ell_1 .. ell_Nx sf sn].
# Returns
mean: GP mean casadi function [mean(z)]
var: GP variance casadi function [var(z)]
covar: GP covariance casadi function [covar(z) = diag(var(z))]
mean_jac: Casadi jacobian of the GP mean function [jac(z)]
"""
Ny = len(invK)
X = ca.SX(X)
N, Nx = ca.SX.size(X)
mean = ca.SX.zeros(Ny, 1)
var = ca.SX.zeros(Ny, 1)
# Casadi symbols
x_s = ca.SX.sym('x', Nx)
z_s = ca.SX.sym('z', Nx)
m_s = ca.SX.sym('m')
ell_s = ca.SX.sym('ell', Nx)
sf2_s = ca.SX.sym('sf2')
X_s = ca.SX.sym('X', N, Nx)
ks_s = ca.SX.sym('ks', N)
v_s = ca.SX.sym('v', N)
kss_s = ca.SX.sym('kss')
alpha_s = ca.SX.sym('alpha', N)
covSE = ca.Function('covSE', [x_s, z_s, ell_s, sf2_s],
[covSEard(x_s, z_s, ell_s, sf2_s)])
ks = ca.SX.zeros(N, 1)
for i in range(N):
ks[i] = covSE(X_s[i, :], z_s, ell_s, sf2_s)
ks_func = ca.Function('ks', [X_s, z_s, ell_s, sf2_s], [ks])
mean_i_func = ca.Function('mean', [ks_s, alpha_s, m_s],
[ca.mtimes(ks_s.T, alpha_s) + m_s])
L_s = ca.SX.sym('L', ca.Sparsity.lower(N))
v_func = ca.Function('v', [L_s, ks_s], [ca.solve(L_s, ks_s)])
var_i_func = ca.Function('var', [v_s, kss_s,],
[kss_s - v_s.T @ v_s])
for output in range(Ny):
ell = ca.SX(hyper[output, 0:Nx])
sf2 = ca.SX(hyper[output, Nx]**2)
alpha_a = ca.SX(alpha[output])
ks = ks_func(X_s, z_s, ell, sf2)
v = v_func(chol[output], ks)
m = get_mean_function(ca.MX(hyper[output, :]), z_s, func=meanFunc)
mean[output] = mean_i_func(ks, alpha_a, m(z_s))
var[output] = var_i_func(v, sf2)
mean_temp = ca.Function('mean_temp', [z_s, X_s], [mean])
var_temp = ca.Function('var_temp', [z_s, X_s], [var])
mean_func = ca.Function('mean', [z_s], [mean_temp(z_s, X)])
covar_func = ca.Function('var', [z_s], [ca.diag(var_temp(z_s, X))])
var_func = ca.Function('var', [z_s], [var_temp(z_s, X)])
mean_jac_z = ca.Function('mean_jac_z', [z_s],
[ca.jacobian(mean_func(z_s), z_s)])
return mean_func, var_func, covar_func, mean_jac_z