def get_f_by_comp(self, V):
V_ = V[0:V.shape[0] - 1, :]
v = V[V.shape[0] - 1, :]
g = inv(dot(transpose(V_), V_))
a = dot(v, g)
P = dot(a, transpose(V_))
return P
def get_z(self, r):
V = self.V[:, 0:r]
Vt = transpose(V)
U = dot(Vt, self.X)
Z = dot(V, U)
return Z
def describ_by_component(self, comp_ind):
V = self.V[:, comp_ind]
Vt = transpose(V)
U = dot(Vt, self.X)
Z = dot(V, U)
t = self.ts_from_z(Z)
return t
def get_v(self, X):
C = dot((1 / (self.K)), dot(X, transpose(X)))
V, _ = qr(C)
eigV = eigvals(C)
eigV = np.sort(eigV)[::-1]
ind = np.where(eigV > 0)[0]
r = len(ind)
return V, r
def preconditioned_conjugate_gradient(A, x0, b, preconditioner, max_iterations):
# Calculate the FFT of the first column on the circular of A
gA = precompute_g(A)
# Store initial solution
x = x0
# Calculate initial residuals
residual = b - fftmul(gA, x0)
# Norm of the initial residual
norm_residual0 = norm(residual, ord=2)
# Calculate M based on the preconditioner and get its inverse
Minv = inv(dot(preconditioner, preconditioner.T))
# Initial z vector
z = dot(Minv, residual)
# z^T*z
zr = inner(z, residual)
# Calculate initial direction
direction = z
for i in range(max_iterations):
if i % 10000 == 0:
logging.info('Preconditioned Conjugate Gradient iteration {}, dimenstion {}'.format(i, A.shape[0]))
# If residuals are too small, terminate the algorithm
if norm(residual, ord=2)/norm_residual0 < 0.0000001:
logging.info('Preconditioned Conjugate Gradient converged after {} iterations'.format(i))
break
# Store previous residuals and direction
old_residual = residual
old_direction = direction
old_z = z
old_zr = zr
# Caclulate new update factor
Ar = fftmul(gA, old_direction)
a = old_zr / inner(old_direction, Ar)
# Update solution
x = x + a*old_direction
# Update residuals
residual = old_residual - a*Ar
z = dot(Minv, residual)
# Update direction
zr = inner(z, residual)
b = zr / old_zr
direction = z + b*old_direction
return x, i
def get_f(self, r):
if r > self.R:
r = self.R
V_ = self.V[0:self.V.shape[0] - 1, 0:r]
v = self.V[self.V.shape[0] - 1, 0:r]
g = inv(dot(transpose(V_), V_))
a = dot(v, g)
P = dot(a, transpose(V_))
return P
def draw_lines(self):
if self.mode == 0:
self.geom = self.geometry()
self.geom = [
np.array(self.geom[i:i + 3] + [1])
for i in range(0, len(self.geom), 3)
]
self.geom = reduce(lambda l, v: l + [v, v], self.geom, [])
self.geom.pop(0)
self.geom.pop()
self.count = len(self.geom)
self.mode = 1
p = glGetFloatv(GL_TRANSPOSE_PROJECTION_MATRIX)
m = glGetFloatv(GL_TRANSPOSE_MODELVIEW_MATRIX)
ng = []
for i in range(0, len(self.geom), 2):
v = la.dot(m, self.geom[i])
w = la.dot(m, self.geom[i + 1])
def multlistcut(mat, vec):
vec = la.dot(mat, vec)
vec /= vec[3]
return list(vec)[0:3]
if v[2] < self.P0 and w[2] < self.P0:
ng += multlistcut(p, v)
ng += multlistcut(p, w)
elif v[2] < self.P0 and w[2] >= self.P0:
s = (w[2] - v[2]) / (self.P0 - v[2])
ng += multlistcut(p, v)
ng += multlistcut(p, (w - v) / s + v)
elif v[2] >= self.P0 and w[2] < self.P0:
s = (v[2] - w[2]) / (self.P0 - w[2])
ng += multlistcut(p, (v - w) / s + w)
ng += multlistcut(p, w)
glMatrixMode(GL_PROJECTION)
glPushMatrix()
glLoadIdentity()
glOrtho(-1, 1, -1, 1, -100, 100)
glMatrixMode(GL_MODELVIEW)
glPushMatrix()
glLoadIdentity()
glEnableClientState(GL_VERTEX_ARRAY)
glVertexPointer(3, GL_FLOAT, 0, ng)
glDrawArrays(GL_LINES, 0, len(ng) / 3)
glDisableClientState(GL_VERTEX_ARRAY)
glMatrixMode(GL_PROJECTION)
glPopMatrix()
glMatrixMode(GL_MODELVIEW)
glPopMatrix()
def draw_lines( self ) : if self.mode == 0 : self.geom = self.geometry() self.geom = [ np.array(self.geom[i:i+3]+[1]) for i in range(0,len(self.geom),3) ] self.geom = reduce( lambda l , v : l+[v,v] , self.geom , [] ) self.geom.pop(0) self.geom.pop() self.count = len(self.geom) self.mode = 1 p = glGetFloatv(GL_TRANSPOSE_PROJECTION_MATRIX) m = glGetFloatv(GL_TRANSPOSE_MODELVIEW_MATRIX) ng = [] for i in range(0,len(self.geom),2) : v = la.dot(m,self.geom[i ]) w = la.dot(m,self.geom[i+1]) def multlistcut( mat , vec ) : vec = la.dot(mat,vec) vec/=vec[3] return list(vec)[0:3] if v[2] < self.P0 and w[2] < self.P0 : ng += multlistcut(p,v) ng += multlistcut(p,w) elif v[2] < self.P0 and w[2] >=self.P0 : s = (w[2] - v[2])/(self.P0-v[2]) ng += multlistcut(p,v) ng += multlistcut(p,(w-v)/s + v) elif v[2] >=self.P0 and w[2] < self.P0 : s = (v[2] - w[2])/(self.P0-w[2]) ng += multlistcut(p,(v-w)/s + w) ng += multlistcut(p,w) glMatrixMode(GL_PROJECTION) glPushMatrix() glLoadIdentity() glOrtho( -1 , 1 , -1 , 1 , -100 , 100 ) glMatrixMode(GL_MODELVIEW) glPushMatrix() glLoadIdentity() glEnableClientState(GL_VERTEX_ARRAY) glVertexPointer(3,GL_FLOAT,0,ng) glDrawArrays(GL_LINES,0,len(ng)/3) glDisableClientState(GL_VERTEX_ARRAY) glMatrixMode(GL_PROJECTION) glPopMatrix() glMatrixMode(GL_MODELVIEW) glPopMatrix()
def Apply(self,h,tf,y0_,f_):
nt=int(tf/h)
t_ = [i*h for i in xrange(nt+1)]
y__= [y0_ for i in xrange(nt+1)]
k__= [y0_ for i in xrange(self.n)]
for i in xrange(nt):
k__[0] = f_(t_[i],y__[i])
for j in xrange(1, self.n):
k__[j] = f_(t_[i] + self.c[j-1]*h, y__[i] + h*dot(self.a[j-1], k__[0:j]))
y__[i+1] = y__[i] + h*dot(self.b, k__)
print(i)
return y__, t_
def RKX(self, f, t0, Y0, n, tf):
h = (tf-t0)/(1.0*n)
Y = zeros(len(Y0),n+1)
Y[:,0] = Y0
t = t0
k = [zeros(len(Y0),1) for i in range(self.N)]
for i in range(n):
k[0] = f(t, Y[:,i])
for j in range(self.N-1):
k[j+1] = f(t + h*self.b[j], Y[:,i] + h*dot(self.a[j],k[0:j+1]) )
Y[:,i+1] = Y[:,i] + h*dot(self.c,k)
t += h
return Y
def move_vec( self , vec ) : vec = np.array(vec) vec = vec * 0.001 vec.resize( 4 , refcheck=False ) vec = la.dot( la.inv( np.reshape(self.currmv,(4,4)) ) , vec ) self.translate( *vec[0:3] ) return vec[0:3]
def move_vec(self, vec):
vec = np.array(vec)
vec = vec * 0.001
vec.resize(4, refcheck=False)
vec = la.dot(la.inv(np.reshape(self.currmv, (4, 4))), vec)
self.translate(*vec[0:3])
return vec[0:3]
def forecast(self, n):
N = len(self.pred_ts)
for k in range(n):
q = self.pred_ts[N - self.L + 1 + k:N + k]
q = np.reshape(q, (len(q), 1))
f = dot(self.F, q)
self.pred_ts = np.concatenate((self.pred_ts, f))
return self.pred_ts[N:]
def compute_cos_similarity(self, doc_vector, query_vector):
'''
Uses the dot produt to compute cosine similarity between vectors
:param doc_vector: a document vector
:param query_vector: a query vector
:return: the cosine similarity between the document and query vectors
'''
return linalg.dot(doc_vector, query_vector) / (
linalg.norm(doc_vector) * linalg.norm(query_vector))
def hsqrt(A):
'''
Computes Hermitian square root of input Hermitian matrix A
Parameters
----------
A : (N, N) array_like
A square array of real or complex elements
Returns
-------
B : (N, N) ndarray (matrix)
Hermitian square root of Ax
'''
w, V = eig(A)
D = np.diag(np.sqrt(w))
B = dot(V, dot(D, V.T))
return B
def _find_nearest( self , pos , containter , mindist = .05 ) :
def dist( a , b ) :
return m.pow(a[0]-b[0],2) + m.pow(a[1]-b[1],2)
mc = la.dot( self.currp , self.currmv )
minv= None
minp= None
for mp in containter :
p = la.dot( mc , np.array((mp[0],mp[1],mp[2],1)) )
p = p / p[3]
if minv == None or minv > dist( pos , p ) :
minp = mp
minv = dist( pos , p )
if not mindist or minv <= mindist :
return ( minv , minp )
else :
return ( float('inf') , None )
def problem4():
logging.basicConfig(filename='problem4.log', level=logging.DEBUG)
logging.info('Starting problem 4')
lmin = 10
lmax = 13
matrix = [my_toeplitz(2**l) for l in range(lmin, lmax + 1)]
preconditioner = [my_preconditioner(2**l) for l in range(lmin, lmax + 1)]
precond_matrix = [
dot(inv(preconditioner[i]), matrix[i]) for i in range(len(matrix))
]
matrix_eig = [eigvals(m) for m in matrix]
precond_matrix_eig = [eigvals(m) for m in precond_matrix]
matrix_cond = [
max(matrix_eig[i]) / min(matrix_eig[i]) for i in range(len(matrix_eig))
]
precond_matrix_cond = [
max(precond_matrix_eig[i]) / min(precond_matrix_eig[i])
for i in range(len(precond_matrix_eig))
]
logging.info('Ending problem 4')
mateig = plt.plot([2**i for i in range(lmin, lmax + 1)],
matrix_cond,
label='CondT')
pmateig = plt.plot([2**i for i in range(lmin, lmax + 1)],
precond_matrix_cond,
label='CondPT')
plt.xlabel('Matrix size (n)')
plt.ylabel('Condition')
mateig_line = mpatches.Patch(color='blue', label='$T_n$')
pmateig_line = mpatches.Patch(color='green', label='$P_n^{-1}T_n$')
plt.legend(handles=[mateig_line, pmateig_line], loc=0)
plt.savefig('4condition')
plt.close()
mateig = plt.plot([2**i for i in range(lmin, lmax + 1)],
log10(matrix_cond),
label='CondT')
pmateig = plt.plot([2**i for i in range(lmin, lmax + 1)],
log10(precond_matrix_cond),
label='CondPT')
plt.xlabel('Matrix size (n)')
plt.ylabel('$Log_{10}(Condition)$')
mateig_line = mpatches.Patch(color='blue', label='$T_n$')
pmateig_line = mpatches.Patch(color='green', label='$P_n^{-1}T_n$')
plt.legend(handles=[mateig_line, pmateig_line], loc=0)
plt.savefig('4logcondition')
def forecast_by_component(self, n, comp_ind):
V = self.V[:, comp_ind]
F = self.get_f_by_comp(V)
N = len(self.pred_ts)
for k in range(n):
q = self.pred_ts[N - self.L + 1 + k:N + k]
q = np.reshape(q, (len(q), 1))
f = dot(F, q)
self.pred_ts = np.concatenate((self.pred_ts, f))
pred = self.pred_ts[N:]
self.pred_ts = self.ts
return pred
def gencosine(a, b, C=1):
"""
Computes generalized cosine of angle
between two vectors a and b in subspace spanned by C
Parameters
----------
a, b : (N, 1) array_like
Input vectors of real or complex element
Returns
-------
x : scalar
Generalized cosine of angle between a and b in subspace of C
"""
# Validate input
if not (isvector(a) and isvector(b)):
raise TypeError("Input not a vector")
if not C.ndim == 2:
raise TypeError("Third argument must be a matrix")
nr = np.abs(dot(a.T, dot(C, b)))**2
dr = dot(a.T, dot(C, a)) * dot(b.T, dot(C, b));
x = float(nr)/dr
return x
def loadCartesianPath(self):
"""
Loads the Path from file specified earlier
"""
rospy.loginfo('Loading Data ...')
data = numpy.loadtxt(self.dataPath)
posList = []
quatList = []
posList.append(data[0, 0:3])
quatList.append(data[0, 3:7])
# Check to send waypoint only if they are different (trajectory execution fails else)
for i in range(1, len(data)):
currPos = posList[-1]
currQuat = quatList[-1]
posDiff = norm(data[i, 0:3] - currPos)
orientDiff = 1 - (dot(data[i, 3:7], currQuat))**2
if posDiff >= self.MIN_POS_DIFF or orientDiff >= self.MIN_ANG_DIFF:
posList.append(data[i, 0:3])
quatList.append(data[i, 3:7])
# Populating waypoint list
for i in range(len(posList)):
goalPosition = geometry_msgs.msg.Point(x=posList[i][0],
y=posList[i][1],
z=posList[i][2])
goalOrientation = geometry_msgs.msg.Quaternion(x=quatList[i][0],
y=quatList[i][1],
z=quatList[i][2],
w=quatList[i][3])
goalPose = geometry_msgs.msg.Pose(position=goalPosition,
orientation=goalOrientation)
self.goalList.append(goalPose)
def multlistcut( mat , vec ) : vec = la.dot(mat,vec) vec/=vec[3] return list(vec)[0:3]
def get_clipping_pos(self):
pos = la.dot(self.currp, la.dot(self.currmv, np.array([0, 0, 0, 1])))
pos = pos / pos[3]
return pos[:3]
#set up variables for the SGD algorithm
weight = np.zeros((np.size(feature,
axis=1), )) #initialize weight vector to all zeros
i = 0
'''
# of iterations allowed should be the minimum of (maximum iteration specified by the user,
and the # of observations in our dataset - i.e. the # of rows in feature matrix)
'''
iter_limit = min(max_iters, len(feature))
history = [
] #initialize a list called history to record the update history for visualization
while i < iter_limit: #keep SGD updates until reaching the maximum # of iterations allowed
#compute the gradient of the loss with respect to w
grad = (linalg.dot(np.transpose(weight), feature[i]) -
target[i]) * feature[i] + beta * weight
update = lr * grad #compute amount needs to be updated to the current weight vector
weight = weight - update #SGD update
history.append(update.mean()) #log the average update magnitude
#If the absolute value of average update is smaller than our tolerance, stop the SGD process
if np.absolute(update.mean()) < tol: break
i += 1
#print("Last 10 updates:")
#print(history[-1:-10:-1])
np.savetxt(output, weight,
delimiter=' ') #save final weight vector to output path
#visualizing the update process, see if converges
plt.plot(history)
def compute_cos_similarity(self, doc_vector, query_vector):
return linalg.dot(doc_vector, query_vector) / (
linalg.norm(doc_vector) * linalg.norm(query_vector))
def get_clipping_pos( self ) : pos = la.dot( self.currp , la.dot( self.currmv , np.array( [0,0,0,1] ) ) ) pos = pos / pos[3] return pos[:3]
def multlistcut(mat, vec):
vec = la.dot(mat, vec)
vec /= vec[3]
return list(vec)[0:3]
def Akl(k, l):
if fixed is None: s = lam
else:
if k == 0 and l == 0: s = lam
else: s = 0.
for m in measurements:
d = 1. / m.uncertainty**2
if k == l and (m.i == k or m.j == k): s += d
if (m.i == k and m.j == l) or (m.j == k and m.i == l): s -= d
return s
# Equation 17
v = [vk(k) for k in range(Na)]
M = matrix([[Akl(k, l) for k in range(Na)] for l in range(Na)])
print dot(inv(M), v)
def print_matrix(mat):
if not isinstance(mat, dict):
m = {}
for i in range(Na):
for j in range(Na):
m["A%d" % i, "A%d" % j] = mat[i, j]
else:
m = mat
print "\n".join([
" ".join(["%8.2g" % m["A%d" % i, "A%d" % j] for j in range(Na)])
for i in range(Na)
])
