def stepSizeSteepestDescent(f,x0,g0):
"""
Description : Calculates the minimized step size for the steepest descent algorithm using Newton's 1D method
Parameters:
1. f : symbolic symbolic representation of the function f
2. x0 : list of independant variables for the function
3. g0 : gradient value at point x0 as a numpy matrix
Output:
gradient vector as numpy matrix
"""
phi,alpha = symbols('phi alpha')
Q = hessian(f,X)
if (poly(f).is_quadratic):
return float(g0*g0.transpose()/matrix2numpy(g0*Q*g0.transpose()))
else:
xStar = x0 - squeeze(asarray(alpha*g0))
def alphaValue(phi,a0): return phi.subs([(alpha,a0)])
a0 = 0.
phi = fValue(f,xStar)
while (True):
a = a0
a0 = a0 - alphaValue(diff(phi,alpha),a0)/alphaValue(diff(diff(phi,alpha),alpha),a0)
if (abs(a-a0)<epsilon): return a0
def test_compile(self):
f = self.generator.compile(tmp_dir='booger')
subs = {}
args = []
for argset in self.arguments:
vals = np.random.random(len(argset))
args.append(vals)
for arg, val in zip(argset, vals):
subs[arg] = val
for matrix in self.matrices:
nr, nc = matrix.shape
args.append(np.empty(nr * nc, dtype=float))
for output, expected in zip(f(*args), self.matrices):
try:
expected = sm.matrix2numpy(expected.subs(subs),
dtype=float).squeeze()
except TypeError:
# dtype kwarg in not supported in earlier SymPy versions
expected = np.asarray(sm.matrix2numpy(expected.subs(subs)),
dtype=float).squeeze()
np.testing.assert_allclose(output, expected)
def testMatrixSymmetries(self):
"""
X and P should have the same pattern as the distance matrix defined by
the lattice.
"""
precision = 20
polygon = self.calc.square_lattice(5)
X,P = self.calc.correlations(polygon, self.maple_link, precision)
# Round X and P down so we can see if elements are distinct or not.
X = sympy.matrix2numpy(X)
P = sympy.matrix2numpy(P)
X = X.astype('float')
P = P.astype('float')
# Get the pattern of the distance matrix.
D = spatial.distance.cdist(polygon,polygon)
# The pattern of the distance matrix
D_pat = sp.zeros(D.shape)
getSignatureMatrix(D_pat,sp.nditer(D),D.shape)
# Get the pattern of X and P.
X_pat = sp.zeros(X.shape)
P_pat = sp.zeros(P.shape)
getSignatureMatrix(X_pat,sp.nditer(X),X.shape)
getSignatureMatrix(P_pat,sp.nditer(P),P.shape)
# Check if patterns match.
eq_(False,(D_pat - X_pat).all())
eq_(False,(D_pat - P_pat).all())
def test_generate_with_specifieds(self):
# This model has three specifieds.
sys = models.n_link_pendulum_on_cart(2, True, True)
kin_diff_eqs = sys.eom_method.kindiffdict()
coord_derivs = sm.Matrix([kin_diff_eqs[c.diff()] for c in
sys.coordinates])
rhs = sys.eom_method.rhs()
common_args = [sys.coordinates, sys.speeds, sys.constants_symbols]
args_dct = {}
args_dct['full rhs'] = [rhs] + common_args
args_dct['full mass matrix'] = [sys.eom_method.forcing_full] + common_args
args_dct['min mass matrix'] = [sys.eom_method.forcing] + common_args
kwargs_dct = {}
kwargs_dct['full rhs'] = {}
kwargs_dct['full mass matrix'] = \
{'mass_matrix': sys.eom_method.mass_matrix_full}
kwargs_dct['min mass matrix'] = \
{'mass_matrix': sys.eom_method.mass_matrix,
'coordinate_derivatives': coord_derivs}
for Subclass in self.ode_function_subclasses:
for system_type, args in args_dct.items():
g = Subclass(*args, specifieds=sys.specifieds_symbols,
**kwargs_dct[system_type])
f = g.generate()
rand = np.random.random
x = rand(g.num_states)
r = rand(g.num_specifieds)
p = rand(g.num_constants)
subs = {}
for arr, syms in zip([x, r, p], [sys.states,
sys.specifieds_symbols,
sys.constants_symbols]):
for val, sym in zip(arr, syms):
subs[sym] = val
try:
expected = sm.matrix2numpy(rhs.subs(subs),
dtype=float).squeeze()
except TypeError:
# Earlier SymPy versions don't support the dtype kwarg.
expected = np.asarray(sm.matrix2numpy(rhs.subs(subs)),
dtype=float).squeeze()
xdot = f(x, 0.0, r, p)
np.testing.assert_allclose(xdot, expected)
def test_matrix2numpy_conversion():
a = Matrix([[1, 2, sin(x)], [x**2, x, Rational(1, 2)]])
b = array([[1, 2, sin(x)], [x**2, x, Rational(1, 2)]])
assert (matrix2numpy(a) == b).all()
assert matrix2numpy(a).dtype == numpy.dtype('object')
c = matrix2numpy(Matrix([[1, 2], [10, 20]]), dtype='int8')
d = matrix2numpy(Matrix([[1, 2], [10, 20]]), dtype='float64')
assert c.dtype == numpy.dtype('int8')
assert d.dtype == numpy.dtype('float64')
def solve(A,b):
if A.dtype==object:
Asym=sympy.Matrix(A)
bsym=sympy.Matrix(b)
# Silly sympy makes row vectors when we want a column vector:
if bsym.shape[0]==1:
bsym = bsym.T
if Asym.is_lower: # Take advantage of structure to solve quickly
xsym=sympy.zeros(b.shape)
if len(b.shape)==1:
xsym = Asym.lower_triangular_solve(bsym)
else:
for i in range(bsym.shape[1]): # Have to do 1 column at a time
xsym[:,i] = Asym.lower_triangular_solve(bsym[:,i])
else: # This is slower:
xsym = Asym.LUsolve(bsym)
xsym = sympy.matrix2numpy(xsym)
if len(b.shape)>1:
shape = [A.shape[1],b.shape[0]]
else:
shape = [A.shape[1]]
return np.reshape(xsym,shape)
else:
return np.linalg.solve(A,b)
def solve(A,b):
"""Solve Ax = b.
If A holds exact values, solve exactly using sympy.
Otherwise, solve in floating-point using numpy.
"""
if A.dtype==object:
Asym=sympy.Matrix(A)
bsym=sympy.Matrix(b)
# Silly sympy makes row vectors when we want a column vector:
if bsym.shape[0]==1:
bsym = bsym.T
if Asym.is_lower: # Take advantage of structure to solve quickly
xsym=sympy.zeros(b.shape)
xsym = Asym.lower_triangular_solve(bsym)
else: # This is slower:
xsym = Asym.LUsolve(bsym)
xsym = sympy.matrix2numpy(xsym)
if len(b.shape)>1:
shape = [A.shape[1],b.shape[0]]
else:
shape = [A.shape[1]]
return np.reshape(xsym,shape)
else:
return np.linalg.solve(A,b)
def mass2lamb(ang3,ang4,ang5,acc3,acc4,acc5):
#Mass matrix
(M,Minv)=mas_inv(ang3,ang4,ang5,acc3,acc4,acc5)
J=Jlm6.subs({q3:ang3,q4:ang4,q5:ang5})
J=sp.matrix2numpy(J)
aux=np.dot(J,Minv)
aux=np.dot(aux,J.T)
#Lambda matrix
u,s,v = np.linalg.svd(aux)
if s[abs(s)<.0025].shape[0] == 0:
# if we're not near a singularity
lmat = np.linalg.inv(aux)
else:
# in the case that the robot is near a singularity
for i in range(len(s)):
if s[i] < .0025: s[i] = 0
else: s[i] = 1.0/float(s[i])
lmat = np.dot(v, np.dot(np.diag(s), u.T))
#Lambda matrix inversion
u,s,v = np.linalg.svd(lmat)
if s[abs(s)<.00005].shape[0] == 0:
# if we're not near a singularity
lmat_inv = np.linalg.inv(lmat)
else:
# in the case that the robot is near a singularity
for i in range(len(s)):
if s[i] < .00005: s[i] = 0
else: s[i] = 1.0/float(s[i])
lmat_inv = np.dot(v, np.dot(np.diag(s), u.T))
return lmat,lmat_inv
def Inv_Dyn(st,F,xdd):
#mu=MU(A,Jlm6,st[0,0],st[1,0],st[2,0],st[3,0],st[4,0],st[5,0])
#ro=RO(st[0,0],st[1,0],st[2,0],st[3,0],st[4,0],st[5,0])
J=Jlm6.subs({q3:st[0,0],q4:st[1,0],q5:st[2,0],qd3:st[3,0],qd4:st[4,0],qd5:st[5,0]})
J=sp.matrix2numpy(J)
J=np.matrix(J)
jdc=ps.Jdc(st[0,0],st[1,0],st[2,0],st[3,0],st[4,0],st[5,0])
#print J.shape,F.shape
T0=np.dot(J.T,F.T)
#T0=np.matrix(T0)
#perturbation=ro#+mu
(L,Li)=lm.mass2lamb(st[0,0],st[1,0],st[2,0],st[3,0],st[4,0],st[5,0])
#F=np.dot(L,xdd.T)#+perturbation
#Null Space computing
I=np.eye(3)
I=np.matrix(I)
aux=np.dot(J.T,jdc.T)
Ns=np.dot((I-aux),T0)
T=np.dot(J.T,F.T)+Ns
print T
#print T
return T
def Jdc(mat,jac,ang3,ang4,ang5,acc3,acc4,acc5):
M_inv=lm.masinv(mat,ang3,ang4,ang5,acc3,acc4,acc5)
(L,Li)=lm.mass2lamb(mat,jac,ang3,ang4,ang5,acc3,acc4,acc5)
jac=M_inv*jac.T
#Value substitution into product jacrix
jac=jac.subs({q3:ang3,q4:ang4,q5:ang5,qd3:acc3,qd4:acc4,qd5:acc5})
#Conversion to numpy matrix
jac=sp.matrix2numpy(jac)
L=sp.matrix2numpy(L)
jdc=np.dot(jac,L)
return jdc
#jp=Jdc(A,Jlm6,0.2,0.2,0.2,1,1,1)
#print jp.shape
def rref(M, tol = 1e-5):
"""
Compute the reduced row echelon form
"""
# TODO: This is very very inefficient!
R = sp.matrix2numpy(sp.Matrix(A).rref(iszerofunc = lambda v : abs(v) < tau)[0],
dtype=np.double)
return row_normalize(R)
def test_matrix2numpy():
a = matrix2numpy(Matrix([[1, x**2], [3*sin(x), 0]]))
assert isinstance(a, ndarray)
assert a.shape == (2, 2)
assert a[0, 0] == 1
assert a[0, 1] == x**2
assert a[1, 0] == 3*sin(x)
assert a[1, 1] == 0
def masinv(mat,ang3,ang4,ang5,acc3,acc4,acc5):
#Value substitution into mass matrix
mat=mat.subs(q3,ang3)
mat=mat.subs(q4,ang4)
mat=mat.subs(q5,ang5)
mat=mat.subs(qd3,acc3)
mat=mat.subs(qd4,acc4)
mat=mat.subs(qd5,acc5)
#Conversion to numpy matrix
mat1=sp.matrix2numpy(mat)
#matrix inversion
num_inv=mat1**-1
return num_inv
def RO(ang3,ang4,ang5,acc3,acc4,acc5):
jdc=ps.Jdc(ang3,ang4,ang5,acc3,acc4,acc5)
#jdc_t=np.transpose(jdc)
g=gm.g.subs(q3,ang3)
g=g.subs(q4,ang4)
g=g.subs(q5,ang5)
g=g.subs(qd3,acc3)
g=g.subs(qd4,acc4)
g=g.subs(qd5,acc5)
#Conversion to numpy matrix
G=sp.matrix2numpy(g)
ro=np.dot(jdc.T,G)
return ro
def testPolygonScramble(self):
# Here, rearrange polygon randomly and repeatedly get results. See if come out different.
# Presets.
precision = 20
maple_link = maple.MapleLink("/opt/maple13/bin/maple -tu") #TODO: hopefully remove this maple dependency...too hardwirey.
L = 3
calc = Calculate()
polygon = calc.square_lattice(L)
X,P = calc.correlations(polygon, maple_link, precision)
# Get eigenvals
with sympy.mpmath.extraprec(500):
XP = X*P
XPnum = sympy.matrix2numpy(XP)
sqrt_eigs = sp.sqrt(linalg.eigvals(XPnum))
sqrt_eigs_orig = sqrt_eigs.real
# Scramble polygon.
sp.random.shuffle(polygon)
# Perform the integration again.
X,P = calc.correlations(polygon, maple_link, precision)
# Get eigenvals
with sympy.mpmath.extraprec(500):
XP = X*P
XPnum = sympy.matrix2numpy(XP)
sqrt_eigs = sp.sqrt(linalg.eigvals(XPnum))
sqrt_eigs_scrambled = sqrt_eigs.real
# Compare.
a = set(tuple([round(float(x),10) for x in sqrt_eigs_orig]))
b = set(tuple([round(float(x),10) for x in sqrt_eigs_scrambled]))
eq_(a,b)
def Jdc(ang3,ang4,ang5,acc3,acc4,acc5):
(M,Minv)=lm.mas_inv(ang3,ang4,ang5,acc3,acc4,acc5)
(L,Li)=lm.mass2lamb(ang3,ang4,ang5,acc3,acc4,acc5)
jac=Jlm6.subs({q3:ang3,q4:ang4,q5:ang5,qd3:acc3,qd4:acc4,qd5:acc5})
jdc=Minv*jac.T
#Conversion to numpy matrix
jdc=sp.matrix2numpy(jdc)
jdc=np.matrix(jdc)
jdc=np.dot(jdc,L)
return jdc
#jp=Jdc(A,Jlm6,0.2,0.2,0.2,1,1,1)
#print jp
#print jp.shape
def MU(ang3,ang4,ang5,acc3,acc4,acc5):
jdc=ps.Jdc(ang3,ang4,ang5,acc3,acc4,acc5)
#jdc_t=np.transpose(jdc)
b=ccm.b.subs(q3,ang3)
b=b.subs(q4,ang4)
b=b.subs(q5,ang5)
b=b.subs(qd3,acc3)
b=b.subs(qd4,acc4)
b=b.subs(qd5,acc5)
#Conversion to numpy matrix
B=sp.matrix2numpy(b)
mu=np.dot(jdc,B)
return mu
def jac_psi(mat,ang3,ang4,ang5,acc3,acc4,acc5):
#Value substitution into product matrix
mat=mat.subs({q3:ang3,q4:ang4,q5:ang5,qd3:acc3,qd4:acc4,qd5:acc5})
mult=mat*mat.T
#print mult
#Conversion to numpy matrix
mat1=sp.matrix2numpy(mult)
#Matrix inversion
Pinv=mat1**-1
#Jacobian pseudoinverse
J_psi=mat.T*Pinv
return J_psi
def mass2lamb(mat,jac,ang3,ang4,ang5,acc3,acc4,acc5):
num_inv=masinv(mat,ang3,ang4,ang5,acc3,acc4,acc5)
lmat=jac*num_inv*jac.T
#Value substitution into lambda matrix
lmat=lmat.subs(q3,ang3)
lmat=lmat.subs(q4,ang4)
lmat=lmat.subs(q5,ang5)
lmat=lmat.subs(qd3,acc3)
lmat=lmat.subs(qd4,acc4)
lmat=lmat.subs(qd5,acc5)
#Conversion to numpy matrix
mat2=sp.matrix2numpy(lmat)
lmat_inv=mat2**-1
return lmat,lmat_inv
def jac_psi(ang3,ang4,ang5,acc3,acc4,acc5):
#Value substitution into product matrix
mat=A.subs({q3:ang3,q4:ang4,q5:ang5,qd3:acc3,qd4:acc4,qd5:acc5})
mult=np.dot(mat,mat.T)
#Conversion to numpy matrix
mat1=sp.matrix2numpy(mult)
#Matrix inversion
Pinv=np.linalg.inv(mat1)
#Jacobian pseudoinverse
J_psi=np.dot(Pinv,mat1)
return J_psi
def mas_inv(ang3,ang4,ang5,acc3,acc4,acc5):
#Value substitution into mass matrix
M=A.subs({q3:ang3,q4:ang4,q5:ang5,qd3:acc3,qd4:acc4,qd5:acc5})
#Conversion to numpy matrix
M=sp.matrix2numpy(M)
#matrix inversion
u,s,v = np.linalg.svd(M)
if s[abs(s)<.0025].shape[0] == 0:
# if we're not near a singularity
Minv = np.linalg.inv(M)
else:
# in the case that the robot is near a singularity
for i in range(len(s)):
if s[i] < .0025: s[i] = 0
else: s[i] = 1.0/float(s[i])
Minv = np.dot(v, np.dot(np.diag(s), u.T))
return M,Minv
def conjugateGradient(f,x0,X):
"""
Description : Returns the minimizer using the Conjugate Gradient Algorithm (as a Direct Method)
Parameters:
1. f : symbolic representation of the function to be minimized
2. x0 : list of independant variables for the function
3. X : symbolic list of the dependant variables in the function f
Output:
Prints the minimizer of the function f to terminal
"""
def printFunction(f,x0):
"""
Description : Prints the minimizer of the function on the screen
Inputs:
f = symbolic representation of the function to be minimized
x0 = Initial guess of the minimizer
"""
print "\n+++++++++++++++++++++++++++++CONJUGATE GRADIENT METHOD+++++++++++++++++++++++++++++"
print "\nThe minimizer of the function\n\nf = %s \nis at \nx = %s" %(f,x0)
print "+++++++++++++++++++++++++++++++++++++++++++++++++=============+++++++++++++++++++++++\n"
return 0
gradF = [f.diff(x) for x in X]
g0 = gradValue(gradF,x0,X)
if(all(abs(i) < epsilon for i in squeeze(asarray(g0,'float')))): return printFunction(f,x0)
Q = matrix2numpy(hessian(f,X))
d0 = -g0
while(True):
alpha = -squeeze(asarray(g0*d0.transpose()))/squeeze(asarray(d0*Q*d0.transpose()))
x0 = [float(i) for i in (x0 + squeeze(asarray(alpha *d0)))]
g0 = gradValue(gradF,x0,X)
if(all(abs(i) < epsilon for i in squeeze(asarray(g0,'float')))): return printFunction(f,x0)
beta = float(squeeze(asarray(g0*Q*d0.transpose())/squeeze(asarray(d0*Q*d0.transpose()))))
d0 = -g0 +beta*d0
def M_func_adaptive(t0, t):
global started_TJM, count_TJM, P_TJM, Mf_TJM
t_mid = 0.5 * (t + t0)
# get first P matrix at t=t0
if not started_TJM:
Mf_TJM = Make_M_from_new_P(t_mid)
started_TJM = True
# check 100 times to see if P(t) need to be re-evaluated
count = np.floor(100 * (t_mid - Eq["t_start"]) /
((Eq["t_stop"] - Eq["t_start"])))
# do I need to check if we need to change P?
if count > count_TJM:
count_TJM = count
# check to see if we need to change P
P_old = P_TJM(t_mid)
P_new = sym.matrix2numpy(get_P(t_mid), dtype=np.complex128)
dP_norm = np.linalg.norm(P_new - P_old, ord='fro')
#print(" count = " + str(count) + ", dP_norm = ", dP_norm)
if dP_norm > 0:
#print(P_new)
#print(" making new P estimate, t_mid = ", t_mid)
Mf_TJM = Make_M_from_new_P(t_mid)
M_ = Mf_TJM(t0, t)
return M_
def assemble(self):
"""Assemble a QasmQobjInstruction"""
instruction = QasmQobjInstruction(name=self.name)
# Evaluate parameters
if self.params:
params = [
x.evalf() if hasattr(x, 'evalf') else x for x in self.params
]
params = [
sympy.matrix2numpy(x, dtype=complex) if isinstance(
x, sympy.Matrix) else x for x in params
]
instruction.params = params
# Add placeholder for qarg and carg params
if self.num_qubits:
instruction.qubits = list(range(self.num_qubits))
if self.num_clbits:
instruction.memory = list(range(self.num_clbits))
# Add condition parameters for assembler. This is needed to convert
# to a qobj conditional instruction at assemble time and after
# conversion will be deleted by the assembler.
if self.condition:
instruction._condition = self.condition
return instruction
def rref_(A, tau=1e-4):
R = sp.matrix2numpy(
sp.Matrix(A).rref(iszerofunc=lambda v: abs(v) < tau)[0],
dtype=np.double)
return row_normalize(R)
def rref_(A, tau = 1e-4):
R = sp.matrix2numpy(sp.Matrix(A).rref(iszerofunc = lambda v : abs(v) < tau)[0],
dtype=np.double)
return row_normalize(R)
def fairly_exact_pinv(a, precision=50):
floats = np.zeros(a.shape, dtype=object)
for index, val in np.ndenumerate(a):
floats[index] = sy.Float(val, precision)
sym_a = sy.Matrix(floats)
return sy.matrix2numpy(sym_a.pinv(), dtype=float)
# w for n_1 and kappa=10
omegaMatrix[0][2] = integralLesserKet11[0][0]
omegaMatrix[1][2] = integralLesserKet10[1][1] + integralLesserKet11[1][1]
# w for n_1 and kappa=11
omegaMatrix[0][3] = integralLesserKet11[0][0]
omegaMatrix[1][3] = integralLesserKet11[1][1]
densityTransform = Matrix(omegaMatrix - kappaMatrix)
nullSpace = densityTransform.nullspace()
nullSpaceList = []
i = 0
#Remember, the vectors P are chances; the normalisation we seek is sum P = 1
for nullVector in nullSpace:
nullSpaceList.append(matrix2numpy(nullVector)[:, 0])
nullSpaceList[i] /= np.sum(nullSpaceList[i])
i += 1
nullSpaceList = np.array(nullSpaceList)
nullSpaceShape = nullSpaceList.shape
if nullSpaceShape[0] == 0:
raise Exception(
"Abort: The single-particle occupation expectations do not converge."
)
n = np.dot(kappaMatrix, nullSpaceList[0])
#linearizer defaults to alphabetical sorting- don't use that
linearizer.r = sympy.Matrix(specified)
#use linearizer object to create symbolic linearization of system
#BE CAREFUL: arg <A_and_B=True> is VERY computationally expensive
#old strategy, works with zero operating points and nothing else
A, B = linearizer.linearize(op_point=[equilibrium_dict, parameter_dict], A_and_B=True)
# op_point = {theta1: 0, theta2: 0, theta3:0,omega1: 0, omega2: 0, omega3: 0}
#new attempt- not working
#A, B = linearizer.linearize(A_and_B=True, op_point=op_point)
# A, B, inp_vec = kane.linearize(A_and_B=True, op_point=op_point, new_method = True)
#convert to numpy
A = sympy.matrix2numpy(A, dtype=float)
B = sympy.matrix2numpy(B, dtype=float)
# pretty_print(A)
# pretty_print(B)
#C gets positon states for output y
C = np.zeros((6,6))
C[0,0] = 1
C[1,1] = 1
C[2,2] = 1
D = np.zeros((6,3))
SS = control.StateSpace(A,B,C,D)
print(SS)
#true if system is controllable
def main(inputFileName, IOFileName, outputFileName):
# Read image coordinates from file
fin = open(inputFileName)
lines = fin.readlines()
fin.close()
data = np.array(map(lambda l: map(float, l.split()), lines))
Lx, Ly, Rx, Ry = map(lambda a: a.flatten(), np.hsplit(data, 4))
# Read interior orientation information from file
fin = open(IOFileName)
data = map(lambda x: float(x), fin.readline().split())
fin.close()
f = data[0]
# Define initial values
XL0 = YL0 = OmegaL0 = PhiL0 = KappaL0 = 0
ZL0 = ZR0 = f
XR0 = abs(Lx - Rx).mean()
YR0 = OmegaR0 = PhiR0 = KappaR0 = 0
# Define initial coordinate for object points
# X0 = np.array(Lx)
# Y0 = np.array(Ly)
# Z0 = np.zeros(len(Lx))
X0 = XR0 * Lx / (Lx - Rx)
Y0 = XR0 * Ly / (Lx - Rx)
Z0 = f - ((XR0 * f) / (Lx - Rx))
# Define symbols
fs, x0s, y0s = symbols("f x0 y0")
XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs = symbols(
u"XL YL ZL ωL, φL, κL".encode(LANG))
XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs = symbols(
u"XR YR ZR ωR, φR, κR".encode(LANG))
xls, yls, xrs, yrs = symbols("xl yl xr yr")
XAs, YAs, ZAs = symbols("XA YA ZA")
# List observation equations
F1 = getEqns(
x0s, y0s, fs, XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs,
xls, yls, XAs, YAs, ZAs)
F2 = getEqns(
x0s, y0s, fs, XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs,
xrs, yrs, XAs, YAs, ZAs)
# Create lists for substitution of initial values and constants
var1 = np.array([x0s, y0s, fs, XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs,
xls, yls, XAs, YAs, ZAs])
var2 = np.array([x0s, y0s, fs, XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs,
xrs, yrs, XAs, YAs, ZAs])
# Define a symbol array for unknown parameters
l = np.array([OmegaRs, PhiRs, KappaRs, YRs, ZRs, XAs, YAs, ZAs])
# Compute coefficient matrix
JF1 = F1.jacobian(l)
JF2 = F2.jacobian(l)
# Create function objects for two parts of coefficient matrix and f matrix
B1 = lambdify(tuple(var1), JF1, modules='sympy')
B2 = lambdify(tuple(var2), JF2, modules='sympy')
F01 = lambdify(tuple(var1), -F1, modules='sympy')
F02 = lambdify(tuple(var2), -F2, modules='sympy')
X = np.ones(1) # Initial value for iteration
lc = 1 # Loop counter
while abs(X.sum()) > 10**-10:
B = np.matrix(np.zeros((4 * len(Lx), 5 + 3 * len(Lx))))
F0 = np.matrix(np.zeros((4 * len(Lx), 1)))
# Column index which is used to update values of B and f matrix
j = 0
for i in range(len(Lx)):
# Create lists of initial values and constants
val1 = np.array([0, 0, f, XL0, YL0, ZL0, OmegaL0, PhiL0, KappaL0,
Lx[i], Ly[i], X0[i], Y0[i], Z0[i]])
val2 = np.array([0, 0, f, XR0, YR0, ZR0, OmegaR0, PhiR0, KappaR0,
Rx[i], Ry[i], X0[i], Y0[i], Z0[i]])
# For coefficient matrix B
b1 = matrix2numpy(B1(*val1)).astype(np.double)
b2 = matrix2numpy(B2(*val2)).astype(np.double)
B[i*4:i*4+2, :5] = b1[:, :5]
B[i*4:i*4+2, 5+j*3:5+(j+1)*3] = b1[:, 5:]
B[i*4+2:i*4+4, :5] = b2[:, :5]
B[i*4+2:i*4+4, 5+j*3:5+(j+1)*3] = b2[:, 5:]
# For constant matrix f
f01 = matrix2numpy(F01(*val1)).astype(np.double)
f02 = matrix2numpy(F02(*val2)).astype(np.double)
F0[i*4:i*4+2, :5] = f01
F0[i*4+2:i*4+4, :5] = f02
j += 1
# Solve unknown parameters
N = np.matrix(B.T * B) # Compute normal matrix
t = B.T * F0 # Compute t matrix
X = N.I * t # Compute the unknown parameters
# Update initial values
OmegaR0 += X[0, 0]
PhiR0 += X[1, 0]
KappaR0 += X[2, 0]
YR0 += X[3, 0]
ZR0 += X[4, 0]
X0 += np.array(X[5::3, 0]).flatten()
Y0 += np.array(X[6::3, 0]).flatten()
Z0 += np.array(X[7::3, 0]).flatten()
# Output messages for iteration process
print "Iteration count: %d" % lc, u"|ΔX| = %.6f".encode(LANG) \
% abs(X.sum())
lc += 1 # Update Loop counter
# Compute residual vector
V = F0 - B * X
# Compute error of unit weight
s0 = ((V.T * V)[0, 0] / (B.shape[0] - B.shape[1]))**0.5
# Compute other informations
# Sigmall = np.eye(B.shape[0])
SigmaXX = s0**2 * N.I
# SigmaVV = s0**2 * (Sigmall - B * N.I * B.T)
# Sigmallhat = s0**2 * (Sigmall - SigmaVV)
param_std = np.sqrt(np.diag(SigmaXX))
pho_res = np.array(V).flatten()
# pho_res = np.sqrt(np.diag(SigmaVV))
# Output results
print "\nExterior orientation parameters:"
print ("%9s"+" %9s"*3) % ("Parameter", "Left pho", "Right pho", "SD right")
print "%-10s %8.4f %9.4f %9.4f" % (
"Omega(deg)", deg(OmegaL0), deg(OmegaR0), deg(param_std[0]))
print "%-10s %8.4f %9.4f %9.4f" % (
"Phi(deg)", deg(PhiL0), deg(PhiR0), deg(param_std[1]))
print "%-10s %8.4f %9.4f %9.4f" % (
"Kappa(deg)", deg(KappaL0), deg(KappaR0), deg(param_std[2]))
print "%-10s %8.4f %9.4f" % (
"XL", XL0, XR0)
print "%-10s %8.4f %9.4f %9.4f" % (
"YL", YL0, YR0, param_std[3])
print "%-10s %8.4f %9.4f %9.4f\n" % (
"ZL", ZL0, ZR0, param_std[4])
print "Object space coordinates:"
print ("%5s"+" %9s"*6) % ("Point", "X", "Y", "Z", "SD-X", "SD-Y", "SD-Z")
for i in range(len(X0)):
print ("%5s"+" %9.4f"*6) % (
("p%d" % i), X0[i], Y0[i], Z0[i],
param_std[3*i+5],
param_std[3*i+6],
param_std[3*i+7])
print "\nPhoto coordinate residuals:"
print ("%5s"+" %9s"*4) % ("Point", "xl-res", "yl-res", "xr-res", "yr-res")
for i in range(len(X0)):
print ("%5s"+" %9.4f"*4) % (
("p%d" % i), pho_res[2*i], pho_res[2*i+1],
pho_res[2*i+len(X0)*2], pho_res[2*i+len(X0)*2+1])
print ("\n%5s"+" %9.4f"*4+"\n") % (
"RMS",
np.sqrt((pho_res[0:len(X0)*2:2]**2).mean()),
np.sqrt((pho_res[1:len(X0)*2+1:2]**2).mean()),
np.sqrt((pho_res[len(X0)*2::2]**2).mean()),
np.sqrt((pho_res[len(X0)*2+1::2]**2).mean()))
print "Standard error of unit weight : %.4f" % s0
print "Degree of freedom: %d" % (B.shape[0] - B.shape[1])
# Write out results
fout = open(outputFileName, "w")
fout.write("%.8f %.8f %.8f\n" % (XL0, YL0, ZL0))
fout.write("%.8f %.8f %.8f\n" % (XR0, YR0, ZR0))
for i in range(len(X0)):
fout.write("%.8f %.8f %.8f\n" % (X0[i], Y0[i], Z0[i]))
fout.close()
return 0
import control as ct
import matplotlib.pyplot as plt
tf=10.0
nstep=50
Kp=0.1
Kd=0.1
errorx=[]
errory=[]
mov=[]
#Initial arm status
q0=np.matrix([0.1,0,0])
qd0=np.matrix([0,0,0])
xy0=dk.coord_q6.subs({q3:q0[0,0],q4:q0[0,1],q5:q0[0,2]})
xy0=sp.matrix2numpy(xy0)
xy0=np.matrix(xy0)
#Target position
q_f=np.matrix([-0.1,-0.1,0.1])
#xy_f=np.matrix([0.600,0.0170])
xy_f=dk.coord_q6.subs({q3:q_f[0,0],q4:q_f[0,1],q5:q_f[0,2]})
xy_f=sp.matrix2numpy(xy_f)
xy_f=np.matrix(xy_f.T)
xyd_f=np.matrix((0,0))
#Straight line trajectory
traj=line(xy0[0,0],xy0[1,0],xy_f[0,0],xy_f[0,1],tf,nstep)
x=traj[0]
y=traj[1]
def assemble_circuits(circuits,
run_config=None,
qobj_header=None,
qobj_id=None):
"""Assembles a list of circuits into a qobj which can be run on the backend.
Args:
circuits (list[QuantumCircuits] or QuantumCircuit): circuits to assemble
run_config (RunConfig): RunConfig object
qobj_header (QobjHeader): header to pass to the results
qobj_id (int): identifier for the generated qobj
Returns:
Qobj: the Qobj to be run on the backends
"""
qobj_header = qobj_header or QobjHeader()
run_config = run_config or RunConfig()
if isinstance(circuits, QuantumCircuit):
circuits = [circuits]
userconfig = QobjConfig(**run_config.to_dict())
experiments = []
max_n_qubits = 0
max_memory_slots = 0
for circuit in circuits:
# header stuff
n_qubits = 0
memory_slots = 0
qubit_labels = []
clbit_labels = []
qreg_sizes = []
creg_sizes = []
for qreg in circuit.qregs:
qreg_sizes.append([qreg.name, qreg.size])
for j in range(qreg.size):
qubit_labels.append([qreg.name, j])
n_qubits += qreg.size
for creg in circuit.cregs:
creg_sizes.append([creg.name, creg.size])
for j in range(creg.size):
clbit_labels.append([creg.name, j])
memory_slots += creg.size
# TODO: why do we need creq_sizes and qreg_sizes in header
# TODO: we need to rethink memory_slots as they are tied to classical bit
experimentheader = QobjExperimentHeader(qubit_labels=qubit_labels,
n_qubits=n_qubits,
qreg_sizes=qreg_sizes,
clbit_labels=clbit_labels,
memory_slots=memory_slots,
creg_sizes=creg_sizes,
name=circuit.name)
# TODO: why do we need n_qubits and memory_slots in both the header and the config
experimentconfig = QobjExperimentConfig(n_qubits=n_qubits,
memory_slots=memory_slots)
instructions = []
for opt in circuit.data:
current_instruction = QobjInstruction(name=opt.name)
if opt.qargs:
qubit_indices = [
qubit_labels.index([qubit[0].name, qubit[1]])
for qubit in opt.qargs
]
current_instruction.qubits = qubit_indices
if opt.cargs:
clbit_indices = [
clbit_labels.index([clbit[0].name, clbit[1]])
for clbit in opt.cargs
]
current_instruction.memory = clbit_indices
if opt.params:
params = list(map(lambda x: x.evalf(), opt.params))
params = [
sympy.matrix2numpy(x, dtype=complex) if isinstance(
x, sympy.Matrix) else x for x in params
]
if len(params) == 1 and isinstance(params[0], numpy.ndarray):
# TODO: Aer expects list of rows for unitary instruction params;
# change to matrix in Aer.
params = params[0]
current_instruction.params = params
# TODO (jay): I really dont like this for snapshot. I also think we should change
# type to snap_type
if opt.name == "snapshot":
current_instruction.label = str(opt.params[0])
current_instruction.type = str(opt.params[1])
if opt.control:
mask = 0
for clbit in clbit_labels:
if clbit[0] == opt.control[0].name:
mask |= (1 << clbit_labels.index(clbit))
current_instruction.conditional = QobjConditional(
mask="0x%X" % mask,
type='equals',
val="0x%X" % opt.control[1])
instructions.append(current_instruction)
experiments.append(
QobjExperiment(instructions=instructions,
header=experimentheader,
config=experimentconfig))
if n_qubits > max_n_qubits:
max_n_qubits = n_qubits
if memory_slots > max_memory_slots:
max_memory_slots = memory_slots
userconfig.memory_slots = max_memory_slots
userconfig.n_qubits = max_n_qubits
return Qobj(qobj_id=qobj_id or str(uuid.uuid4()),
config=userconfig,
experiments=experiments,
header=qobj_header,
type=QobjType.QASM.value)
elif theta_ == 0:
break
k_bi += 1
x_next = x0 + lk * dk # x k+1
print("f: {}, x: {}, d: {} lambda: {}, k: {}".format(f.subs({x: x0}).evalf(), x0, dk, lk, k))
if not (x_next[0] - x_next[1] < 50 and x_next[0] > 0 and x_next[1] > 0 and x_next[0] + x_next[1] < 100):
print("constraint!")
break
else:
x0 = x_next
k += 1
x_val = [sp.matrix2numpy(x) for x in x_val]
x1 = [abs(x[0] - x_true[0])[0] for x in x_val]
x2 = [abs(x[1] - x_true[1])[0] for x in x_val]
x__ = [i + j for i, j in zip(x1, x2)]
f_val = [abs(f_true - v) for v in f_val]
print(f_true)
# plot
fig = plt.figure((i + 1), figsize=(10, 10))
fig.suptitle("gamma = " + str(gamma))
x_fig = fig.add_subplot(211)
x_fig.set_title("||xk - x*|| - (" + str(x_[0]) + ", " + str(x_[1]) + ")")
x_fig.set_xlabel("iter")
x_fig.set_ylabel("x")
x_fig.plot(range(k + 1), x__, marker='.', c='b')
x_fig.set_xlim(0, 30)
def assemble_circuits(circuits,
qobj_id=None,
qobj_header=None,
run_config=None):
"""Assembles a list of circuits into a qobj which can be run on the backend.
Args:
circuits (list[QuantumCircuits]): circuit(s) to assemble
qobj_id (int): identifier for the generated qobj
qobj_header (QobjHeader): header to pass to the results
run_config (RunConfig): configuration of the runtime environment
Returns:
QasmQobj: the Qobj to be run on the backends
"""
qobj_config = QasmQobjConfig()
if run_config:
qobj_config = QasmQobjConfig(**run_config.to_dict())
# Pack everything into the Qobj
experiments = []
max_n_qubits = 0
max_memory_slots = 0
for circuit in circuits:
# header stuff
n_qubits = 0
memory_slots = 0
qubit_labels = []
clbit_labels = []
qreg_sizes = []
creg_sizes = []
for qreg in circuit.qregs:
qreg_sizes.append([qreg.name, qreg.size])
for j in range(qreg.size):
qubit_labels.append([qreg.name, j])
n_qubits += qreg.size
for creg in circuit.cregs:
creg_sizes.append([creg.name, creg.size])
for j in range(creg.size):
clbit_labels.append([creg.name, j])
memory_slots += creg.size
# TODO: why do we need creq_sizes and qreg_sizes in header
# TODO: we need to rethink memory_slots as they are tied to classical bit
experimentheader = QobjExperimentHeader(qubit_labels=qubit_labels,
n_qubits=n_qubits,
qreg_sizes=qreg_sizes,
clbit_labels=clbit_labels,
memory_slots=memory_slots,
creg_sizes=creg_sizes,
name=circuit.name)
# TODO: why do we need n_qubits and memory_slots in both the header and the config
experimentconfig = QasmQobjExperimentConfig(n_qubits=n_qubits,
memory_slots=memory_slots)
# Convert conditionals from QASM-style (creg ?= int) to qobj-style
# (register_bit ?= 1), by assuming device has unlimited register slots
# (supported only for simulators). Map all measures to a register matching
# their clbit_index, create a new register slot for every conditional gate
# and add a bfunc to map the creg=val mask onto the gating register bit.
is_conditional_experiment = any(op.control
for (op, qargs, cargs) in circuit.data)
max_conditional_idx = 0
instructions = []
for op_context in circuit.data:
op = op_context[0]
qargs = op_context[1]
cargs = op_context[2]
current_instruction = QasmQobjInstruction(name=op.name)
if qargs:
qubit_indices = [
qubit_labels.index([qubit[0].name, qubit[1]])
for qubit in qargs
]
current_instruction.qubits = qubit_indices
if cargs:
clbit_indices = [
clbit_labels.index([clbit[0].name, clbit[1]])
for clbit in cargs
]
current_instruction.memory = clbit_indices
# If the experiment has conditional instructions, assume every
# measurement result may be needed for a conditional gate.
if op.name == "measure" and is_conditional_experiment:
current_instruction.register = clbit_indices
if op.params:
# Evalute Sympy parameters
params = [
x.evalf() if hasattr(x, 'evalf') else x for x in op.params
]
params = [
sympy.matrix2numpy(x, dtype=complex) if isinstance(
x, sympy.Matrix) else x for x in params
]
current_instruction.params = params
# TODO: I really dont like this for snapshot. I also think we should change
# type to snap_type
if op.name == 'snapshot':
current_instruction.label = str(op.params[0])
current_instruction.snapshot_type = str(op.params[1])
if op.name == 'unitary':
if op._label:
current_instruction.label = op._label
if op.control:
# To convert to a qobj-style conditional, insert a bfunc prior
# to the conditional instruction to map the creg ?= val condition
# onto a gating register bit.
mask = 0
val = 0
for clbit in clbit_labels:
if clbit[0] == op.control[0].name:
mask |= (1 << clbit_labels.index(clbit))
val |= (((op.control[1] >> clbit[1]) & 1) <<
clbit_labels.index(clbit))
conditional_reg_idx = memory_slots + max_conditional_idx
conversion_bfunc = QasmQobjInstruction(
name='bfunc',
mask="0x%X" % mask,
relation='==',
val="0x%X" % val,
register=conditional_reg_idx)
instructions.append(conversion_bfunc)
current_instruction.conditional = conditional_reg_idx
max_conditional_idx += 1
instructions.append(current_instruction)
experiments.append(
QasmQobjExperiment(instructions=instructions,
header=experimentheader,
config=experimentconfig))
if n_qubits > max_n_qubits:
max_n_qubits = n_qubits
if memory_slots > max_memory_slots:
max_memory_slots = memory_slots
qobj_config.memory_slots = max_memory_slots
qobj_config.n_qubits = max_n_qubits
return QasmQobj(qobj_id=qobj_id,
config=qobj_config,
experiments=experiments,
header=qobj_header)
duration = 1000
ac = [1] * 100 + [0] * duration + [-1] * 100
ac = np.array(ac)
ac = ac * 0.5
hold = np.zeros((300, 2))
acceldata[:1200, 0] = acceldata[:1200, 0] + ac
duration = 1700
ac = [1] * 100 + [0] * duration + [-1] * 100
ac = np.array(ac)
ac = ac * 0.5
acceldata[1100:3000, 1] = acceldata[1100:3000, 1] + ac
angel = 10 / 180 * math.pi
rmat = sympy.matrix2numpy(rotate.evalf(subs={theta: angel}), dtype='float')
acceldata[1100:3000] = np.dot(acceldata[1100:3000], rmat)
duration = 1100
ac = [1] * 100 + [0] * duration + [-1] * 100
ac = np.array(ac)
ac = ac * 0.5
acceldata[2900:4200, 0] = acceldata[2900:4200, 0] - ac
duration = 400
ac = [1] * 100 + [0] * duration + [-1] * 100
ac = np.array(ac)
ac = ac * 0.5
acceldata[4200:4800, 1] = acceldata[4200:4800, 1] - ac
angel = 30 / 180 * math.pi
rmat = sympy.matrix2numpy(rotate.evalf(subs={theta: angel}), dtype='float')
def params(self, parameters):
self._params = []
for single_param in parameters:
# example: u2(pi/2, sin(pi/4))
if isinstance(single_param, (ParameterExpression)):
self._params.append(single_param)
# example: OpenQASM parsed instruction
elif isinstance(single_param, node.Node):
warnings.warn(
'Using qasm ast node as a circuit.Instruction '
'parameter is deprecated as of the 0.11.0, and '
'will be removed no earlier than 3 months after '
'that release date. You should convert the qasm '
'node to a supported type int, float, complex, '
'str, circuit.ParameterExpression, or ndarray '
'before setting Instruction.parameters',
DeprecationWarning,
stacklevel=3)
self._params.append(single_param.sym())
# example: u3(0.1, 0.2, 0.3)
elif isinstance(single_param, (int, float)):
self._params.append(single_param)
# example: Initialize([complex(0,1), complex(0,0)])
elif isinstance(single_param, complex):
self._params.append(single_param)
# example: snapshot('label')
elif isinstance(single_param, str):
self._params.append(Parameter(single_param))
# example: numpy.array([[1, 0], [0, 1]])
elif isinstance(single_param, numpy.ndarray):
self._params.append(single_param)
elif isinstance(single_param, numpy.number):
self._params.append(single_param.item())
elif 'sympy' in str(type(single_param)):
import sympy
if isinstance(single_param, sympy.Basic):
warnings.warn(
'Parameters of sympy.Basic is deprecated '
'as of the 0.11.0, and will be removed no '
'earlier than 3 months after that release '
'date. You should convert this to a '
'supported type prior to using it as a '
'a parameter.',
DeprecationWarning,
stacklevel=3)
self._params.append(single_param)
elif isinstance(single_param, sympy.Matrix):
warnings.warn(
'Parameters of sympy.Matrix is deprecated '
'as of the 0.11.0, and will be removed no '
'earlier than 3 months after that release '
'date. You should convert the sympy Matrix '
'to a numpy matrix with sympy.matrix2numpy '
'prior to using it as a parameter.',
DeprecationWarning,
stacklevel=3)
matrix = sympy.matrix2numpy(single_param, dtype=complex)
self._params.append(matrix)
elif isinstance(single_param, sympy.Expr):
warnings.warn(
'Parameters of sympy.Expr is deprecated '
'as of the 0.11.0, and will be removed no '
'earlier than 3 months after that release '
'date. You should convert the sympy Expr '
'to a supported type prior to using it as '
'a parameter.',
DeprecationWarning,
stacklevel=3)
self._params.append(single_param)
else:
raise CircuitError("invalid param type {0} in instruction "
"{1}".format(type(single_param),
self.name))
else:
raise CircuitError("invalid param type {0} in instruction "
"{1}".format(type(single_param), self.name))
def numerical_linear(self):
return (sy.matrix2numpy(self.A.xreplace(self.open_loop_par_map),
dtype=float),
sy.matrix2numpy(self.B.xreplace(self.open_loop_par_map),
dtype=float))
def main():
# Define image coordinate of conjugate points
Lx = np.array([-4.87, 89.296, 0.256, 90.328, -4.673, 88.591])
Ly = np.array([1.992, 2.706, 84.138, 83.854, -86.815, -85.269])
Rx = np.array([-97.920, -1.485, -90.906, -1.568, -100.064, -0.973])
Ry = np.array([-2.91, -1.836, 78.980, 79.482, -95.733, -94.312])
# Define interior orientation parameters
f = 152.113
# Define initial values
XL0 = YL0 = OmegaL0 = PhiL0 = KappaL0 = 0
ZL0 = ZR0 = f
XR0 = abs(Lx - Rx).mean()
YR0 = OmegaR0 = PhiR0 = KappaR0 = 0
# Define initial coordinate for object points
# X0 = np.array(Lx)
# Y0 = np.array(Ly)
# Z0 = np.zeros(len(Lx))
X0 = XR0 * Lx / (Lx - Rx)
Y0 = XR0 * Ly / (Lx - Rx)
Z0 = f - (XR0 * f / (Lx - Rx))
# Define symbols
fs, x0s, y0s = symbols("f x0 y0")
XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs = symbols(u"XL YL ZL ωL, φL, κL".encode(LANG))
XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs = symbols(u"XR YR ZR ωR, φR, κR".encode(LANG))
xls, yls, xrs, yrs = symbols("xl yl xr yr")
XAs, YAs, ZAs = symbols("XA YA ZA")
# List observation equations
F1 = getEqns(x0s, y0s, fs, XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs, xls, yls, XAs, YAs, ZAs)
F2 = getEqns(x0s, y0s, fs, XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs, xrs, yrs, XAs, YAs, ZAs)
# Create lists for substitution of initial values and constants
var1 = np.array([x0s, y0s, fs, XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs, xls, yls, XAs, YAs, ZAs])
var2 = np.array([x0s, y0s, fs, XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs, xrs, yrs, XAs, YAs, ZAs])
# Define a symbol array for unknown parameters
l = np.array([OmegaRs, PhiRs, KappaRs, YRs, ZRs, XAs, YAs, ZAs])
# Compute coefficient matrix
JF1 = F1.jacobian(l)
JF2 = F2.jacobian(l)
# Create function objects for two parts of coefficient matrix and f matrix
B1 = lambdify(tuple(var1), JF1, modules="sympy")
B2 = lambdify(tuple(var2), JF2, modules="sympy")
F01 = lambdify(tuple(var1), -F1, modules="sympy")
F02 = lambdify(tuple(var2), -F2, modules="sympy")
X = np.ones(1) # Initial value for iteration
lc = 1 # Loop counter
while abs(X.sum()) > 10 ** -12:
B = np.matrix(np.zeros((4 * len(Lx), 5 + 3 * len(Lx))))
F0 = np.matrix(np.zeros((4 * len(Lx), 1)))
# Column index which is used to update values of B and f matrix
j = 0
for i in range(len(Lx)):
# Create lists of initial values and constants
val1 = np.array([0, 0, f, XL0, YL0, ZL0, OmegaL0, PhiL0, KappaL0, Lx[i], Ly[i], X0[i], Y0[i], Z0[i]])
val2 = np.array([0, 0, f, XR0, YR0, ZR0, OmegaR0, PhiR0, KappaR0, Rx[i], Ry[i], X0[i], Y0[i], Z0[i]])
# For coefficient matrix B
b1 = matrix2numpy(B1(*val1)).astype(np.double)
b2 = matrix2numpy(B2(*val2)).astype(np.double)
B[i * 4 : i * 4 + 2, :5] = b1[:, :5]
B[i * 4 : i * 4 + 2, 5 + j * 3 : 5 + (j + 1) * 3] = b1[:, 5:]
B[i * 4 + 2 : i * 4 + 4, :5] = b2[:, :5]
B[i * 4 + 2 : i * 4 + 4, 5 + j * 3 : 5 + (j + 1) * 3] = b2[:, 5:]
# For constant matrix f
f01 = matrix2numpy(F01(*val1)).astype(np.double)
f02 = matrix2numpy(F02(*val2)).astype(np.double)
F0[i * 4 : i * 4 + 2, :5] = f01
F0[i * 4 + 2 : i * 4 + 4, :5] = f02
j += 1
# Solve unknown parameters
N = B.T * B # Compute normal matrix
t = B.T * F0 # Compute t matrix
X = N.I * t # Compute the unknown parameters
# Update initial values
OmegaR0 += X[0, 0]
PhiR0 += X[1, 0]
KappaR0 += X[2, 0]
YR0 += X[3, 0]
ZR0 += X[4, 0]
X0 += np.array(X[5::3, 0]).flatten()
Y0 += np.array(X[6::3, 0]).flatten()
Z0 += np.array(X[7::3, 0]).flatten()
# Output messages for iteration process
print "Iteration count: %d" % lc, u"|ΔX| = %.6f".encode(LANG) % abs(X.sum())
lc += 1 # Update Loop counter
# Compute residual vector
V = F0 - B * X
# Compute error of unit weight
s0 = ((V.T * V)[0, 0] / (B.shape[0] - B.shape[1])) ** 0.5
# Compute other informations
# Sigmall = np.eye(B.shape[0])
SigmaXX = s0 ** 2 * N.I
# SigmaVV = s0**2 * (Sigmall - B * N.I * B.T)
# Sigmallhat = s0**2 * (Sigmall - SigmaVV)
param_std = np.sqrt(np.diag(SigmaXX))
pho_res = np.array(V).flatten()
# pho_res = np.sqrt(np.diag(SigmaVV))
# Output results
print "\nExterior orientation parameters:"
print ("%9s" + " %9s" * 3) % ("Parameter", "Left pho", "Right pho", "SD right")
print "%-10s %8.4f %9.4f %9.4f" % ("Omega(deg)", deg(OmegaL0), deg(OmegaR0), deg(param_std[0]))
print "%-10s %8.4f %9.4f %9.4f" % ("Phi(deg)", deg(PhiL0), deg(PhiR0), deg(param_std[1]))
print "%-10s %8.4f %9.4f %9.4f" % ("Kappa(deg)", deg(KappaL0), deg(KappaR0), deg(param_std[2]))
print "%-10s %8.4f %9.4f" % ("XL", XL0, XR0)
print "%-10s %8.4f %9.4f %9.4f" % ("YL", YL0, YR0, param_std[3])
print "%-10s %8.4f %9.4f %9.4f\n" % ("ZL", ZL0, ZR0, param_std[4])
print "Object space coordinates:"
print ("%5s" + " %9s" * 6) % ("Point", "X", "Y", "Z", "SD-X", "SD-Y", "SD-Z")
for i in range(len(X0)):
print ("%5s" + " %9.4f" * 6) % (
chr(97 + i),
X0[i],
Y0[i],
Z0[i],
param_std[3 * i + 5],
param_std[3 * i + 6],
param_std[3 * i + 7],
)
print "\nPhoto coordinate residuals:"
print ("%5s" + " %9s" * 4) % ("Point", "xl-res", "yl-res", "xr-res", "yr-res")
for i in range(len(X0)):
print ("%5s" + " %9.4f" * 4) % (
chr(97 + i),
pho_res[2 * i],
pho_res[2 * i + 1],
pho_res[2 * i + len(X0) * 2],
pho_res[2 * i + len(X0) * 2 + 1],
)
print ("\n%5s" + " %9.4f" * 4 + "\n") % (
"RMS",
np.sqrt((pho_res[0 : len(X0) * 2 : 2] ** 2).mean()),
np.sqrt((pho_res[1 : len(X0) * 2 + 1 : 2] ** 2).mean()),
np.sqrt((pho_res[len(X0) * 2 :: 2] ** 2).mean()),
np.sqrt((pho_res[len(X0) * 2 + 1 :: 2] ** 2).mean()),
)
print "Standard error of unit weight : %.4f" % s0
print "Degree of freedom: %d" % (B.shape[0] - B.shape[1])
return 0
def acrobot_ode(x, t):
#for linearized model
u = my_pi(x)
dxdt = acrobot(x, u)
dxdt = (sympy.matrix2numpy(dxdt).astype(float)).squeeze()
return dxdt
