Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def test_0(self):
self.message("Typemap array -> DM")
arrays = [array([[1,2],[3,4],[5,6]],dtype=double),array([[3.2,4.6,9.9]])]
for i in range(len(arrays)):
m = arrays[i]
zt=c.transpose(c.transpose(m))
self.assertTrue(isinstance(zt,DM),"DM expected")
self.checkarray(m,zt,"DM(numpy.ndarray)")
self.checkarray(m,zt.full(),"DM(numpy.ndarray).full()")
if scipy_available:
self.checkarray(m,zt.sparse(),"DM(numpy.ndarray).sparse()")
def test_0a(self):
self.message("Typemap array -> IM")
arrays = [array([[1,2,3],[4,5,6]],dtype=int32),array([[1,2,3],[4,5,6]]),array([[1,2,3],[4,5,6]],dtype=int)]
for i in range(len(arrays)):
m = arrays[i]
zt=c.transpose(c.transpose(m))
self.assertTrue(isinstance(zt,IM),"IM expected")
self.checkarray(m,zt,"IM(numpy.ndarray)")
self.checkarray(m,zt.full(),"IM(numpy.ndarray).full()")
#if scipy_available:
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(
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]
kss = sf2
ks = ca.MX.zeros(n, 1)
for i in range(n):
ks[i] = covSEard2(X[i, :], inputmean, ell, sf2)
invKks = ca.mtimes(iK, ks)
mean[a] = ca.mtimes(ks.T, alpha)
var[a] = kss - ca.mtimes(ks.T, invKks)
d_mean[a] = ca.mtimes(ca.transpose(w[a] * v[:, a] * ks), alpha)
for d in range(E):
for e in range(E):
dd_var1a = ca.mtimes(ca.transpose(v[:, d] * ks), iK)
dd_var1b = ca.mtimes(dd_var1a, v[e] * ks)
dd_var2 = ca.mtimes(ca.transpose(v[d] * v[e] * ks), invKks)
dd_var[d, e] = -2 * w[d] * w[e] * (dd_var1b + dd_var2)
if d == e:
dd_var[d, e] = dd_var[d, e] + 2 * w[d] * (kss - var[d])
# covariance with noisy input
for a in range(E):
covar1 = ca.mtimes(d_mean, d_mean.T)
covar[0, 0] = inputcov[a]
covariance[a, a] = var[a] + ca.trace(ca.mtimes(covar, .5 * dd_var + covar1))
#covariance[a] = var[a] + ca.trace(ca.mtimes(inputcov, .5 * dd_var + covar1))
return [mean, covariance]
def maha(a1, b1, Q1, n):
aQ = ca.mtimes(a1, Q1)
bQ = ca.mtimes(b1, Q1)
K1 = ca.repmat(ca.sum2(aQ * a1), 1, n) + ca.repmat(ca.transpose(ca.sum2(bQ * b1)), n, 1) - 2 * ca.mtimes(aQ, ca.transpose(b1))
return K1
ks = ca.MX.zeros(N, 1)
for i in range(N):
ks[i] = covSE(X[i, :], inputmean, ell, sf2)
invKks = ca.mtimes(iK, ks)
mean[a] = ca.mtimes(ks.T, alpha)
var[a] = kss - ca.mtimes(ks.T, invKks)
d_mean[a] = ca.mtimes(ca.transpose(w[a] * v[:, a] * ks), alpha)
#BUG: This don't take into account the covariance between states
for d in range(Ny):
for e in range(Ny):
dd_var1a = ca.mtimes(ca.transpose(v[:, d] * ks), iK)
dd_var1b = ca.mtimes(dd_var1a, v[e] * ks)
dd_var2 = ca.mtimes(ca.transpose(v[d] * v[e] * ks), invKks)
dd_var[d, e] = -2 * w[d] * w[e] * (dd_var1b + dd_var2)
if d == e:
dd_var[d, e] = dd_var[d, e] + 2 * w[d] * (kss - var[d])
mean_mat = ca.mtimes(d_mean, d_mean.T)
covar_temp[0, 0] = inputcovar[a, a]
covariance[a, a] = var[a] + ca.trace(ca.mtimes(covar_temp, .5
* dd_var + mean_mat))
return [mean, covariance]
if for_loop is not None:
if isinstance(indices[0], ca.MX):
if len(indices) > 1:
s = s[:, indices[1]]
indexed_symbol = _new_mx('{}[{},{}]'.format(tree.name, for_loop.name, indices[1]), s.size2())
index_function = lambda i : (i, indices[1])
else:
indexed_symbol = _new_mx('{}[{}]'.format(tree.name, for_loop.name))
index_function = lambda i : i
# If the indexed symbol is empty, we know we do not have to
# map the for loop over it
if np.prod(s.shape) != 0:
for_loop.register_indexed_symbol(indexed_symbol, index_function, True, tree, indices[0])
else:
s = ca.transpose(s[indices[0], :])
indexed_symbol = _new_mx('{}[{},{}]'.format(tree.name, indices[0], for_loop.name), s.size2())
index_function = lambda i: (indices[0], i)
if np.prod(s.shape) != 0:
for_loop.register_indexed_symbol(indexed_symbol, index_function, False, tree, indices[1])
return indexed_symbol
else:
if len(indices) == 1:
return s[indices[0]]
else:
return s[indices[0], indices[1]]