def test_consistency(self):
a = array([[0.,1],[-1,0]])
assert_array_almost_equal(expm(a), expm2(a))
assert_array_almost_equal(expm(a), expm3(a))
a = array([[1j,1],[-1,-2j]])
assert_array_almost_equal(expm(a), expm2(a))
assert_array_almost_equal(expm(a), expm3(a))
def test_consistency(self):
a = array([[0., 1], [-1, 0]])
assert_array_almost_equal(expm(a), expm2(a))
assert_array_almost_equal(expm(a), expm3(a))
a = array([[1j, 1], [-1, -2j]])
assert_array_almost_equal(expm(a), expm2(a))
assert_array_almost_equal(expm(a), expm3(a))
def updateParams(self):
self.pop.sort(key=op.attrgetter('f'))
self.pSigma = np.dot(1.0 - self.cSigma, self.pSigma) + np.dot(
np.sqrt(self.cSigma * (2.0 - self.cSigma) * self.muEff),
sum(np.dot(self.rankWeight[i], self.pop[i].z) for i in range(self.popsize)))
rate = np.linalg.norm(self.pSigma) / self.expectationChiDistribution
if rate >= 1.0 :
wsum = 0
for i in range(self.popsize):
self.weight[i] = self.hatWeight[i] * np.expm1(self.alpha * np.linalg.norm(self.pop[i].z) + 1.0)
wsum += self.weight[i]
for i in range(self.popsize):
self.weight[i] = self.weight[i] / wsum - 1.0 / self.popsize
else:
self.weight = self.rankWeight
if rate >= 1.0:
self.etaB = self.etaBMove
self.etaSigma = self.etaSigmaMove
elif rate >= 0.1:
self.etaB = self.etaBStag
self.etaSigma = self.etaSigmaStag
else:
self.etaB = self.etaBConv
self.etaSigma = self.etaSigmaConv
GDelta = sum(np.dot(self.weight[i], self.pop[i].z) for i in range(self.popsize))
GMu = sum(self.weight[i] * (np.outer(self.pop[i].z, self.pop[i].z) - np.eye(self.dim)) for i in range(self.popsize))
GSigma = np.trace(GMu) / self.dim
GB = GMu - GSigma * np.eye(self.dim)
self.mu += self.etaMu * self.sigma * np.dot(self.B, GDelta)
self.sigma *= (np.expm1(0.5 * self.etaSigma * GSigma) + 1.0)
self.B = np.dot(self.B, linalg.expm3(0.5 * self.etaB * GB))
def set_angle(list_of_atoms, new_ang, atoms_ring, xyz, conn_mat):
"""Set a new angle between three atoms
Args:
list_of_atoms: list of three atoms
new_ang: value of dihedral angle (in degrees) to be set
atoms_ring: dictionary of atoms in the ring. It recognizes
if the last atom is 'C0O' (obsolete)
xyz: numpy array with atoms xyz positions
conn_mat: connectivity matrix
Returns:
xyz: modified numpy array with new atoms positions
"""
#Determine the axis of rotation:
old_ang, axor = measure_angle(list_of_atoms, xyz)
norm_axor = np.sqrt(np.sum(axor**2))
normalized_axor = axor / norm_axor
#Determine which atoms should be dragged along with the bond:
carried_atoms = determine_carried_atoms(list_of_atoms[1],
list_of_atoms[2], conn_mat)
#Each carried_atom is rotated by euler-rodrigues formula:
#Also, I move the midpoint of the bond to the mid atom
#the rotation step and then move the atom back.
rot_angle = np.pi * (new_ang - old_ang) / 180.
#Shake it, baby! Rotation matrix:
#print old_ang, new_ang, rot_angle*180./np.pi
rot1 = expm3(np.cross(np.eye(3), normalized_axor * rot_angle))
translation = xyz[list_of_atoms[1], :]
for at in carried_atoms:
xyz[at, :] = np.dot(rot1, xyz[at, :] - translation)
xyz[at, :] = xyz[at, :] + translation
return xyz
def set_angle(list_of_atoms, new_ang, atoms_ring, xyz, conn_mat):
"""Set a new angle between three atoms
Args:
list_of_atoms: list of three atoms
new_ang: value of dihedral angle (in degrees) to be set
atoms_ring: dictionary of atoms in the ring. It recognizes
if the last atom is 'C0O' (obsolete)
xyz: numpy array with atoms xyz positions
conn_mat: connectivity matrix
Returns:
xyz: modified numpy array with new atoms positions
"""
#Determine the axis of rotation:
old_ang, axor = measure_angle(list_of_atoms, xyz)
norm_axor = np.sqrt(np.sum(axor**2))
normalized_axor = axor/norm_axor
#Determine which atoms should be dragged along with the bond:
carried_atoms = determine_carried_atoms(list_of_atoms[1],
list_of_atoms[2], conn_mat)
#Each carried_atom is rotated by euler-rodrigues formula:
#Also, I move the midpoint of the bond to the mid atom
#the rotation step and then move the atom back.
rot_angle = np.pi*(new_ang - old_ang)/180.
#Shake it, baby! Rotation matrix:
#print old_ang, new_ang, rot_angle*180./np.pi
rot1 = expm3(np.cross(np.eye(3), normalized_axor*rot_angle))
translation = xyz[list_of_atoms[1], :]
for at in carried_atoms:
xyz[at, :] = np.dot(rot1, xyz[at, :]-translation)
xyz[at, :] = xyz[at, :]+translation
return xyz
def M(axis,theta): return expm3(np.cross(np.eye(3),axis/norm(axis)*theta))
#Explanation of the M matrix.
#if a is an unit vector of axis such that a = axis/nomr(axis)
#and A = Ia is teh skew-symmetric matrix associated to a
#then M = exp(theta, A) is the rotation matrix
#expm3 computes the taylor series of the exponential: \sum_{k=0}^{2theta} \frac{1}{k!} (theta A)^k
ex_dof = np.array(self.dofList[self.nshared:])
def create_pose(vertex, scale=0, angle_deg=0):
"""Compute rotation matrix from viewpoint vertex and inplane rotation """
rot = compute_rotation_from_vertex(vertex)
transform = np.eye(4)
rodriguez = np.asarray([0, 0, 1]) * (angle_deg * math.pi / 180.0)
angle_axis = expm3(np.cross(np.eye(3), rodriguez))
transform[0:3, 0:3] = np.matmul(angle_axis, rot)
transform[0:3, 3] = [0, 0, scale]
return transform
def update_parameters(self):
self.pop.sort(key=op.attrgetter('f'))
grad_delta = sum(np.dot(self.weight[i], self.pop[i].z) for i in range(self.popsize))
grad_mu = sum(self.weight[i] * (np.outer(self.pop[i].z, self.pop[i].z) - np.eye(self.dim)) for i in range(self.popsize))
grad_sigma = np.trace(grad_mu) / self.dim
grad_B = grad_mu - grad_sigma * np.eye(self.dim)
self.mu += self.eta_mu * self.sigma * np.dot(self.B, grad_delta)
self.sigma *= (np.expm1(0.5 * self.eta_sigma * grad_sigma) + 1.0)
self.B = np.dot(self.B, linalg.expm3(0.5 * self.eta_B * grad_B))
def rotation_matrix_around_vec( axis: mp.XYZ, theta: float ):
"""
calcualted the rotation matrix to roatate an boject around axis theta radian degrees
based on:
http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/
and
http://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector
"""
return linalg.expm3(np.cross(np.eye(3), axis.as_list()/np.float64(linalg.norm(axis.as_list()))*theta))
def create_pose(vertex, scale=0, angle_deg=0):
"""Compute rotation matrix from viewpoint vertex and inplane rotation """
rot = compute_rotation_from_vertex(vertex)
transform = np.eye(4)
rodriguez = np.asarray([0, 0, 1]) * (angle_deg * math.pi / 180.0)
angle_axis = expm3(np.cross(np.eye(3), rodriguez))
transform[0:3, 0:3] = np.matmul(angle_axis, rot)
transform[0:3, 3] = [0, 0, scale]
return transform
def prob_network(scores):
# get known score fractions.
normed = scores/((scores + scores.T) + (scores == 0))
exped = expm3(normed, 20)
# get rid of diagonal part.
exped -= diag(diag(exped))
# divide by symmetric part.
return exped/((exped + exped.T) + (exped == 0) + (exped.T == 0))
def rotation_matrix_around_vec(axis: mp.XYZ, theta: float):
"""
calcualted the rotation matrix to roatate an boject around axis theta radian degrees
based on:
http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/
and
http://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector
"""
return linalg.expm3(
np.cross(
np.eye(3),
axis.as_list() / np.float64(linalg.norm(axis.as_list())) * theta))
def set_dihedral(list_of_atoms, new_dih, atoms_ring, xyz, conn_mat):
"""Set a new dihedral angle between two planes defined by
atoms first and last three atoms of the supplied list.
Args:
list_of_atoms: list of four atoms
new_dih: value of dihedral angle (in degrees) to be set
atoms_ring: dictionary of atoms in the ring. It recognizes
if the last atom is 'C0O'
xyz: numpy array with atoms xyz positions
conn_mat: connectivity matrix
Returns:
xyz: modified numpy array with new atoms positions
"""
#Determine the axis of rotation:
old_dih, axor = measure_dihedral(list_of_atoms, xyz)
norm_axor = np.sqrt(np.sum(axor**2))
normalized_axor = axor / norm_axor
#Check if the bond is the last bond, next to broken one.
#If yes, refer to the oxygen:
if 'O0a' in atoms_ring.keys():
if list_of_atoms[-1] == atoms_ring['O0a']:
new_dih += 120.0
else:
if list_of_atoms[-1] == atoms_ring['O0b']:
new_dih -= 120.0
#Determine which atoms should be dragged along with the bond:
carried_atoms = determine_carried_atoms(list_of_atoms[1],
list_of_atoms[2], conn_mat)
#Each carried_atom is rotated by Euler-Rodrigues formula:
#Reverse if the angle is less than zero, so it rotates in
#right direction.
#Also, I move the midpoint of the bond to the center for
#the rotation step and then move the atom back.
if old_dih >= 0.0:
rot_angle = np.pi * (new_dih - old_dih) / 180.
else:
rot_angle = -np.pi * (new_dih - old_dih) / 180.
#Shake it, baby! Rotation matrix:
rot1 = expm3(np.cross(np.eye(3), normalized_axor * rot_angle))
translation = (xyz[list_of_atoms[1], :] + xyz[list_of_atoms[2], :]) / 2
for at in carried_atoms:
xyz[at, :] = np.dot(rot1, xyz[at, :] - translation)
xyz[at, :] = xyz[at, :] + translation
return xyz
def set_dihedral(list_of_atoms, new_dih, atoms_ring, xyz, conn_mat):
"""Set a new dihedral angle between two planes defined by
atoms first and last three atoms of the supplied list.
Args:
list_of_atoms: list of four atoms
new_dih: value of dihedral angle (in degrees) to be set
atoms_ring: dictionary of atoms in the ring. It recognizes
if the last atom is 'C0O'
xyz: numpy array with atoms xyz positions
conn_mat: connectivity matrix
Returns:
xyz: modified numpy array with new atoms positions
"""
#Determine the axis of rotation:
old_dih, axor = measure_dihedral(list_of_atoms, xyz)
norm_axor = np.sqrt(np.sum(axor**2))
normalized_axor = axor/norm_axor
#Check if the bond is the last bond, next to broken one.
#If yes, refer to the oxygen:
if 'O0a' in atoms_ring.keys():
if list_of_atoms[-1] == atoms_ring['O0a']:
new_dih += 120.0
else:
if list_of_atoms[-1] == atoms_ring['O0b']:
new_dih -= 120.0
#Determine which atoms should be dragged along with the bond:
carried_atoms = determine_carried_atoms(list_of_atoms[1],
list_of_atoms[2], conn_mat)
#Each carried_atom is rotated by Euler-Rodrigues formula:
#Reverse if the angle is less than zero, so it rotates in
#right direction.
#Also, I move the midpoint of the bond to the center for
#the rotation step and then move the atom back.
if old_dih >= 0.0:
rot_angle = np.pi*(new_dih - old_dih)/180.
else:
rot_angle = -np.pi*(new_dih - old_dih)/180.
#Shake it, baby! Rotation matrix:
rot1 = expm3(np.cross(np.eye(3), normalized_axor*rot_angle))
translation = (xyz[list_of_atoms[1], :]+xyz[list_of_atoms[2], :])/2
for at in carried_atoms:
xyz[at, :] = np.dot(rot1, xyz[at, :]-translation)
xyz[at, :] = xyz[at, :]+translation
return xyz
def _get_pascal_weights(depth):
assert 0 < depth < 10, "Depth must be between [0,10], got {}".format(depth)
depth = depth + 1
# TODO: expm3 is deprecated, but expm can;t work with depth 7 and 8
# Issue 8029: https://github.com/scipy/scipy/issues/8029
diag = linalg.expm3(np.diag(np.arange(1, depth), -1))
signed = diag[1:, 1:].dot(
np.diag([
x if i % 2 == 0 else -x for i, x in enumerate([1] * (depth - 1))
])).astype(int)
if depth > 2:
signed[1, :] = np.array([-1, 2] + [0] * (depth - 3)) # fix first row
if depth > 1:
signed[0, :] = np.array([1] + [0] * (depth - 2)) # fix second row
return signed if depth > 1 else signed[0]
def skew3(w):
return np.cross(np.identity(3), w)
if __name__ == '__main__':
f = "/home/jacob/repos/experiments/bin/bloop"
data = np.loadtxt(f)
norms = np.linalg.norm(data, axis=1)
a = data[(norms != 0) & (norms < 1e2), :]
ray = np.array([1.0, 0.0, 0.0])
rays = []
for row in a:
R = scla.expm3(skew3(row[3:]))
direction = R.dot(ray)
rays.append(direction)
rays = np.vstack(rays) * -200.0
ax = ax3d()
ax.scatter(a[:, 0], a[:, 1], a[:, 2])
print rays.shape
print a.shape
ax.quiver(a[:, 0], a[:, 1], a[:, 2], rays[:, 0], rays[:, 1], rays[:, 2])
plt.show()
m = zeros((i,i), float64)
half = int(round(i*i/2))+1
r = random.randint(0,i, (half*2, ))
n = random.random((half, ))
for z in range(0, half*2, 2):
m[r[z], r[z+1]] = n[z/2]
start = time()
expm(m)
t[tt] = time() - start
start = time()
expm2(m)
t2[tt] = time() - start
start = time()
expm3(m)
t3[tt] = time() - start
start = time()
expf(i, 1, m, i)
t4[tt] = time() - start
tt += 1
print s
print t
plot(s, t, label="Pade")
plot(s, t2, label="Eigenvalue")
plot(s, t3, label="Taylor")
plot(s, t4, label="Fortran")
legend(loc = 'best')
show()
def test_zero(self):
a = array([[0.,0],[0,0]])
assert_array_almost_equal(expm(a),[[1,0],[0,1]])
assert_array_almost_equal(expm2(a),[[1,0],[0,1]])
assert_array_almost_equal(expm3(a),[[1,0],[0,1]])
def push_protons(grid):
"""
Initialize arrays of proton positions, velocities, and energies, then simulate proton propogation through the grid.
Parameters
----------
grid : Grid
Grid object produced by load_grid()
Returns
-------
film_x : array of size NP
'x' positions of protons on film plane relative to film axes (in cm)
film_y : array of size NP
'y' positions of protons on film plane relative to film axes (in cm)
Ep : array of size NP
Energies of protons at image plane (in MeV).
traces : ndarray of shape (ntraces, nz_prot+2, 3)
Record of particle traces for random subset of protons. First dimension is the proton being tracked,
second dimension is the index of the step, third dimension is the coordinates (x,y,z) at each location
"""
NP = params.NP
ntraces = params.ntraces
nsteps = params.nsteps
# Array to store trajectories of a subset of the protons.
# Three extra indices for when the protons are at the source, at the beginning of the grid,
# and at the film. The 3 is for x,y,z coords of each proton.
traces = np.zeros((ntraces, nsteps+3, 3))
try:
# The user can choose to supply their own initial x,y,z positions and velocities.
# (can supply either all of these or none of them, but not just some)
# If these are provided, don't need to define params.source_loc, params.prop_dir,
# params.l_s2start, params.E0, params.spread_angle, or params.r_source. Note that
# protons should all be moving in a roughly similar direction, otherwise
# large errors will be introduced because of how the equations of motion are
# implemented in the pusher.
x,y,z = params.x, params.y, params.z
vx,vy,vz = params.vx, params.vy, params.vz
prop_dir = np.array([np.average(vx),np.average(vy),np.average(vz)])
prop_dir /= linalg.norm(prop_dir)
print('Using user-defined initial positions and velocities.')
except AttributeError:
# If user didn't supply initial positions and velocities, assume isotropic source
print('Initializing positions and velocities assuming isotropic proton source.')
start_time = timer()
# Protons "start" at source (meaning velocities are set as if they did)
traces[:,0,0:3] = params.source_loc
v0 = np.sqrt(2.0*params.E0/911.0)*c
# 3D position of proton source
source_loc = np.array(params.source_loc)
# unit vector defining propogation direction
prop_dir = np.array(params.prop_dir)/linalg.norm(params.prop_dir)
# Distance from source to where protons actually start their propogation
l_s2start = params.l_s2start
# Angle between prop_dir and the outer edge of the cone defining the initial proton spread
phi_max = params.spread_angle*np.pi/180.0
# Together, theta and phi give an isotropic distribution of protons on the surface of a
# unit sphere sector of angle phi_max. phis are polar angles from the distribution
# center, and thetas are azimuthal angles.
phis = np.arccos((1-np.cos(phi_max))*np.random.random_sample(NP) + np.cos(phi_max))
thetas = 2*np.pi*np.random.random_sample(NP)
# Initialize at a starting plane as if source is at [0,0,0] pointing along z axis
positions = l_s2start*(np.array([0,0,1])[:,np.newaxis] + np.tan(phis)*np.array([np.cos(thetas),np.sin(thetas),np.zeros(NP)]))
# Then rotate to correct direction and add in actual source position
rot_axis = np.cross([0,0,1],prop_dir)
if linalg.norm(rot_axis) != 0:
rot_axis /= linalg.norm(rot_axis)
rot_angle = np.arccos(np.dot([0,0,1],prop_dir))
# New favorite way to define a rotation matrix:
rot_matrix = linalg.expm3(np.cross(np.eye(3), rot_axis*rot_angle))
positions = np.dot(rot_matrix,positions)
positions += source_loc[:,np.newaxis]
# Calculate velocity vector based on starting position
s2pos = positions-source_loc[:,np.newaxis]
velocities = v0*s2pos/np.linalg.norm(s2pos,axis=0)
x,y,z = positions
vx,vy,vz = velocities
try:
# Add random noise to account for finite source.
# NOTE: this means that protons might not necessarily align with
# the starting end ending planes on either side of the grid, so
# be careful if you have, for example, a very thin sheet of metal
# at one end of your grid, that your l_prop will still push them
# all through it.
r_source = params.r_source
x += (2*np.random.random_sample(NP)-1)*r_source
y += (2*np.random.random_sample(NP)-1)*r_source
z += (2*np.random.random_sample(NP)-1)*r_source
except AttributeError: pass
end_time = timer()
print("Time elapsed during proton initialization: " + str(end_time-start_time) + " s")
# Proton step size
ds_prot = params.l_prop/nsteps
print("Pushing " + str(NP) + " protons...")
# Push protons using compiled module written in C
start_time = timer()
_pmover.pmover(qm, ds_prot, grid.dx, grid.dy, grid.dz, grid.xoffset, grid.yoffset, grid.zoffset, NP, nsteps,\
grid.nx, grid.ny, grid.nz, ntraces, grid.cyl_coords, x, y, z, vx, vy, vz, prop_dir, grid.vals, traces)
traces[:,nsteps+1,0] = x[0:ntraces]
traces[:,nsteps+1,1] = y[0:ntraces]
traces[:,nsteps+1,2] = z[0:ntraces]
end_time = timer()
print("Time elapsed during proton push through grid: " + str(end_time-start_time) + " s")
# final propagation to film plane
start_time = timer()
film_axis1 = np.array(params.film_axis1)/linalg.norm(params.film_axis1)
film_axis2 = np.array(params.film_axis2)/linalg.norm(params.film_axis2)
film_perp = np.cross(film_axis1,film_axis2) # Film normal vector
film_loc = np.array(params.film_loc)
positions = np.array([x,y,z])
velocities = np.array([vx,vy,vz])
positions += velocities*(np.dot((film_loc[:,np.newaxis]-positions).T,film_perp)/np.dot(velocities.T,film_perp))
# Project final positions onto basis vectors that span the film
film_x = np.dot((positions-film_loc[:,np.newaxis]).T,film_axis1)
film_y = np.dot((positions-film_loc[:,np.newaxis]).T,film_axis2)
x,y,z = positions
end_time = timer()
print("Time elapsed during final propogation and projection onto film plane: " + str(end_time-start_time) + " s")
Ep = 0.5*911.0*(vx**2+vy**2+vz**2)/c**2
traces[:,nsteps+2,0] = x[0:ntraces]
traces[:,nsteps+2,1] = y[0:ntraces]
traces[:,nsteps+2,2] = z[0:ntraces]
film_x = np.nan_to_num(film_x)
film_y = np.nan_to_num(film_y)
Ep = np.nan_to_num(Ep)
traces = np.nan_to_num(traces)
return film_x,film_y,Ep,traces
def rot_from_axis_angle(axis, theta):
axis = axis / np.linalg.norm(axis)
return linalg.expm3(np.cross(np.eye(3), axis * theta))
def kf_likelihood_g(self, F, L, Qc, H, R, Pinf, dF, dQc, dPinf, dR, X, Y):
# Evaluate marginal likelihood gradient
# State dimension, number of data points and number of parameters
n = F.shape[0]
steps = Y.shape[1]
nparam = dF.shape[2]
# Time steps
t = X.squeeze()
# Allocate space
e = 0
eg = np.zeros(nparam)
# Set up
m = np.zeros([n, 1])
P = Pinf.copy()
dm = np.zeros([n, nparam])
dP = dPinf.copy()
mm = m.copy()
PP = P.copy()
# Initial dt
dt = -np.Inf
# Allocate space for expm results
AA = np.zeros([2 * n, 2 * n, nparam])
FF = np.zeros([2 * n, 2 * n])
# Loop over all observations
for k in range(0, steps):
# The previous time step
dt_old = dt
# The time discretization step length
if k > 0:
dt = t[k] - t[k - 1]
else:
dt = 0
# Loop through all parameters (Kalman filter prediction step)
for j in range(0, nparam):
# Should we recalculate the matrix exponential?
if abs(dt - dt_old) > 1e-9:
# The first matrix for the matrix factor decomposition
FF[:n, :n] = F
FF[n:, :n] = dF[:, :, j]
FF[n:, n:] = F
# Solve the matrix exponential
AA[:, :, j] = linalg.expm3(FF * dt)
# Solve the differential equation
foo = AA[:, :, j].dot(np.vstack([m, dm[:, j:j + 1]]))
mm = foo[:n, :]
dm[:, j:j + 1] = foo[n:, :]
# The discrete-time dynamical model
if j == 0:
A = AA[:n, :n, j]
Q = Pinf - A.dot(Pinf).dot(A.T)
PP = A.dot(P).dot(A.T) + Q
# The derivatives of A and Q
dA = AA[n:, :n, j]
dQ = dPinf[:,:,j] - dA.dot(Pinf).dot(A.T) \
- A.dot(dPinf[:,:,j]).dot(A.T) - A.dot(Pinf).dot(dA.T)
# The derivatives of P
dP[:,:,j] = dA.dot(P).dot(A.T) + A.dot(dP[:,:,j]).dot(A.T) \
+ A.dot(P).dot(dA.T) + dQ
# Set predicted m and P
m = mm
P = PP
# Start the Kalman filter update step and precalculate variables
S = H.dot(P).dot(H.T) + R
# We should calculate the Cholesky factor if S is a matrix
# [LS,notposdef] = chol(S,'lower');
# The Kalman filter update (S is scalar)
HtiS = H.T / S
iS = 1 / S
K = P.dot(HtiS)
v = Y[:, k] - H.dot(m)
vtiS = v.T / S
# Loop through all parameters (Kalman filter update step derivative)
for j in range(0, nparam):
# Innovation covariance derivative
dS = H.dot(dP[:, :, j]).dot(H.T) + dR[:, :, j]
# Evaluate the energy derivative for j
eg[j] = eg[j] \
- .5*np.sum(iS*dS) \
+ .5*H.dot(dm[:,j:j+1]).dot(vtiS.T) \
+ .5*vtiS.dot(dS).dot(vtiS.T) \
+ .5*vtiS.dot(H.dot(dm[:,j:j+1]))
# Kalman filter update step derivatives
dK = dP[:, :, j].dot(HtiS) - P.dot(HtiS).dot(dS) / S
dm[:, j:j + 1] = dm[:, j:j + 1] + dK.dot(v) - K.dot(H).dot(
dm[:, j:j + 1])
dKSKt = dK.dot(S).dot(K.T)
dP[:, :,
j] = dP[:, :, j] - dKSKt - K.dot(dS).dot(K.T) - dKSKt.T
# Evaluate the energy
# e = e - .5*S.shape[0]*np.log(2*np.pi) - np.sum(np.log(np.diag(LS))) - .5*vtiS.dot(v);
e = e - .5 * S.shape[0] * np.log(2 * np.pi) - np.sum(
np.log(np.sqrt(S))) - .5 * vtiS.dot(v)
# Finish Kalman filter update step
m = m + K.dot(v)
P = P - K.dot(S).dot(K.T)
# Make sure the covariances stay symmetric
P = (P + P.T) / 2
dP = (dP + dP.transpose([1, 0, 2])) / 2
# raise NameError('Debug me')
# Return the gradient
return eg
print "Krylov Time: %f" % (end_time-start_time)
start_time = time.time()
testExp = expm(t*M)
testV = testExp*v
end_time = time.time()
print "Dense Pade Exp Time: %f" % (end_time - start_time)
start_time = time.time()
testExp2 = expm2(t*M)
testV2 = testExp2*v
end_time = time.time()
print "Dense Eig Decomp Exp Time: %f" % (end_time - start_time)
start_time = time.time()
testExp3 = expm3(t*M)
testV3 = testExp3*v
end_time = time.time()
print "Dense Taylor Exp Time: %f" % (end_time - start_time)
relativeErr = norm(testV - w)/norm(testV)
print "Relative Difference Between exp(M)*v using Krylov and Dense Pade Approx: %f" % relativeErr
if abs(relativeErr) > 1e-6:
print "Tolerance Exceeded:"
print "Testing Arnoldi Exponential on Random Dense Complex %dx%d" % (nTest, nTest)
M = mat(complex(0.0,normConst) * random.randn(nTest,nTest))
normM = norm(M)
v = mat(random.randn(nTest,1), dtype=complex)
def rotationMatrix(axis, theta):
from numpy import cross, eye, dot
from scipy.linalg import expm3, norm
return expm3(cross(eye(3), axis / norm(axis) * theta))
def rotationMatrix(axis, theta):
from numpy import cross, eye
from scipy.linalg import expm3, norm
return expm3(cross(eye(3), axis / norm(axis) * theta))
def M(axis, theta):
return expm3(cross(eye(3), axis/norm(axis)*theta))
def M(axis, theta): # rotation matrix - supply unit axis and angle
return expm3(cross(eye(3), axis / norm(axis) * theta))
def kf_likelihood_g_notstable(self, F, L, Qc, H, R, Pinf, dF, dQc, dPinf,
dR, X, Y):
# Evaluate marginal likelihood gradient
# State dimension, number of data points and number of parameters
steps = Y.shape[1]
nparam = dF.shape[2]
n = F.shape[0]
# Time steps
t = X.squeeze()
# Allocate space
e = 0
eg = np.zeros(nparam)
# Set up
Z = np.zeros(F.shape)
QC = L.dot(Qc).dot(L.T)
m = np.zeros([n, 1])
P = Pinf.copy()
dm = np.zeros([n, nparam])
dP = dPinf.copy()
mm = m.copy()
PP = P.copy()
# % Initial dt
dt = -np.Inf
# Allocate space for expm results
AA = np.zeros([2 * F.shape[0], 2 * F.shape[0], nparam])
AAA = np.zeros([4 * F.shape[0], 4 * F.shape[0], nparam])
FF = np.zeros([2 * F.shape[0], 2 * F.shape[0]])
FFF = np.zeros([4 * F.shape[0], 4 * F.shape[0]])
# Loop over all observations
for k in range(0, steps):
# The previous time step
dt_old = dt
# The time discretization step length
if k > 0:
dt = t[k] - t[k - 1]
else:
dt = t[1] - t[0]
# Loop through all parameters (Kalman filter prediction step)
for j in range(0, nparam):
# Should we recalculate the matrix exponential?
if abs(dt - dt_old) > 1e-9:
# The first matrix for the matrix factor decomposition
FF[:n, :n] = F
FF[n:, :n] = dF[:, :, j]
FF[n:, n:] = F
# Solve the matrix exponential
AA[:, :, j] = linalg.expm3(FF * dt)
# Solve using matrix fraction decomposition
foo = AA[:, :, j].dot(np.vstack([m, dm[:, j:j + 1]]))
# Pick the parts
mm = foo[:n, :]
dm[:, j:j + 1] = foo[n:, :]
# Should we recalculate the matrix exponential?
if abs(dt - dt_old) > 1e-9:
# Define W and G
W = L.dot(dQc[:, :, j]).dot(L.T)
G = dF[:, :, j]
# The second matrix for the matrix factor decomposition
FFF[:n, :n] = F
FFF[2 * n:-n, :n] = G
FFF[:n, n:2 * n] = QC
FFF[n:2 * n, n:2 * n] = -F.T
FFF[2 * n:-n, n:2 * n] = W
FFF[-n:, n:2 * n] = -G.T
FFF[2 * n:-n, 2 * n:-n] = F
FFF[2 * n:-n, -n:] = QC
FFF[-n:, -n:] = -F.T
# Solve the matrix exponential
AAA[:, :, j] = linalg.expm3(FFF * dt)
# Solve using matrix fraction decomposition
foo = AAA[:, :, j].dot(
np.vstack([P, np.eye(n), dP[:, :, j],
np.zeros([n, n])]))
# Pick the parts
C = foo[:n, :]
D = foo[n:2 * n, :]
dC = foo[2 * n:-n, :]
dD = foo[-n:, :]
# The prediction step covariance (PP = C/D)
if j == 0:
PP = linalg.solve(D.T, C.T).T
PP = (PP + PP.T) / 2
# Sove dP for j (C/D == P_{k|k-1})
dP[:, :, j] = linalg.solve(D.T, (dC - PP.dot(dD)).T).T
# Set predicted m and P
m = mm
P = PP
# Start the Kalman filter update step and precalculate variables
S = H.dot(P).dot(H.T) + R
# We should calculate the Cholesky factor if S is a matrix
# [LS,notposdef] = chol(S,'lower');
# The Kalman filter update (S is scalar)
HtiS = H.T / S
iS = 1 / S
K = P.dot(HtiS)
v = Y[:, k] - H.dot(m)
vtiS = v.T / S
# Loop through all parameters (Kalman filter update step derivative)
for j in range(0, nparam):
# Innovation covariance derivative
dS = H.dot(dP[:, :, j]).dot(H.T) + dR[:, :, j]
# Evaluate the energy derivative for j
eg[j] = eg[j] \
- .5*np.sum(iS*dS) \
+ .5*H.dot(dm[:,j:j+1]).dot(vtiS.T) \
+ .5*vtiS.dot(dS).dot(vtiS.T) \
+ .5*vtiS.dot(H.dot(dm[:,j:j+1]))
# Kalman filter update step derivatives
dK = dP[:, :, j].dot(HtiS) - P.dot(HtiS).dot(dS) / S
dm[:, j:j + 1] = dm[:, j:j + 1] + dK.dot(v) - K.dot(H).dot(
dm[:, j:j + 1])
dKSKt = dK.dot(S).dot(K.T)
dP[:, :,
j] = dP[:, :, j] - dKSKt - K.dot(dS).dot(K.T) - dKSKt.T
# Evaluate the energy
# e = e - .5*S.shape[0]*np.log(2*np.pi) - np.sum(np.log(np.diag(LS))) - .5*vtiS.dot(v);
e = e - .5 * S.shape[0] * np.log(2 * np.pi) - np.sum(
np.log(np.sqrt(S))) - .5 * vtiS.dot(v)
# Finish Kalman filter update step
m = m + K.dot(v)
P = P - K.dot(S).dot(K.T)
# Make sure the covariances stay symmetric
P = (P + P.T) / 2
dP = (dP + dP.transpose([1, 0, 2])) / 2
# raise NameError('Debug me')
# Report
#print e
#print eg
# Return the gradient
return eg
def rot3d(self, axis, theta):
return expm3(np.cross(np.eye(3), axis / norm(axis) * theta))
def test_zero(self):
a = array([[0.,0],[0,0]])
assert_array_almost_equal(expm(a),[[1,0],[0,1]])
assert_array_almost_equal(expm2(a),[[1,0],[0,1]])
assert_array_almost_equal(expm3(a),[[1,0],[0,1]])
xLeft = yarp.DVector() xRight = yarp.DVector() # Se calcula la FK (cinematica directa) # 6-element vector describing current position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians) iccLeft.stat(xLeft) iccRight.stat(xRight) # Esta matriz representa la matriz homogenea 4x4 que va de 0 a N (0~origen, N~gripper) H_left_0_N = np.eye(4) H_right_0_N = np.eye(4) # https://stackoverflow.com/a/25709323 # axis2dcm convierte: scaled axis-angle representation (radians) -> dcm (matriz de cosenos directores) axis2dcm = lambda(axis): lin.expm3(np.cross(np.eye(3), axis)) # Matriz que queremos obtener: H(N-T) matriz homogenea desde el gripper(N) hasta el tool(T) # H(0-T) = H(0-N) * H(N-T) # H(N-T) = [H(0-N)]^-1 * H(0-T) # # r r r | px # r r r | py # r r r | pz # ---------- # 0 0 0 | 1 # # ---- Matriz H(0-N) a partir de los 6 valores del stat # H_left_0_N[:3,:3] -> disecciono la matriz unitaria, cogiendo las 3 primeras filas y las 3 primeras columnas H_left_0_N[:3,:3] = axis2dcm(xLeft[3:]) # xLeft[3:] -> 3 ultimos elementos del vector obtenido por stat (orientacion) # H_left_0_N[:3,3] -> cojo los valores de las 3 primeras filas de la cuarta columna
def rotate_vector(vector , axis , angle):
theta = np.radians(angle)
rotmat = expm3(np.cross(np.eye(3), axis/norm(axis)*theta))
return np.dot(rotmat,vector)
def M(axis, theta):
return expm3(cross(eye(3), axis/norm(axis)*theta))
print "Krylov Time: %f" % (end_time - start_time)
start_time = time.time()
testExp = expm(t * M)
testV = testExp * v
end_time = time.time()
print "Dense Pade Exp Time: %f" % (end_time - start_time)
start_time = time.time()
testExp2 = expm2(t * M)
testV2 = testExp2 * v
end_time = time.time()
print "Dense Eig Decomp Exp Time: %f" % (end_time - start_time)
start_time = time.time()
testExp3 = expm3(t * M)
testV3 = testExp3 * v
end_time = time.time()
print "Dense Taylor Exp Time: %f" % (end_time - start_time)
relativeErr = norm(testV - w) / norm(testV)
print "Relative Difference Between exp(M)*v using Krylov and Dense Pade Approx: %f" % relativeErr
if abs(relativeErr) > 1e-6:
print "Tolerance Exceeded:"
print "Testing Arnoldi Exponential on Random Dense Complex %dx%d" % (
nTest, nTest)
M = mat(complex(0.0, normConst) * random.randn(nTest, nTest))
normM = norm(M)
v = mat(random.randn(nTest, 1), dtype=complex)
