How to use the casadi.diag function in casadi
To help you get started, we’ve selected a few casadi examples, based on popular ways it is used in public projects.
Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
adbuerger / casiopeia / concept_tests / sd_check_pendulum_linear.py View on Github 
report.write("\n\n repetitions = " + str(repetitions))
report.write("\n sigma = " + str(sigma))
report.write("\n\n p_true = " + str(ca.DM(ptrue)))
report.write("\n\n p_mean = " + str(ca.DM(p_mean)))
report.write("\n phat_last_exp = " + \
str(ca.DM(pe_test.estimated_parameters)))
report.write("\n\n p_sd = " + str(ca.DM(p_std)))
report.write("\n sd_from_covmat = " \
+ str(ca.diag(ca.sqrt(pe_test.covariance_matrix))))
report.write("\n beta = " + str(pe_test.beta))
report.write("\n\n delta_abs_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix)))))
report.write("\n delta_rel_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DM(p_std)) \
+ "\n")
report.close()
try:
os.system("rst2pdf " + fname)
except:
print("Generating PDF report failed, is rst2pdf installed correctly?")(in1,v1,x[[1,0],0],sparsify(DM([[0,1],[1,0]]))),
(in1,v1,w,sparsify(DM([[1,0],[0,2]]))),
(in1,v1,w2,blockcat([[1,MX(1,1)],[x[1],x[0]]])),
(in1,v1,ww,2*c.diag(x)),
(in1,v1,wwf,vertcat(*[x[[1,0]].T,x[[1,0]].T])),
(in1,v1,yy[:,0],DM.eye(2)),
(in1,v1,yy2[:,0],2*c.diag(x)),
(in1,v1,yyy[:,0],sparsify(DM([[0,1],[1,0]]))),
(in1,v1,mtimes(y,x),y),
(in1,v1,mtimes(x.T,y.T),y),
(in1,v1,mac(y,x,DM.zeros(Sparsity.triplet(2,1,[1],[0]))),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
(in1,v1,mac(x.T,y.T,DM.zeros(Sparsity.triplet(2,1,[1],[0]).T)),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
(in1,v1,mtimes(y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])],x),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
(in1,v1,mtimes(x.T,y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])].T),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
(in1,v1,mtimes(y,x**2),y*2*vertcat(*[x.T,x.T])),
(in1,v1,sin(x),c.diag(cos(x))),
(in1,v1,sin(x**2),c.diag(cos(x**2)*2*x)),
(in1,v1,x*y[:,0],c.diag(y[:,0])),
(in1,v1,x*y.nz[[0,1]],c.diag(y.nz[[0,1]])),
(in1,v1,x*y.nz[[1,0]],c.diag(y.nz[[1,0]])),
(in1,v1,x*y[[0,1],0],c.diag(y[[0,1],0])),
(in1,v1,x*y[[1,0],0],c.diag(y[[1,0],0])),
(in1,v1,c.dot(x,x),(2*x).T),
(in1,v1,c.dot(x**2,x),(3*x**2).T),
#(in1,v1,c.det(horzcat(*[x,DM([1,2])])),DM([-1,2])), not implemented
(in1,v1,f1.call(in1)[1],y),
(in1,v1,f1.call([x**2,y])[1],y*2*vertcat(*[x.T,x.T])),
(in1,v1,f2.call(in1)[0],DM.zeros(0,2)),
(in1,v1,f2(x**2,y),DM.zeros(0,2)),
(in1,v1,f3.call(in1)[0],DM.zeros(0,2)),
(in1,v1,f3.call([x**2,y])[0],DM.zeros(0,2)),
(in1,v1,f4.call(in1)[0],DM.zeros(0,2)),pe_test.print_estimation_results()
# Generate report
print("\np_mean = " + str(ca.DM(p_mean)))
print("phat_last_exp = " + str(ca.DM(pe_test.estimated_parameters)))
print("\np_sd = " + str(ca.DM(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(pe_test.covariance_matrix))))
print("beta = " + str(pe_test.beta))
print("\ndelta_abs_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix)))))
print("delta_rel_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DM(p_std)))
fname = os.path.basename(__file__)[:-3] + ".rst"
report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================
Simulate system. Then: add gaussian noise N~(0, sigma^2), estimate,
store estimated parameter, repeat.
.. code-block:: python
y_randn = sim_true.simulation_results + sigma * \
(np.random.randn(*sim_true.estimated_parameters.shape))def test_general_convex_sparse(self):
self.message("Convex sparse QP with solvers: " + str([conic for conic,options,aux_options in conics]))
H = c.diag([2,1,0.2,0.7,1.3])
H[1,2]=0.1
H[2,1]=0.1
G = DM([-2,-6,1,0,0])
A = DM([[1, 0,0.1,0.7,-1],[0.1, 2,-0.3,4,0.1]])
A = sparsify(A)
LBA = DM([-inf])
UBA = DM([2, 2])
LBX = DM([0]*5)
UBX = DM([inf]*5)
for conic, qp_options, aux_options in conics:def test_diag_sparse(self):
self.message("diag sparse")
for n in [[0,1,0,0,2,3,4,5,6,0],[1,2,3,0],[0,1,2,3]]:
d = DM(n)
D = DM(n)
d = sparsify(d)
m = c.diag(d)
M = sparsify(c.diag(D))
self.checkarray(m.sparsity().colind(),M.sparsity().colind())
self.checkarray(m.sparsity().row(),M.sparsity().row())n = ca.MX.size(F[:, 1])[0]
mean = ca.MX.zeros(E, 1)
beta = ca.MX.zeros(n, E)
log_k = ca.MX.zeros(n, E)
v = X - ca.repmat(inputmean, n, 1)
#invK = MX(invK)
covariance = ca.MX.zeros(E, E)
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(E):
beta[:, a] = ca.mtimes(invK[a], F[:, a])
iLambda = ca.diag(ca.exp(-2 * hyper[a, :D]))
R = inputcov + ca.diag(ca.exp(2 * hyper[a, :D]))
iR = ca.mtimes(iLambda, (ca.MX.eye(D) - ca.solve((ca.MX.eye(D) + ca.mtimes(inputcov, iLambda)), (ca.mtimes(inputcov, iLambda)))))
T = ca.mtimes(v, iR)
c = ca.exp(2 * hyper[a, D]) / ca.sqrt(determinant(R)) * ca.exp(ca.sum2(hyper[a, :D]))
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, :D]), n, 1)
v1 = v / t
log_k[:, a] = 2 * hyper[a, D] - ca.sum2(v1 * v1) * 0.5
# covariance with noisy input
for a in range(E):
ii = v / ca.repmat(ca.exp(2 * hyper[a, :D]), n, 1)
for b in range(a + 1):
R = ca.mtimes(inputcov, ca.diag(ca.exp(-2 * hyper[a, :D]) + ca.exp(-2 * hyper[b, :D]))) + ca.MX.eye(D)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):# 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)))EPS_U = ca.MX.sym("EPS_U", N)
X0 = ca.MX.sym("X0", 4)
V = ca.vertcat([P, EPS_U, X0])
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,:].Tfor t in range(Nt):
# Input to GP
K_t = var['K', t].reshape((Nu, Ny))
u_t = u_func(var['mean', t], var['v', t], K_t)
z = ca.vertcat(var['mean', t], u_t)
covar_x_t = var['covariance', t].reshape((Ny, Ny))
# Calculate next step
mean_next, covar_x_next = gp_func(z, covar_x_t)
# Continuity constraints
con_eq.append(var['mean', t + 1] - mean_next)
con_eq.append(var['covariance', t + 1] - covar_x_next.reshape((Ny * Ny,1)))
# Chance state constraints
con_ineq.append(mean_next + quantile_x * ca.sqrt(ca.diag(covar_x_next) ))
con_ineq_ub.append(xub)
con_ineq_lb.append(np.full((Ny,), -ca.inf))
con_ineq.append(mean_next - quantile_x * ca.sqrt(ca.diag(covar_x_next)))
con_ineq_ub.append(np.full((Ny,), ca.inf))
con_ineq_lb.append(xlb)
# Input constraints
con_ineq.append(u_t)
con_ineq_ub.extend(uub)
con_ineq_lb.append(ulb)
u_delta = u_t - u_past
obj += l_func(var['mean', t], covar_x_t, u_t, u_delta, K_t)
u_t = u_past
covar_x_t = covar_x_next
obj += lf_func(var['mean', Nt], var['covariance', Nt].reshape((Ny, Ny)))casadi
CasADi -- framework for algorithmic differentiation and numeric optimization
