for name in dae.xNames():
for k in range(nk + 1):
mhe.guess(name, traj.lookup(name, timestep=k), timestep=k)
for name in dae.pNames():
mhe.guess(name, traj.lookup(name))
# make objective
obj = 0
for name in [
'x', 'y', 'z', 'dx', 'dy', 'dz', 'w1', 'w2', 'w3', 'aileron',
'elevator'
]:
for k in range(nk + 1):
f = mhe.lookup(name, timestep=k) - traj.lookup(name, timestep=k)
mhe.addGaussNewtonObjF(f)
# obj += 0.5*f*f
# for name in dae.pNames():
# mhe.guess(name,traj.lookup(name))
# 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
# obj += (nmhe('z',timestep=k) - xTraj[k][1])**2
# obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2
# obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2
# obj += 1e-8*nmhe('x',timestep=k)**2
# obj += 1e-8*nmhe('z',timestep=k)**2
# 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
# obj += (nmhe('z',timestep=k) - xTraj[k][1])**2
# obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2
# obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2
# obj += 1e-8*nmhe('x',timestep=k)**2
# obj += 1e-8*nmhe('z',timestep=k)**2
# obj += 1e-8*nmhe('dx',timestep=k)**2
# obj += 1e-8*nmhe('dz',timestep=k)**2
# obj += 1e-8*nmhe('m')**2
f=open('data/crosswind_opt.dat','r')
traj = pickle.load(f)
f.close()
for name in dae.xNames():
for k in range(nk+1):
mhe.guess(name,traj.lookup(name,timestep=k),timestep=k)
for name in dae.pNames():
mhe.guess(name,traj.lookup(name))
# make objective
obj = 0
for name in ['x','y','z','dx','dy','dz','w1','w2','w3','aileron','elevator']:
for k in range(nk+1):
f = mhe.lookup(name,timestep=k) - traj.lookup(name,timestep=k)
mhe.addGaussNewtonObjF(f)
# obj += 0.5*f*f
# for name in dae.pNames():
# mhe.guess(name,traj.lookup(name))
# 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
# obj += (nmhe('z',timestep=k) - xTraj[k][1])**2
# obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2
# obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2
# obj += 1e-8*nmhe('x',timestep=k)**2
# obj += 1e-8*nmhe('z',timestep=k)**2
# 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
# obj += (nmhe('z',timestep=k) - xTraj[k][1])**2
# obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2
# obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2
# obj += 1e-8*nmhe('x',timestep=k)**2
# obj += 1e-8*nmhe('z',timestep=k)**2
# obj += 1e-8*nmhe('dx',timestep=k)**2
# obj += 1e-8*nmhe('dz',timestep=k)**2
# obj += 1e-8*nmhe('m')**2
