def firstInitElem(m33,sortEigs,alpha,A):
l1=sortEigs[0]
l2=sortEigs[1]
l3=sortEigs[2]
m1=-m33+l1+l2+l3
m2=-((l2-l3)*(l1**2-l1*l2-l1*l3+l2*l3)*(m33**3-2*m33**2*l1+m33*l1**2-2*m33**2*l2+3*m33*l1*l2-l1**2*l2+m33*l2**2- \
l1*l2**2-2*m33**2*l3+3*m33*l1*l3-l1**2*l3+3*m33*l2*l3-2*l1*l2*l3-l2**2*l3+m33*l3**2-l1*l3**2-l2*l3**2))/ \
(2*m33*l1**2*l2+2*m33**2*l1**2*l2-l1**3*l2-3*m33*l1**3*l2+l1**4*l2-2*m33*l1*l2**2-2*m33**2*l1*l2**2+l1**3*l2**2+ \
l1*l2**3+3*m33*l1*l2**3-l1**2*l2**3-l1*l2**4-2*m33*l1**2*l3-2*m33**2*l1**2*l3+l1**3*l3+3*m33*l1**3*l3-l1**4*l3+ \
2*m33*l2**2*l3+2*m33**2*l2**2*l3-l2**3*l3-3*m33*l2**3*l3+l2**4*l3+2*m33*l1*l3**2+2*m33**2*l1*l3**2-l1**3*l3**2- \
2*m33*l2*l3**2-2*m33**2*l2*l3**2+l2**3*l3**2-l1*l3**3-3*m33*l1*l3**3+l1**2*l3**3+l2*l3**3+3*m33*l2*l3**3-l2**2*l3**3+ \
l1*l3**4-l2*l3**4)
m3=((l2-l3)*(l1**2-l1*l2-l1*l3+l2*l3)*(m33**3+m33**4-m33**2*l1-2*m33**3*l1+m33**2*l1**2-m33**2*l2-2*m33**3*l2+ \
m33*l1*l2+3*m33**2*l1*l2-m33*l1**2*l2+m33**2*l2**2-m33*l1*l2**2-m33**2*l3-2*m33**3*l3+m33*l1*l3+3*m33**2*l1*l3- \
m33*l1**2*l3+m33*l2*l3+3*m33**2*l2*l3-l1*l2*l3-4*m33*l1*l2*l3+l1**2*l2*l3-m33*l2**2*l3+l1*l2**2*l3+m33**2*l3**2- \
m33*l1*l3**2-m33*l2*l3**2+l1*l2*l3**2))/(-2*m33*l1**2*l2-2*m33**2*l1**2*l2-m33**3*l1**2*l2+l1**3*l2+3*m33*l1**3*l2+ \
2*m33**2*l1**3*l2-l1**4*l2-m33*l1**4*l2+2*m33*l1*l2**2+2*m33**2*l1*l2**2+m33**3*l1*l2**2-l1**3*l2**2-2*m33*l1**3*l2**2+ \
l1**4*l2**2-l1*l2**3-3*m33*l1*l2**3-2*m33**2*l1*l2**3+l1**2*l2**3+2*m33*l1**2*l2**3+l1*l2**4+m33*l1*l2**4-l1**2*l2**4+ \
2*m33*l1**2*l3+2*m33**2*l1**2*l3+m33**3*l1**2*l3-l1**3*l3-3*m33*l1**3*l3-2*m33**2*l1**3*l3+l1**4*l3+m33*l1**4*l3- \
2*m33*l2**2*l3-2*m33**2*l2**2*l3-m33**3*l2**2*l3+l2**3*l3+3*m33*l2**3*l3+2*m33**2*l2**3*l3-l2**4*l3-m33*l2**4*l3- \
2*m33*l1*l3**2-2*m33**2*l1*l3**2-m33**3*l1*l3**2+l1**3*l3**2+2*m33*l1**3*l3**2-l1**4*l3**2+2*m33*l2*l3**2+2*m33**2*l2*l3**2+ \
m33**3*l2*l3**2-l2**3*l3**2-2*m33*l2**3*l3**2+l2**4*l3**2+l1*l3**3+3*m33*l1*l3**3+2*m33**2*l1*l3**3-l1**2*l3**3- \
2*m33*l1**2*l3**3-l2*l3**3-3*m33*l2*l3**3-2*m33**2*l2*l3**3+l2**2*l3**3+2*m33*l2**2*l3**3-l1*l3**4-m33*l1*l3**4+ \
l1**2*l3**4+l2*l3**4+m33*l2*l3**4-l2**2*l3**4)
b3=np.sum(ml.eye(3)-A,1)/(1-m3-m33)
b2=(-m33*ml.eye(3)+A)*b3/(1-m2)
b1=(-m3*b3+A*b2)/(1-m1)
B=ml.hstack((b1,b2,b3))
a1=alpha*b1
return (a1,m1,m2,m3,B)
def firstInitElem(m33, sortEigs, alpha, A):
l1 = sortEigs[0]
l2 = sortEigs[1]
l3 = sortEigs[2]
m1 = -m33 + l1 + l2 + l3
m2=-((l2-l3)*(l1**2-l1*l2-l1*l3+l2*l3)*(m33**3-2*m33**2*l1+m33*l1**2-2*m33**2*l2+3*m33*l1*l2-l1**2*l2+m33*l2**2- \
l1*l2**2-2*m33**2*l3+3*m33*l1*l3-l1**2*l3+3*m33*l2*l3-2*l1*l2*l3-l2**2*l3+m33*l3**2-l1*l3**2-l2*l3**2))/ \
(2*m33*l1**2*l2+2*m33**2*l1**2*l2-l1**3*l2-3*m33*l1**3*l2+l1**4*l2-2*m33*l1*l2**2-2*m33**2*l1*l2**2+l1**3*l2**2+ \
l1*l2**3+3*m33*l1*l2**3-l1**2*l2**3-l1*l2**4-2*m33*l1**2*l3-2*m33**2*l1**2*l3+l1**3*l3+3*m33*l1**3*l3-l1**4*l3+ \
2*m33*l2**2*l3+2*m33**2*l2**2*l3-l2**3*l3-3*m33*l2**3*l3+l2**4*l3+2*m33*l1*l3**2+2*m33**2*l1*l3**2-l1**3*l3**2- \
2*m33*l2*l3**2-2*m33**2*l2*l3**2+l2**3*l3**2-l1*l3**3-3*m33*l1*l3**3+l1**2*l3**3+l2*l3**3+3*m33*l2*l3**3-l2**2*l3**3+ \
l1*l3**4-l2*l3**4)
m3=((l2-l3)*(l1**2-l1*l2-l1*l3+l2*l3)*(m33**3+m33**4-m33**2*l1-2*m33**3*l1+m33**2*l1**2-m33**2*l2-2*m33**3*l2+ \
m33*l1*l2+3*m33**2*l1*l2-m33*l1**2*l2+m33**2*l2**2-m33*l1*l2**2-m33**2*l3-2*m33**3*l3+m33*l1*l3+3*m33**2*l1*l3- \
m33*l1**2*l3+m33*l2*l3+3*m33**2*l2*l3-l1*l2*l3-4*m33*l1*l2*l3+l1**2*l2*l3-m33*l2**2*l3+l1*l2**2*l3+m33**2*l3**2- \
m33*l1*l3**2-m33*l2*l3**2+l1*l2*l3**2))/(-2*m33*l1**2*l2-2*m33**2*l1**2*l2-m33**3*l1**2*l2+l1**3*l2+3*m33*l1**3*l2+ \
2*m33**2*l1**3*l2-l1**4*l2-m33*l1**4*l2+2*m33*l1*l2**2+2*m33**2*l1*l2**2+m33**3*l1*l2**2-l1**3*l2**2-2*m33*l1**3*l2**2+ \
l1**4*l2**2-l1*l2**3-3*m33*l1*l2**3-2*m33**2*l1*l2**3+l1**2*l2**3+2*m33*l1**2*l2**3+l1*l2**4+m33*l1*l2**4-l1**2*l2**4+ \
2*m33*l1**2*l3+2*m33**2*l1**2*l3+m33**3*l1**2*l3-l1**3*l3-3*m33*l1**3*l3-2*m33**2*l1**3*l3+l1**4*l3+m33*l1**4*l3- \
2*m33*l2**2*l3-2*m33**2*l2**2*l3-m33**3*l2**2*l3+l2**3*l3+3*m33*l2**3*l3+2*m33**2*l2**3*l3-l2**4*l3-m33*l2**4*l3- \
2*m33*l1*l3**2-2*m33**2*l1*l3**2-m33**3*l1*l3**2+l1**3*l3**2+2*m33*l1**3*l3**2-l1**4*l3**2+2*m33*l2*l3**2+2*m33**2*l2*l3**2+ \
m33**3*l2*l3**2-l2**3*l3**2-2*m33*l2**3*l3**2+l2**4*l3**2+l1*l3**3+3*m33*l1*l3**3+2*m33**2*l1*l3**3-l1**2*l3**3- \
2*m33*l1**2*l3**3-l2*l3**3-3*m33*l2*l3**3-2*m33**2*l2*l3**3+l2**2*l3**3+2*m33*l2**2*l3**3-l1*l3**4-m33*l1*l3**4+ \
l1**2*l3**4+l2*l3**4+m33*l2*l3**4-l2**2*l3**4)
b3 = np.sum(ml.eye(3) - A, 1) / (1 - m3 - m33)
b2 = (-m33 * ml.eye(3) + A) * b3 / (1 - m2)
b1 = (-m3 * b3 + A * b2) / (1 - m1)
B = ml.hstack((b1, b2, b3))
a1 = alpha * b1
return (a1, m1, m2, m3, B)
def elementary (erep, b, k):
bestdist = evalfun (erep, k)
bestrep = erep
repSize = erep[0].shape[1]
for i in range(repSize):
for j in range(repSize):
if i!=j:
# create elementary transformation matrix with +b
B = ml.eye(repSize)
B[i,j] = b
B[i,i] = 1.0 - b
# apply similarity transform
newrep = transfun (erep, B)
newdist = evalfun (newrep, k)
# store result if better
if newdist < bestdist:
bestrep = newrep
bestdist = newdist
# create elementary transformation matrix with -b
B = ml.eye(repSize)
B[i,j] = -b
B[i,i] = 1.0 + b
# apply similarity transform
newrep = transfun (erep, B)
newdist = evalfun (newrep, k)
# store result if better
if newdist < bestdist:
bestrep = newrep
bestdist = newdist
return (bestrep, bestdist)
def __init__(self, **kwargs):
super(Sensor, self).__init__( **kwargs )
self.properties.update( measurementList = kwargs.get('measurementList', self.measurementList ),
stateList = kwargs.get('stateList',self.stateList ) )
for attribute in self.measurementList:
setattr(self, attribute, kwargs.get(attribute, 0.) )
for key in self.stateList:
attribute = key.split('.')[1]
setattr( self, attribute, kwargs.get(attribute, 0.) )
self.properties.update(
Z = kwargs.get('Z', matlib.zeros( [len(self), 1] )),
StateMap = kwargs.get('StateMap', matlib.zeros( [len(self) , 1] )),
MeasurementJacobian = kwargs.get('MeasurementJacobian', matlib.eye( len(self) ) ),
MeasurementCovariance = kwargs.get('MeasurementCovariance', 0.3 * matlib.eye( len(self) )),
)
# self.Z = kwargs.get('Z', matlib.zeros( [len(self), 1] ))
# self.StateMap = kwargs.get('StateMap', matlib.zeros( [len(self) , 1] ))
# self.MeasurementJacobian = kwargs.get('MeasurementJacobian', matlib.eye( len(self) ) )
# self.MeasurementCovariance = kwargs.get('MeasurementCovariance', 0.3 * matlib.eye( len(self) ))
self.set_Z()
def __init__(self):
# model dimensions
self.xDim = 2 # state space dimension
self.uDim = 2 # control input dimension
self.qDim = 2 # dynamics noise dimension
self.zDim = 2 # observation dimension
self.rDim = 2 # observtion noise dimension
# belief space dimension
# note that we only store the lower (or upper) triangular portion
# of the covariance matrix to eliminate redundancy
self.bDim = int(self.xDim + self.xDim*((self.xDim+1)/2.))
self.dT = 1. # time step for dynamics function
self.T = 15 # number of time steps in trajectory
self.alpha_belief = 10. # weighting factor for penalizing uncertainty at intermediate time steps
self.alpha_final_belief = 10. # weighting factor for penalizing uncertainty at final time step
self.alpha_control = 1. # weighting factor for penalizing control cost
self.xMin = ml.vstack([-5,-3]) # minimum limits on state (xMin <= x)
self.xMax = ml.vstack([5,3]) # maximum limits on state (x <= xMax)
self.uMin = ml.vstack([-1,-1]) # minimum limits on control (uMin <= u)
self.uMax = ml.vstack([1,1]) # maximum limits on control (u <= uMax)
self.Q = ml.eye(self.qDim) # dynamics noise variance
self.R = ml.eye(self.rDim) # observation noise variance
self.start = ml.zeros([self.xDim,1]) # start state, OVERRIDE
self.goal = ml.zeros([self.xDim,1]) # end state, OVERRIDE
self.sqpParams = LightDarkSqpParams()
def GS_basis(Kc, verbose=False):
(n, n_) = Kc.shape
assert n == n_
if verbose: print "Beginning GS process. n = {}".format(n)
G = mat.eye(n,n)
deleted = 0
for ell in xrange(n):
if verbose:
if ell%10 == 0: print "ell = {}".format(ell)
ell -= deleted
for i in xrange(ell):
G[:,ell] -= (G.T * Kc * G)[ell,i] * G[:,i]
cf = (G.T*Kc*G)[ell,ell]
if cf < (10**-8):
if verbose: print "Deleting column " + str(ell)
G = mat.delete(G, ell, 1)
deleted += 1
else:
G[:,ell] /= cf ** .5
GKG = G.T*Kc*G
(dummy, g_dim) = G.shape
assert not mat.isnan(G).any()
if verbose:
print "GKG Divergence: {:e}".format(sum(abs(GKG - mat.eye(g_dim,g_dim))))
return G
def elementary(erep, b, k):
bestdist = evalfun(erep, k)
bestrep = erep
repSize = erep[0].shape[1]
for i in range(repSize):
for j in range(repSize):
if i != j:
# create elementary transformation matrix with +b
B = ml.eye(repSize)
B[i, j] = b
B[i, i] = 1.0 - b
# apply similarity transform
newrep = transfun(erep, B)
newdist = evalfun(newrep, k)
# store result if better
if newdist < bestdist:
bestrep = newrep
bestdist = newdist
# create elementary transformation matrix with -b
B = ml.eye(repSize)
B[i, j] = -b
B[i, i] = 1.0 + b
# apply similarity transform
newrep = transfun(erep, B)
newdist = evalfun(newrep, k)
# store result if better
if newdist < bestdist:
bestrep = newrep
bestdist = newdist
return (bestrep, bestdist)
def __init__(self):
# model dimensions
self.xDim = 2 # state space dimension
self.uDim = 2 # control input dimension
self.qDim = 2 # dynamics noise dimension
self.zDim = 2 # observation dimension
self.rDim = 2 # observtion noise dimension
# belief space dimension
# note that we only store the lower (or upper) triangular portion
# of the covariance matrix to eliminate redundancy
self.bDim = int(self.xDim + self.xDim*((self.xDim+1)/2.))
self.dT = 1. # time step for dynamics function
self.T = 15 # number of time steps in trajectory
self.alpha_belief = 10. # weighting factor for penalizing uncertainty at intermediate time steps
self.alpha_final_belief = 10. # weighting factor for penalizing uncertainty at final time step
self.alpha_control = 1. # weighting factor for penalizing control cost
self.xMin = ml.vstack([-5,-3]) # minimum limits on state (xMin <= x)
self.xMax = ml.vstack([5,3]) # maximum limits on state (x <= xMax)
self.uMin = ml.vstack([-1,-1]) # minimum limits on control (uMin <= u)
self.uMax = ml.vstack([1,1]) # maximum limits on control (u <= uMax)
self.Q = ml.eye(self.qDim) # dynamics noise variance
self.R = ml.eye(self.rDim) # observation noise variance
self.start = ml.zeros([self.xDim,1]) # start state, OVERRIDE
self.goal = ml.zeros([self.xDim,1]) # end state, OVERRIDE
self.sqpParams = LightDarkSqpParams()
def updatePsi(self):
self.PsicXc = np.zeros((self.nc,self.nc), dtype=np.float)
self.PsicXe = np.zeros((self.ne,self.ne), dtype=np.float)
self.PsicXcXe = np.zeros((self.nc,self.ne), dtype=np.float)
#
# print self.thetac
# print self.pc
# print self.distanceXc
newPsicXc = np.exp(-np.sum(self.thetac*np.power(self.distanceXc,self.pc), axis=2))
print newPsicXc[0]
self.PsicXc = np.triu(newPsicXc,1)
self.PsicXc = self.PsicXc + self.PsicXc.T + np.mat(eye(self.nc))+np.multiply(np.mat(eye(self.nc)),np.spacing(1))
self.UPsicXc = np.linalg.cholesky(self.PsicXc)
self.UPsicXc = self.UPsicXc.T
print self.PsicXc[0]
print self.UPsicXc
exit()
newPsicXe = np.exp(-np.sum(self.thetac*np.power(self.distanceXe,self.pc), axis=2))
self.PsicXe = np.triu(newPsicXe,1)
self.PsiXe = self.PsicXe + self.PsicXe.T + np.mat(eye(self.ne))+np.multiply(np.mat(eye(self.ne)),np.spacing(1))
self.UPsicXe = np.linalg.cholesky(self.PsicXe)
self.UPsicXe = self.UPsicXe.T
newPsiXeXc = np.exp(-np.sum(self.thetad*np.power(self.distanceXcXe,self.pd), axis=2))
self.PsicXcXe = np.triu(newPsiXeXc,1)
def rel2abs(pred):
# relative to absolute transform
data_size = pred.shape[0]
#print(pred.shape)
#print(pred)
R = rb.rpy2tr(pred[:, 0:3])
#print(R[0])
t = rb.transl(pred[:, 3:6])
Tl = []
Tl.append(npmat.eye(4)) # T0
Tl.append(
npmat.mat(
np.concatenate((np.concatenate(
(R[0][0:3, 0:3], t[0][0:3, 3]), 1), npmat.mat([0, 0, 0, 1])),
0))) #T1
T = np.zeros((data_size + 1, 4, 4))
T[0, :, :] = npmat.eye(4)
T[1, :, :] = npmat.mat(
np.concatenate((np.concatenate(
(R[0][0:3, 0:3], t[0][0:3, 3]), 1), npmat.mat([0, 0, 0, 1])), 0))
for k in range(2, data_size + 1):
Tn = npmat.mat(
np.concatenate(
(np.concatenate((R[k - 1][0:3, 0:3], t[k - 1][0:3, 3]),
1), npmat.mat([0, 0, 0, 1])),
0)) #relative transform from k-1 to k
Tl.append(Tl[k - 1].dot(Tn))
T[k, :, :] = Tl[k]
return Tl, T
def compute_x_T_and_two_integrals(A, a, x0, T):
'''
Define a new variable:
z(t) = (1, x(t), w(t), y(t))
where:
w(t) = int_0^t x(s) ds
y(t) = int_0^t w(s) ds
and then the dynamic of z(t) is:
d1 = 0
dx = a*1 + A*x
dw = x
dy = w
which we can re-write in matrix form as:
(0 0 0 0)
(a A 0 0)
dz = (0 I 0 0) z
(0 0 I 0)
dz = C z
So we can compute x(t), w(t), y(t) by computing
z(t) = e^{tC} z(0)
with z(0) = (1, x(0), 0, 0)
'''
n = A.shape[0]
C = matlib.zeros((3 * n + 1, 3 * n + 1))
C[1:1 + n, 0] = a
C[1:1 + n, 1:1 + n] = A
C[1 + n:1 + 2 * n, 1:1 + n] = matlib.eye(n)
C[1 + 2 * n:, 1 + n:1 + 2 * n] = matlib.eye(n)
z0 = np.vstack((1, x0, matlib.zeros((2 * n, 1))))
e_TC = expm(T * C)
z = e_TC * z0
x = z[1:1 + n, 0]
int_x = z[1 + n:2 * n + 1, 0]
int2_x = z[1 + 2 * n:, 0]
return x, int_x, int2_x
def GS_basis(Kc, verbose=False):
(n, n_) = Kc.shape
assert n == n_
if verbose: print "Beginning GS process. n = {}".format(n)
G = mat.eye(n, n)
deleted = 0
for ell in xrange(n):
if verbose:
if ell % 10 == 0: print "ell = {}".format(ell)
ell -= deleted
for i in xrange(ell):
G[:, ell] -= (G.T * Kc * G)[ell, i] * G[:, i]
cf = (G.T * Kc * G)[ell, ell]
if cf < (10**-8):
if verbose: print "Deleting column " + str(ell)
G = mat.delete(G, ell, 1)
deleted += 1
else:
G[:, ell] /= cf**.5
GKG = G.T * Kc * G
(dummy, g_dim) = G.shape
assert not mat.isnan(G).any()
if verbose:
print "GKG Divergence: {:e}".format(
sum(abs(GKG - mat.eye(g_dim, g_dim))))
return G
def test_basic(self):
# test basic construction
T = Transform()
assert_allclose(T, np.matrix(np.eye(4)))
# test initializing with rotation matrix
T = Transform(Rpitch(pi / 4))
N = np.eye(4)
N[:3, :3] = Rpitch(pi / 4)
assert_allclose(T, N)
# test initializing with position vector
pos = np.matrix([5, 2, 4]).T
T = Transform(p=pos)
N = np.eye(4)
N[:3, 3] = pos
assert_allclose(T, N)
# test initializing with both rotation and position vector
R = Ryaw(5)
p = np.matrix('3;2;4')
T = Transform(R, p)
N = np.eye(4)
N[:3, :3] = R
N[:3, 3] = p
assert_allclose(T, N)
def __new__(cls, R=np.eye(3), p=np.matrix([0, 0, 0]).T):
if R is None and p is None:
obj = super(Transform, cls).__new__(cls, np.eye(4))
else:
obj = super(Transform,
cls).__new__(cls, np.block([[R, p], [0, 0, 0, 1]]))
# obj = np.block([[R, p], [0, 0, 0, 1]]) appears to be about the same
return obj.view(cls)
def getOrthColumns(m):
'''
Constructs the orthogonally complementing columns of the input.
Input of the form pxr is assumed to have r<=p,
and have either full column rank r or rank 0 (scalar or matrix)
Output is of the form px(p-r), except:
a) if M square and full rank p, returns scalar 0
b) if rank(M)=0 (zero matrix), returns I_p
(Note you cannot pass scalar zero, because dimension info would be
missing.)
Return type is as input type.
'''
if type(m) == type(asarray(m)):
m = mat(m)
output = 'array'
else: output = 'matrix'
p, r = m.shape
# first catch the stupid input case
if p < r: raise ValueError, 'need at least as many rows as columns'
# we use lstsq(M, ones) just to exploit its rank-finding algorithm,
rk = lstsq(m, ones(p).T)[2]
# first the square and full rank case:
if rk == p: result = zeros((p,0)) # note the shape! hopefully octave-like
# then the zero-matrix case (within machine precision):
elif rk == 0: result = eye(p)
# now the rank-deficient case:
elif rk < r:
raise ValueError, 'sorry, matrix does not have full column rank'
# (what's left should be ok)
else:
# we have to watch out for zero rows in M,
# if they are in the first p-r positions!
# so the (probably inefficient) algorithm:
# 1. check the rank of each row
# 2. if zero, then also put a zero row in c
# 3. if not, put the next unit vector in c-row
idr = eye(r)
idpr = eye(p-r)
c = empty([0,r]) # starting point
co = empty([0, p-r]) # will hold orth-compl.
idrcount = 0
for row in range(p):
# (must be ones() instead of 1 because of 2d-requirement
if lstsq( m[row,:], ones(1) )[2] == 0 or idrcount >= r:
c = r_[ c, zeros(r) ]
co = r_[ co, idpr[row-idrcount, :] ]
else: # row is non-zero, and we haven't used all unit vecs
c = r_[ c, idr[idrcount, :] ]
co = r_[ co, zeros(p-r) ]
idrcount += 1
# earlier non-general (=bug) line: c = mat(r_[eye(r), zeros((p-r, r))])
# and: co = mat( r_[zeros((r, p-r)), eye(p-r)] )
# old:
# result = ( eye(p) - c * (M.T * c).I * M.T ) * co
result = co - c * solve(m.T * c, m.T * co)
if output == 'array': return result.A
else: return result
def DMRAPFromMoments(moms, Nm):
"""
Creates a discrete marked rational arrival process that
has the same marginal and lag-1 joint moments as given
(see [1]_).
Parameters
----------
moms : vector of doubles
The list of marginal moments. To obtain a discrete
marked rational process of order M, 2*M-1 marginal
moments are required.
Nm : list of matrices, shape (M,M)
The list of lag-1 joint moment matrices. The
length of the list determines K, the number of arrival
types of the discrete rational process.
Returns
-------
H : list of matrices, shape (M,M)
The H0, H1, ..., HK matrices of the discrete marked
rational process
References
----------
.. [1] Andras Horvath, Gabor Horvath, Miklos Telek, "A
traffic based decomposition of two-class queueing
networks with priority service," Computer Networks
53:(8) pp. 1235-1248. (2009)
"""
v, H0 = MGFromMoments(moms)
H0i = la.inv(ml.eye(H0.shape[0]) - H0)
Ge = ml.zeros(H0.shape)
G1 = ml.zeros(H0.shape)
H0ip = ml.eye(H0.shape[0])
for i in range(H0.shape[0]):
Ge[i, :] = v * H0ip
G1[:, i] = np.sum(H0ip, 1)
H0ip *= (i + 1) * H0i
if i > 0:
H0ip *= H0
Gei = la.inv(Ge)
G1i = la.inv(G1)
H = [H0]
for i in range(1, len(Nm) + 1):
Nmi = Nm[i - 1]
row1 = np.array([FactorialMomsFromMoms(Nmi[0, 1:].A.flatten())])
col1 = np.array([FactorialMomsFromMoms(Nmi[1:, 0].A.flatten())]).T
mid = JFactorialMomsFromJMoms(Nmi[1:, 1:])
Nmi = np.bmat([[[[Nmi[0, 0]]], row1], [col1, mid]])
H.append((ml.eye(H0.shape[0]) - H0) * Gei * Nmi * G1i)
return H
def regupdatePsi(self):
self.Psi = np.zeros((self.n,self.n), dtype=np.float)
self.one = np.ones(self.n)
self.psi = np.zeros((self.n,1))
newPsi = np.exp(-np.sum(self.theta*np.power(self.distance,self.pl), axis=2))
self.Psi = np.triu(newPsi,1)
self.Psi = self.Psi + self.Psi.T + eye(self.n) + eye(self.n) * (self.Lambda)
self.U = np.linalg.cholesky(self.Psi)
self.U = np.matrix(self.U.T)
def regupdatePsi(self):
self.Psi = np.zeros((self.n, self.n), dtype=np.float)
self.one = np.ones(self.n)
self.psi = np.zeros((self.n, 1))
newPsi = np.exp(-np.sum(self.theta * np.power(self.distance, self.pl), axis=2))
self.Psi = np.triu(newPsi, 1)
self.Psi = self.Psi + self.Psi.T + eye(self.n) + eye(self.n) * (self.Lambda)
self.U = np.linalg.cholesky(self.Psi)
self.U = np.matrix(self.U.T)
def DMRAPFromMoments (moms, Nm):
"""
Creates a discrete marked rational arrival process that
has the same marginal and lag-1 joint moments as given
(see [1]_).
Parameters
----------
moms : vector of doubles
The list of marginal moments. To obtain a discrete
marked rational process of order M, 2*M-1 marginal
moments are required.
Nm : list of matrices, shape (M,M)
The list of lag-1 joint moment matrices. The
length of the list determines K, the number of arrival
types of the discrete rational process.
Returns
-------
H : list of matrices, shape (M,M)
The H0, H1, ..., HK matrices of the discrete marked
rational process
References
----------
.. [1] Andras Horvath, Gabor Horvath, Miklos Telek, "A
traffic based decomposition of two-class queueing
networks with priority service," Computer Networks
53:(8) pp. 1235-1248. (2009)
"""
v, H0 = MGFromMoments (moms)
H0i = la.inv(ml.eye(H0.shape[0])-H0)
Ge = ml.zeros(H0.shape)
G1 = ml.zeros(H0.shape)
H0ip = ml.eye(H0.shape[0])
for i in range(H0.shape[0]):
Ge[i,:] = v * H0ip
G1[:,i] = np.sum(H0ip, 1)
H0ip *= (i+1) * H0i
if i>0:
H0ip *= H0
Gei = la.inv(Ge)
G1i = la.inv(G1)
H = [H0]
for i in range(1,len(Nm)+1):
Nmi = Nm[i-1]
row1 = np.array([FactorialMomsFromMoms(Nmi[0,1:].A.flatten())])
col1 = np.array([FactorialMomsFromMoms(Nmi[1:,0].A.flatten())]).T
mid = JFactorialMomsFromJMoms(Nmi[1:,1:])
Nmi = np.bmat([[[[Nmi[0,0]]], row1 ], [col1, mid]])
H.append((ml.eye(H0.shape[0])-H0)*Gei*Nmi*G1i)
return H
def updatePsi(self):
self.Psi = np.zeros((self.n,self.n), dtype=np.float)
self.one = np.ones(self.n)
self.psi = np.zeros((self.n,1))
newPsi = np.exp(-np.sum(self.theta*np.power(self.distance,self.pl), axis=2))
self.Psi = np.triu(newPsi,1)
self.Psi = self.Psi + self.Psi.T + np.mat(eye(self.n))+np.multiply(np.mat(eye(self.n)),np.spacing(1))
self.U = np.linalg.cholesky(self.Psi)
self.U = self.U.T
def updatePsi(self):
self.Psi = np.zeros((self.n,self.n), dtype=np.float)
self.one = np.ones(self.n)
self.psi = np.zeros((self.n,1))
newPsi = np.exp(-np.sum(self.theta*np.power(self.distance,self.pl), axis=2))
self.Psi = np.triu(newPsi,1)
self.Psi = self.Psi + self.Psi.T + np.mat(eye(self.n))+np.multiply(np.mat(eye(self.n)),np.spacing(1))
self.U = np.linalg.cholesky(self.Psi)
self.U = self.U.T
def regupdatePsi(self):
self.Psi = np.zeros((self.n,self.n), dtype=np.float)
self.one = np.ones(self.n)
for i in xrange(self.n):
for j in xrange(i+1,self.n):
self.Psi[i,j]=np.exp(-np.sum(self.theta*np.power(np.abs((self.X[i]-self.X[j])),self.pl)))
self.Psi = self.Psi + self.Psi.T + eye(self.n) + eye(self.n) * (self.Lambda)
self.U = np.linalg.cholesky(self.Psi)
self.U = np.matrix(self.U.T)
def test_advanced(self):
T = Transform()
# right multiplying matrix by a transform results in a matrix
self.assertEqual(type((np.eye(4) * T)), np.matrix)
# right multiplying transform by a matrix results in a transform
self.assertEqual(type(T * np.eye(4)), Transform)
# right multiplying transform by a non 4x4 matrix results in a matrix
p = np.matrix('1;3;2')
print type(T * p)
self.assertEqual(type(T * p), type(p))
def _compute_Apc(self, q, com_est):
Mlf, Mrf = self.robot.get_Mlf_Mrf(q, False)
pyl, pzl = Mlf.translation[1:].A1
pyr, pzr = Mrf.translation[1:].A1
cy, cz = com_est.A1
A_pc = matlib.zeros((3, 4))
A_pc[:2, :2] = matlib.eye(2)
A_pc[:2, 2:] = matlib.eye(2)
A_pc[2, :] = np.matrix([-(pzl - cz), pyl - cy, -(pzr - cz), pyr - cy])
b_g = np.vstack((self.m * self.g, matlib.zeros(1)))
return A_pc, b_g
def getMfeature(self):
mata = npmatrix.empty((3, 4)) # random data
mata = npmatrix.zeros((3, 4)) # zeros
mata = npmatrix.ones((3, 4)) # one
mata = npmatrix.eye(3) # one along diagonal
mata = npmatrix.eye(3, 5) # one along diagonal
mata = npmatrix.identity(3) # identity square matrix
mata = npmatrix.rand(3, 7) # rand data
mata = npmatrix.ones((3, 1)) # one
print(mata)
print(mata.shape)
print(mata.dtype)
def analyze_gain_tuning_tsid_rigid(conf):
nc = conf.nc
(H, A, B) = compute_integrator_dynamics(matlib.zeros((nc, 4 * nc)))
In = matlib.eye(nc)
A[nc:, :] = 0.0
B[nc:2 * nc, :] = In
B[3 * nc:, :] = 0.0
P = matlib.eye(5 * nc)
def compute_system_matrices(gains):
K = np.hstack([gains[0] * In, gains[1] * In, 0.0 * In, 0.0 * In])
return A, B, K
analyze_results(conf, compute_system_matrices, P)
def compute_double_integral_x_T(A,
a,
x0,
T,
dt=None,
compute_also_integral=False,
invertible_A=True):
if (dt is not None):
N = int(T / dt)
int2_x = matlib.zeros_like(x0)
for i in range(1, N):
int_x = compute_integral_x_T(A, a, x0, i * dt)
int2_x += dt * int_x
return int2_x
if (invertible_A):
e_TA = expm(T * A)
Ainv_a = solve(A, a)
Ainv_x0_plus_Ainv_a = solve(A, x0 + Ainv_a)
Ainv2_x0_plus_Ainv_a = solve(A, Ainv_x0_plus_Ainv_a)
I = matlib.eye(A.shape[0])
int2_x = (
e_TA - I
) * Ainv2_x0_plus_Ainv_a - T * Ainv_x0_plus_Ainv_a - 0.5 * T * T * Ainv_a
if compute_also_integral:
int_x = (e_TA - I) * Ainv_x0_plus_Ainv_a - T * Ainv_a
return int_x, int2_x
return int2_x
n = A.shape[0]
C = matlib.zeros((n + 3, n + 3))
C[0:n, 0:n] = A
C[0:n, n] = a
C[0:n, n + 1] = x0
C[n:n + 2, n + 1:] = matlib.eye(2)
z0 = matlib.zeros((n + 3, 1))
z0[-1, 0] = 1.0
e_TC = expm(T * C)
z = e_TC * z0
int2_x = z[:n, 0]
# print("A\n", A)
# print("a\n", a.T)
# print("x0\n", x0.T)
# print("C\n", C)
# print("z0\n", z0.T)
# print("z\n", z.T)
return int2_x
def __init__(self,*args, **kwargs):
super(Gyroscope, self).__init__()
self.properties = dict()
if len(args)==1:
arg = args[0]
if type(arg)==dict:
for key, value in arg.items():
setattr(self, key, value)
elif type(arg) == tuple or type(arg) == list:
self.yaw = arg[0]
self.pitch = arg[1]
self.roll = arg[2]
else: #np vector
self.yaw = arg.item(0)
self.pitch = arg.item(1)
self.roll = arg.item(2)
elif len(args)==3:
self.yaw = args[0]
self.pitch = args[1]
self.roll = args[2]
for key, value in kwargs.items():
setattr(self, key, value)
self.Covariance = kwargs.get('Covariance', 0.4 * matlib.eye(3) )
def compute_quadratic_state_integral_ALDS(H, x0, T, dt=None):
''' Assuming the state x(t) evolves in time according to a linear dynamic:
dx(t)/dt = H * x(t)
Compute the following integral:
int_{0}^{T} x(t)^T * x(t) dt
'''
if (dt is None):
w, V = eig(H) # H = V*matlib.diagflat(w)*V^{-1}
print "Eigenvalues H:", np.sort_complex(w).T
Lambda_inv = matlib.diagflat(1.0 / w)
e_2T_Lambda = matlib.diagflat(np.exp(2 * T * w))
int_e_2T_Lambda = 0.5 * Lambda_inv * (e_2T_Lambda - matlib.eye(n))
# V_inv = np.linalg.inv(V)
# cost = x0.T*(V_inv.T*(int_e_2T_Lambda*(V_inv*x0)))
V_inv_x0 = np.linalg.solve(V, x0)
cost = V_inv_x0.T * int_e_2T_Lambda * V_inv_x0
return cost[0, 0]
N = int(T / dt)
x = simulate_ALDS(H, x0, dt, N)
cost = 0.0
not_finite_warning_printed = False
for i in range(N):
if (np.all(np.isfinite(x[:, i]))):
cost += dt * (x[:, i].T * x[:, i])[0, 0]
elif (not not_finite_warning_printed):
print 'WARNING: x is not finite at time step %d' % (i) #, x[:,i].T
not_finite_warning_printed = True
return cost
def compute_integrator_dynamics(K):
''' Compute the matrices associated to an n-th order continuous time integrator.
The form of the dynamics is:
dx = A*x + B*u
The control law is a linear state feedback:
u = -K*x
Input parameters:
K : n-dimensional vector of feedback gains
Returns a tuple containing the following elements:
H: closed-loop dynamics matrix (A-B*K)
A: state transition matrix
B: control input matrix
'''
m = K.shape[0] # size of pos vector
n = K.shape[1] / m # integrator order
H = matlib.zeros((m * n, m * n))
A = matlib.zeros((m * n, m * n))
B = matlib.zeros((m * n, m))
H[-m:, :] = -K
I = matlib.eye(m)
B[-m:, :] = I
for i in range(n - 1):
H[m * i:m * (i + 1), m * (i + 1):m * (i + 2)] = I
A[m * i:m * (i + 1), m * (i + 1):m * (i + 2)] = I
return (H, A, B)
def __init__(self,nextState,measurement,uncertainty,state,covariance): self.F=nextState self.H=measurement self.R=uncertainty self.state=state self.covariance=covariance self.I=eye(F.shape[1])
def __init__(self, R=npm.eye(3), t=npm.zeros(3)):
"""
Pose is defined as p_cw (pose of the world wrt camera)
"""
p = RigidTransform.from_Rt(R, t)
RigidTransform.__init__(self, xyzw=p.quat.to_xyzw(), tvec=p.tvec)
self.__cached_inverse = None
def MarginalDistributionFromDMRAP(H):
"""
Returns the matrix geometrically distributed marginal
distribution of a discrete marked rational arrival
process.
Parameters
----------
H : list/cell of matrices of shape(M,M), length(N)
The H0...HN matrices of the DMRAP
precision : double, optional
Numerical precision for checking if the input is valid.
The default value is 1e-14
Returns
-------
alpha : matrix, shape (1,M)
The initial vector of the matrix geometrically
distributed marginal distribution
A : matrix, shape (M,M)
The matrix parameter of the matrix geometrically
distributed marginal distribution
"""
if butools.checkInput and not CheckDMRAPRepresentation(H):
raise Exception(
"MarginalDistributionFromDMRAP: Input is not a valid DMRAP representation!"
)
return (DRPSolve(
la.inv(ml.eye(H[0].shape[0]) - H[0]) * SumMatrixList(H[1:])), H[0])
def DRPSolve (P):
"""
Computes the stationary solution of a discrete time
Markov chain.
Parameters
----------
P : matrix, shape (M,M)
The matrix parameter of the rational process
Returns
-------
pi : row vector, shape (1,M)
The vector that satisfies
`\pi\, P = \pi, \sum_i \pi_i=1`
Notes
-----
Discrete time rational processes are like discrete time
Markov chains, but the P matrix does not have to pass
the :func:`CheckProbMatrix` test (but the rowsums still
have to be ones).
"""
if butools.checkInput and np.any(np.sum(P,1)-1.0>butools.checkPrecision):
raise Exception("DRPSolve: The matrix has a rowsum which isn't 1!")
if not isinstance(P,np.ndarray):
P = np.array(P)
return CRPSolve(P-ml.eye(P.shape[0]))
def __init__(self, **kwargs):
super(BasicProcess, self).__init__( **kwargs )
self.properties.update( stateList = kwargs.get('stateList', self.stateList) ) #List from property to index in X state
self.properties.update(
X = kwargs.get('X', matlib.zeros( [len(self), 1] )),
ProcessJacobian = kwargs.get('ProcessJacobian', matlib.eye( len(self) ) ),
Covariance = kwargs.get('Covariance', 0.3 * matlib.eye( len(self) ) ),
Ts = kwargs.get('Ts', 0.0),
position = kwargs.get('position', SixDofObject()),
quaternion = kwargs.get('quaternion', Quaternion()),
velocity = kwargs.get('velocity', SixDofObject()),
acceleration = kwargs.get('acceleration', SixDofObject())
)
self.time = rospy.get_time()
"""
def MarginalDistributionFromDRAP(H0, H1):
"""
Returns the matrix geometrically distributed marginal
distribution of a discrete rational arrival process.
Parameters
----------
H0 : matrix, shape (M,M)
The H0 matrix of the discrete rational arrival process
H1 : matrix, shape (M,M)
The H1 matrix of the discrete rational arrival process
precision : double, optional
Numerical precision for checking if the input is valid.
The default value is 1e-14
Returns
-------
alpha : matrix, shape (1,M)
The initial vector of the matrix geometrically
distributed marginal distribution
A : matrix, shape (M,M)
The matrix parameter of the matrix geometrically
distributed marginal distribution
"""
if butools.checkInput and not CheckDRAPRepresentation(H0, H1):
raise Exception(
"MarginalDistributionFromDRAP: Input is not a valid DRAP representation!"
)
return (DRPSolve(la.inv(ml.eye(H0.shape[0]) - H0) * H1), H0)
def __init__(self, **kwargs):
super(GPS, self).__init__()
self.latitude = kwargs.get('latitude', 0.0)
self.longitude = kwargs.get('longitude', 0.0)
self.altitude = kwargs.get('altitude', 0.0)
easting, northing, number, letter = utm.from_latlon( self.latitude, self.longitude )
self.easting = easting
self.northing = northing
self.zero = dict(
latitude = 0.0,
longitude = 0.0,
altitude = 0.0,
easting = 0.0,
northing = 0.0,
x = 0.0,
y = 0.0,
z = 0.0,
yaw = 0.0,)
#self.set_enu()
self.calibrated = False
self.valid = False
self.Covariance = kwargs.get('Covariance', matlib.eye(3) )
def __init__(self, *args, **kwargs):
super(Accelerometer, self).__init__()
self.properties = dict()
if len(args)==1:
arg = args[0]
if type(arg)==dict:
for key, value in arg.items():
setattr(self, key, value)
elif type(arg) == tuple or type(arg) == list:
self.x = arg[0]
self.y = arg[1]
self.z = arg[2]
else: #np vector
self.x = arg.item(0)
self.y = arg.item(1)
self.z = arg.item(2)
elif len(args)==3:
self.x = args[0]
self.y = args[1]
self.z = args[2]
for key, value in kwargs.items():
setattr(self, key, value)
self.g = sqrt( self.x ** 2 + self.y ** 2 + self.z ** 2 )
self.Covariance = kwargs.get('Covariance', 0.4 * matlib.eye(3) )
def FluidStationaryDistr (mass0, ini, K, clo, x):
"""
Returns the stationary distribution of a Markovian
fluid model at the given points.
Parameters
----------
mass0 : matrix, shape (1,Np+Nm)
The stationary probability vector of zero level
ini : matrix, shape (1,Np)
The initial vector of the stationary density
K : matrix, shape (Np,Np)
The matrix parameter of the stationary density
clo : matrix, shape (Np,Np+Nm)
The closing matrix of the stationary density
x : vector, length (K)
The distribution function is computed at these
points.
Returns
-------
pi : matrix, shape (K,Nm+Np)
The ith row of pi is the probability that the fluid
level is less than or equal to x(i), while being in
different states of the background process.
"""
m = clo.shape[1]
y = ml.empty((len(x),m))
closing = -K.I*clo
for i in range(len(x)):
y[i,:] = mass0 + ini*(ml.eye(K.shape[0])-la.expm(K*x[i]))*closing
return y
def compute_integral_x_T(A, a, x0, T, dt=None, invertible_A=True):
if (dt is not None):
N = int(T / dt)
int_x = dt * x0
for i in range(1, N):
x = compute_x_T(A, a, x0, i * dt)
int_x += dt * x
return int_x
if (invertible_A):
e_TA = expm(T * A)
Ainv_a = solve(A, a)
Ainv_x0_plus_Ainv_a = solve(A, x0 + Ainv_a)
I = matlib.eye(A.shape[0])
return (e_TA - I) * Ainv_x0_plus_Ainv_a - T * Ainv_a
n = A.shape[0]
C = matlib.zeros((n + 2, n + 2))
C[0:n, 0:n] = A
C[0:n, n] = a
C[0:n, n + 1] = x0
C[n:n + 1, n + 1:] = 1.0
z0 = matlib.zeros((n + 2, 1))
z0[-1, 0] = 1.0
e_TC = expm(T * C)
z = e_TC * z0
int_x = z[:n, 0]
return int_x
def vecm2varcoeffs(gammas, maxlag, alpha, beta):
'''
Converts Vecm coeffs to levels VAR representation.
Gammas need to be coeffs in shape #endo x (maxlag-1)*#endo,
such that contemp_diff = alpha*ect + Gammas * lagged_diffs
is okay when contemp_diff is #endo x 1.
We expect matrix input!
'''
if alpha.shape != beta.shape: # hope this computes for tuples
raise ValueError, 'alpha and beta must have equal dim'
N_y = alpha.shape[0]
if beta.shape[0] != N_y:
raise ValueError, "alpha or beta dim doesn't match"
if gammas.shape[0] != N_y:
raise ValueError, "alpha or gammas dim doesn't match"
if gammas.shape[1] != (maxlag - 1) * N_y:
raise ValueError, "maxlag or gammas dim doesn't match"
# starting point first lag:
levelscoeffs = eye(N_y) + alpha * beta.T + gammas[:, :N_y]
# intermediate lags:
for lag in range(1, maxlag - 1):
levelscoeffs = c_[ levelscoeffs, gammas[:, N_y*lag : N_y*(lag+1)] - \
gammas[:, N_y*(lag-1) : N_y*lag ] ]
# last diff-lag, now this should be N_y x maxlags*N_y:
return c_[levelscoeffs, -gammas[:, -N_y:]]
def DRPSolve(P):
"""
Computes the stationary solution of a discrete time
Markov chain.
Parameters
----------
P : matrix, shape (M,M)
The matrix parameter of the rational process
Returns
-------
pi : row vector, shape (1,M)
The vector that satisfies
`\pi\, P = \pi, \sum_i \pi_i=1`
Notes
-----
Discrete time rational processes are like discrete time
Markov chains, but the P matrix does not have to pass
the :func:`CheckProbMatrix` test (but the rowsums still
have to be ones).
"""
if butools.checkInput and np.any(
np.sum(P, 1) - 1.0 > butools.checkPrecision):
raise Exception("DRPSolve: The matrix has a rowsum which isn't 1!")
if not isinstance(P, np.ndarray):
P = np.array(P)
return CRPSolve(P - ml.eye(P.shape[0]))
def permutation_matrix(dim):
'''
Generate haar permuation matrix.
'''
I = M.eye(dim, dim)
return M.concatenate((I[::2][:],
I[1::2][:]))
def MarginalDistributionFromDMRAP (H):
"""
Returns the matrix geometrically distributed marginal
distribution of a discrete marked rational arrival
process.
Parameters
----------
H : list/cell of matrices of shape(M,M), length(N)
The H0...HN matrices of the DMRAP
precision : double, optional
Numerical precision for checking if the input is valid.
The default value is 1e-14
Returns
-------
alpha : matrix, shape (1,M)
The initial vector of the matrix geometrically
distributed marginal distribution
A : matrix, shape (M,M)
The matrix parameter of the matrix geometrically
distributed marginal distribution
"""
if butools.checkInput and not CheckDMRAPRepresentation (H):
raise Exception("MarginalDistributionFromDMRAP: Input is not a valid DMRAP representation!")
return (DRPSolve(la.inv(ml.eye(H[0].shape[0])-H[0])*SumMatrixList(H[1:])), H[0])
def eig_ortho(Kc, Beta):
"""Takes Beta, a matrix with coefficients for eigenvectors of two datasets. This function returns Omega, which has coefficients for the 'combined' orthonormal eigenvector set.
Specifically, Beta[:,0:r] represents the coefficients for an orthonormal set of eigenvectors, and Beta[:,r:2r] represents the coefficients for another orthonormal set of eigenvectors. Omega has the coefficients for the combined orthonormal basis
Omega[:,0:r] = Beta[:,0:r] since those eigenvectors were already orthonormal. Omega[:,r:2*r] is the second set of eigenvectors, orthogonalized relative to the first set"""
(n, rr) = Beta.shape
# rr literally stands for '2r'
(n_, n__) = Kc.shape
assert n == n_ == n__
Gamma = mat.eye(rr)
r = rr / 2
for i in xrange(
r,
rr): # Skips the first r columns since they're already orthonormal
for j in xrange(
i
): # Make sure the vector is orthogonal to each previous vector
Omega = Beta * Gamma
g = float(Omega.T[i, :] * Kc * Omega[:, j])
Gamma[:, i] -= g * Gamma[:, j]
Omega = Beta * Gamma
nrm = float(Omega.T[i, :] * Kc * Omega[:, i])
assert nrm > 0
nrm **= .5
Gamma[:, i] /= nrm
return Beta * Gamma
def nk_bhatta(X1, X2, eta):
# Returns the non-kernelized Bhattacharrya
#I.e. fits normal distributions in input space and calculates Bhattacharrya overlap between them
(n1, d1) = X1.shape
(n2, d ) = X2.shape
assert d1 == d
mu1 = mat.sum(X1,0) / n1
mu2 = mat.sum(X2,0) / n2
X1c = X1 - mat.tile(mu1, (n1,1))
X2c = X2 - mat.tile(mu2, (n2,1))
Eta = mat.eye(d) * eta
S1 = X1c.T * X1c / n1 + Eta
S2 = X2c.T * X2c / n2 + Eta
mu3 = .5 * (S1.I * mu1.T + S2.I * mu2.T).T
S3 = 2 * (S1.I + S2.I).I
d1 = la.det(S1) ** -.25
d2 = la.det(S2) ** -.25
d3 = la.det(S3) ** .5
dterm = d1 * d2 * d3
e1 = -.25 * mu1 * S1.I * mu1.T
e2 = -.25 * mu2 * S2.I * mu2.T
e3 = .5 * mu3 * S3 * mu3.T
eterm = math.exp(e1 + e2 + e3)
return float(dterm * eterm)
def vecm2varcoeffs(gammas, maxlag, alpha, beta):
'''
Converts Vecm coeffs to levels VAR representation.
Gammas need to be coeffs in shape #endo x (maxlag-1)*#endo,
such that contemp_diff = alpha*ect + Gammas * lagged_diffs
is okay when contemp_diff is #endo x 1.
We expect matrix input!
'''
if alpha.shape != beta.shape: # hope this computes for tuples
raise ValueError, 'alpha and beta must have equal dim'
N_y = alpha.shape[0]
if beta.shape[0] != N_y:
raise ValueError, "alpha or beta dim doesn't match"
if gammas.shape[0] != N_y:
raise ValueError, "alpha or gammas dim doesn't match"
if gammas.shape[1] != (maxlag-1)*N_y:
raise ValueError, "maxlag or gammas dim doesn't match"
# starting point first lag:
levelscoeffs = eye(N_y) + alpha * beta.T + gammas[ : , :N_y ]
# intermediate lags:
for lag in range(1, maxlag-1):
levelscoeffs = c_[ levelscoeffs, gammas[:, N_y*lag : N_y*(lag+1)] - \
gammas[:, N_y*(lag-1) : N_y*lag ] ]
# last diff-lag, now this should be N_y x maxlags*N_y:
return c_[ levelscoeffs, -gammas[:, -N_y: ] ]
def MarginalDistributionFromDRAP (H0, H1):
"""
Returns the matrix geometrically distributed marginal
distribution of a discrete rational arrival process.
Parameters
----------
H0 : matrix, shape (M,M)
The H0 matrix of the discrete rational arrival process
H1 : matrix, shape (M,M)
The H1 matrix of the discrete rational arrival process
precision : double, optional
Numerical precision for checking if the input is valid.
The default value is 1e-14
Returns
-------
alpha : matrix, shape (1,M)
The initial vector of the matrix geometrically
distributed marginal distribution
A : matrix, shape (M,M)
The matrix parameter of the matrix geometrically
distributed marginal distribution
"""
if butools.checkInput and not CheckDRAPRepresentation (H0, H1):
raise Exception("MarginalDistributionFromDRAP: Input is not a valid DRAP representation!")
return (DRPSolve(la.inv(ml.eye(H0.shape[0])-H0)*H1), H0)
def Jacobi(A, N, TOL = 0.000000000005, max_it = 100):
it = 1
while (it <= max_it):
for i in range(N - 1):
j = i + 1
for k in range(j):
P = matlib.eye(N, N)
if (A[j, k] != A[k, j]):
c = 2 * A[j, k] * sgn(A[j, j] - A[k, k])
b = math.fabs(A[j, j] - A[k, k])
P[j, j] = P[k, k] = math.sqrt((1 + b / math.sqrt(c*c + b*b)) / 2)
P[k, j] = c / (2 * P[j, j] + math.sqrt(c*c + b*b))
P[j, k] = - P[k, j]
else:
P[j, j] = P[k, k] = math.sqrt(1/2)
P[k, j] = P[j, j]
P[j, k] = - P[k, j]
A = np.transpose(P).dot(A).dot(P)
if (sum_off_diagonal(A, N) < TOL):
print('it = ' + str(it) + ', A = ')
for r in range(N):
row = ' '
for c in range(N):
row += (' ' + str(A[r, c])).rjust(28)
print(row)
return A
it += 1
print("Maximum number of iterations exceeded")
return
def sde_coupled(x0, y0, t0, b, sigma, N, h, delta=None):
if delta is None:
delta = h
# t = np.arange(T[0], T[1]+h/2, h)
t = np.arange(t0, t0 + N * h, h)
t.shape = (t.shape[0], 1)
# N = t.size
d = x0.shape[0]
dB = np.mat(np.random.normal(0, np.sqrt(h), (d, N)))
dB[:, 0] = 0
X = mat.zeros((d, N))
X[:, 0] = x0
Y = mat.zeros((d, N))
Y[:, 0] = y0
Id = mat.eye(d)
dBhat = mat.zeros((d, N))
for i in range(0, N - 1):
dBhat[:,
i + 1] = (Id - 2 * proj(sigma, X[:, i] - Y[:, i])) * dB[:, i + 1]
X[:, i + 1] = X[:, i] + h * b(t[i], X[:, i]) + sigma * dB[:, i + 1]
Y[:, i + 1] = Y[:, i] + h * b(t[i], Y[:, i]) + sigma * dBhat[:, i + 1]
if la.norm(X[:, i + 1] - Y[:, i + 1]) < delta:
Y[:, i + 1] = X[:, i + 1]
if d == 1 and (X[:, i] - Y[:, i]) * (X[:, i + 1] - Y[:, i + 1]) < 0:
Y[:, i + 1] = X[:, i + 1]
B = np.cumsum(dB, axis=1)
Bhat = np.cumsum(dBhat, axis=1)
return (X, Y, B, Bhat, t.T)
def hotCmntsForTest(self, postId, nCmnts = 5):
self.buildgraph(postId)
testsizes = [shape(self.prg)[0], 800, 600, 400, 200]
for size in testsizes:
self.prg = self.prg[0:size,0:size]
lil = lil_matrix(self.prg)
start = clock()
#eig = eigs(self.prg, k=1, return_eigenvectors =False)
eig = eigs(lil, return_eigenvectors =False, maxiter=10, tol=1E-5)
eig = eig[0].real
eig = 1/eig
eigTime = clock() - start
print 'test_size:',size, 'eigTime:',eigTime
one = ones(size)
m = eye(size) - eig*lil
start = clock()
cmnts_ranking = lu_solve((m, one), one)
solveTime = clock() - start
print 'test_size:',size, 'solveTime:',solveTime
def GM1StationaryDistr(B, R, K):
"""
Returns the stationary distribution of the G/M/1 type
Markov chain up to a given level K.
Parameters
----------
A : length(M) list of matrices of shape (N,N)
Matrix blocks of the G/M/1 type generator in the
regular part, from 0 to M-1.
B : length(M) list of matrices of shape (N,N)
Matrix blocks of the G/M/1 type generator at the
R : matrix, shape (N,N)
Matrix R of the G/M/1 type Markov chain
K : integer
The stationary distribution is returned up to
this level.
Returns
-------
pi : array, shape (1,(K+1)*N)
The stationary probability vector up to level K
"""
if not isinstance(B, np.ndarray):
B = np.vstack(B)
m = R.shape[0]
I = ml.eye(m)
temp = (I - R).I
if np.max(temp < -100 * butools.checkPrecision):
raise Exception(
"The spectral radius of R is not below 1: GM1 is not pos. recurrent"
)
maxb = B.shape[0] // m
BR = B[(maxb - 1) * m:, :]
for i in range(maxb - 1, 0, -1):
BR = R * BR + B[(i - 1) * m:i * m, :]
pix = DTMCSolve(BR)
pix = pix / np.sum(pix * temp)
pi = [pix]
sumpi = np.sum(pix)
numit = 1
while sumpi < 1.0 - 1e-10 and numit < 1 + K:
pix = pix * R
# compute pi_(numit+1)
numit += 1
sumpi += np.sum(pix)
pi.append(ml.matrix(pix))
if butools.verbose:
print("Accumulated mass after ", numit, " iterations: ", sumpi)
if butools.verbose and numit == K + 1:
print("Maximum Number of Components ", numit - 1, " reached")
return np.hstack(pi)
def MomentsFromMG (alpha, A, K=0):
"""
Returns the first K moments of a matrix geometric
distribution.
Parameters
----------
alpha : vector, shape (1,M)
The initial vector of the matrix-geometric distribution.
The sum of the entries of alpha is less or equal to 1.
A : matrix, shape (M,M)
The matrix parameter of the matrix-geometric
distribution.
K : int, optional
Number of moments to compute. If K=0, 2*M-1 moments are
computed. The default value is 0.
prec : double, optional
Numerical precision for checking the input.
The default value is 1e-14.
Returns
-------
moms : row vector of doubles
The vector of moments.
"""
if butools.checkInput and not CheckMGRepresentation (alpha, A):
raise Exception("MomentsFromMG: Input is not a valid MG representation!")
m = A.shape[0]
if K==0:
K = 2*m-1
Ai = la.inv(ml.eye(m)-A)
return MomsFromFactorialMoms([math.factorial(i)*np.sum(alpha*Ai**i*A**(i-1)) for i in range(1,K+1)])
def __init__(self,nextState,measurement,uncertainty,state,covariance): self.F=matrix(nextState) self.H=matrix(measurement) self.R=matrix(uncertainty) self.state=matrix(state) self.covariance=matrix(covariance) self.I=eye(F.shape[1])
def _update(self):
self.K = self.kernel(self.x)
if self.K.shape[0] == 0:
self.L = np.zeros((0, 0))
else:
sn2 = np.exp(2*self.kernel.lik)
self.K = self.K + sn2*np.eye(len(self))
self.L = scipy.linalg.cholesky(self.K, lower=True)
def LagkJointMomentsFromMRAP (H, K=0, L=1):
"""
Returns the lag-L joint moments of a marked rational
arrival process.
Parameters
----------
H : list/cell of matrices of shape(M,M), length(N)
The H0...HN matrices of the MRAP to check
K : int, optional
The dimension of the matrix of joint moments to
compute. If K=0, the MxM joint moments will be
computed. The default value is 0
L : int, optional
The lag at which the joint moments are computed.
The default value is 1
prec : double, optional
Numerical precision to check if the input is valid.
The default value is 1e-14
Returns
-------
Nm : list/cell of matrices of shape(K+1,K+1), length(N)
The matrices containing the lag-L joint moments,
starting from moment 0.
"""
if butools.checkInput and not CheckMRAPRepresentation (H):
raise Exception("LagkJointMomentsFromMRAP: Input is not a valid MRAP representation!")
if K==0:
K = H[0].shape[0]-1
M = len(H)-1
H0 = H[0]
sumH = ml.zeros(H[0].shape)
for i in range(M):
sumH += H[i+1]
H0i = la.inv(-H0)
P = H0i*sumH
pi = DRPSolve(P)
Pw = ml.eye(H0.shape[0])
H0p = [ml.matrix(Pw)]
for i in range(1,K+1):
Pw *= i*H0i
H0p.append(ml.matrix(Pw))
Pl = la.matrix_power (P, L-1)
Nm = []
for m in range(M):
Nmm = ml.zeros ((K+1,K+1))
for i in range(K+1):
for j in range(K+1):
Nmm[i,j] = np.sum (pi * H0p[i] * H0i * H[m+1] * Pl * H0p[j])
Nm.append(ml.matrix(Nmm))
return Nm
def GM1StationaryDistr (B, R, K):
"""
Returns the stationary distribution of the G/M/1 type
Markov chain up to a given level K.
Parameters
----------
A : length(M) list of matrices of shape (N,N)
Matrix blocks of the G/M/1 type generator in the
regular part, from 0 to M-1.
B : length(M) list of matrices of shape (N,N)
Matrix blocks of the G/M/1 type generator at the
R : matrix, shape (N,N)
Matrix R of the G/M/1 type Markov chain
K : integer
The stationary distribution is returned up to
this level.
Returns
-------
pi : array, shape (1,(K+1)*N)
The stationary probability vector up to level K
"""
if not isinstance(B,np.ndarray):
B = np.vstack(B)
m = R.shape[0]
I = ml.eye(m)
temp = (I-R).I
if np.max(temp<-100*butools.checkPrecision):
raise Exception("The spectral radius of R is not below 1: GM1 is not pos. recurrent")
maxb = B.shape[0]//m
BR = B[(maxb-1)*m:,:]
for i in range(maxb-1,0,-1):
BR = R * BR + B[(i-1)*m:i*m,:]
pix = DTMCSolve(BR)
pix = pix / np.sum(pix*temp)
pi = [pix]
sumpi = np.sum(pix)
numit = 1
while sumpi < 1.0-1e-10 and numit < 1+K:
pix = pix*R; # compute pi_(numit+1)
numit += 1
sumpi += np.sum(pix);
pi.append(ml.matrix(pix))
if butools.verbose:
print("Accumulated mass after ", numit, " iterations: ", sumpi)
if butools.verbose and numit == K+1:
print("Maximum Number of Components ", numit-1, " reached")
return np.hstack(pi)
def bhatta_matrix(self):
"""Returns a matrix of Bhattacharrya kernel evaluations between datasets"""
n = len(self.datasets)
BM = mat.eye(n)
for i in xrange(n):
for j in xrange(i):
BM[i,j] = self.bhatta(i,j)
BM[j,i] = BM[i,j]
return BM
def __init__(self, **kwargs):
super(GPS, self).__init__(**kwargs)
self.MeasurementCovariance= 0.4 * matlib.eye( len(self.measurementList) )
easting, northing, number, letter = utm.from_latlon( kwargs.get('latitude', 0) , kwargs.get('longitude', 0) )
self.gps_zero = dict( x = easting, y = northing, z = kwargs.get('altitude', utm.conversion.R), yaw = pi/2)
self.calibrated = False
