for d in ['dx', 'dy', 'dz']:
mhe.bound(d, (-70, 70))
for w in ['w1', 'w2', 'w3']:
mhe.bound(w, (-4 * pi, 4 * pi))
# mhe.bound('endTime',(0.5,10))
# mhe.bound('endTime',(0.5,numLoops*7))
mhe.bound('w0', (10, 10))
# boundary conditions
# mhe.bound('y',(0,0),timestep=0)
# guesses
# mhe.guess('endTime',5.4)
mhe.guess('w0', 10)
# constrain invariants
def constrainInvariantErrs():
dcm = mhe.lookup('dcm', timestep=0)
err = C.mul(dcm.T, dcm)
mhe.constrain(
C.veccat([
err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1],
err[0, 2], err[1, 2]
]), '==', 0)
mhe.constrain(mhe.lookup('c', timestep=0), '==', 0)
mhe.constrain(mhe.lookup('cdot', timestep=0), '==', 0)
constrainInvariantErrs()
nmhe.bound('dx',(-50,50))
nmhe.bound('dz',(-50,50))
nmhe.bound('m',(0.28,0.32))
# boundary conditions
# nmhe.bound('x',(r,r),timestep=0)
# nmhe.bound('z',(0,0),timestep=0)
# nmhe.bound('dx',(0,0),timestep=0)
# nmhe.bound('dz',(0,0),timestep=0)
# constrain invariants
nmhe.constrain(nmhe.lookup('c',timestep=0),'==',0)
nmhe.constrain(nmhe.lookup('cdot',timestep=0),'==',0)
# initial guess
nmhe.guess('m',0.3)
for k in range(nk+1):
nmhe.guess('x', xTraj[k][0],timestep=k)
nmhe.guess('z', xTraj[k][1]+0.1,timestep=k)
nmhe.guess('dx',xTraj[k][2],timestep=k)
nmhe.guess('dz',xTraj[k][3],timestep=k)
# make objective
obj = 0
for k in range(nk+1):
nmhe.addGaussNewtonObjF(nmhe('x',timestep=k) - xTraj[k][0])
nmhe.addGaussNewtonObjF(nmhe('z',timestep=k) - xTraj[k][1])
nmhe.addGaussNewtonObjF(nmhe('dx',timestep=k) - xTraj[k][2])
nmhe.addGaussNewtonObjF(nmhe('dz',timestep=k) - xTraj[k][3])
# obj += (nmhe('x',timestep=k) - xTraj[k][0])**2
for d in ['dx','dy','dz']:
mhe.bound(d,(-70,70))
for w in ['w1','w2','w3']:
mhe.bound(w,(-4*pi,4*pi))
# mhe.bound('endTime',(0.5,10))
# mhe.bound('endTime',(0.5,numLoops*7))
mhe.bound('w0',(10,10))
# boundary conditions
# mhe.bound('y',(0,0),timestep=0)
# guesses
# mhe.guess('endTime',5.4)
mhe.guess('w0',10)
# constrain invariants
def constrainInvariantErrs():
dcm = mhe.lookup('dcm',timestep=0)
err = C.mul(dcm.T,dcm)
mhe.constrain( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0)
mhe.constrain(mhe.lookup('c',timestep=0), '==', 0)
mhe.constrain(mhe.lookup('cdot',timestep=0), '==', 0)
constrainInvariantErrs()
# initial guess
print "loading trajectory..."
f=open('data/crosswind_opt.dat','r')
traj = pickle.load(f)
f.close()
nmhe.bound('dx', (-50, 50))
nmhe.bound('dz', (-50, 50))
nmhe.bound('m', (0.28, 0.32))
# boundary conditions
# nmhe.bound('x',(r,r),timestep=0)
# nmhe.bound('z',(0,0),timestep=0)
# nmhe.bound('dx',(0,0),timestep=0)
# nmhe.bound('dz',(0,0),timestep=0)
# constrain invariants
nmhe.constrain(nmhe.lookup('c', timestep=0), '==', 0)
nmhe.constrain(nmhe.lookup('cdot', timestep=0), '==', 0)
# initial guess
nmhe.guess('m', 0.3)
for k in range(nk + 1):
nmhe.guess('x', xTraj[k][0], timestep=k)
nmhe.guess('z', xTraj[k][1] + 0.1, timestep=k)
nmhe.guess('dx', xTraj[k][2], timestep=k)
nmhe.guess('dz', xTraj[k][3], timestep=k)
# make objective
obj = 0
for k in range(nk + 1):
nmhe.addGaussNewtonObjF(nmhe('x', timestep=k) - xTraj[k][0])
nmhe.addGaussNewtonObjF(nmhe('z', timestep=k) - xTraj[k][1])
nmhe.addGaussNewtonObjF(nmhe('dx', timestep=k) - xTraj[k][2])
nmhe.addGaussNewtonObjF(nmhe('dz', timestep=k) - xTraj[k][3])
# obj += (nmhe('x',timestep=k) - xTraj[k][0])**2