if __name__ == '__main__':
context = zmq.Context(1)
publisher = context.socket(zmq.PUB)
publisher.bind("tcp://*:5563")
print "reading config..."
# conf = readConfig('stingray.ini','configspec.ini')
conf = readConfig('config.ini', 'configspec.ini')
print "creating model..."
dae = models.crosswind(conf) #,extraParams=['endTime'])
nk = 30
# setup MHE
mhe = Nmhe(dae, nk)
# bounds
mhe.bound('aileron', (-0.04, 0.04))
mhe.bound('elevator', (-0.1, 0.3))
mhe.bound('x', (-200, 200))
mhe.bound('y', (-200, 200))
if 'minAltitude' in conf:
mhe.bound('z', (conf['minAltitude'], 200))
else:
mhe.bound('z', (0.5, 200))
mhe.bound('r', (1, 200))
mhe.bound('dr', (-30, 30))
for e in ['e11', 'e21', 'e31', 'e12', 'e22', 'e32', 'e13', 'e23', 'e33']:
for k in range(nk):
uTraj.append(np.array([5*C.sin(k/float(nk))]))
# uTraj.append(np.array([0]))
newton.isolver.setInput(xTraj[-1],0)
newton.isolver.setInput(uTraj[-1],1)
newton.isolver.setInput(p[-1],2)
newton.isolver.output().set(1)
newton.isolver.evaluate()
xTraj.append(np.array(newton.isolver.output(1)).squeeze())
# for k in range(nk):
# print xTraj[k],uTraj[k]
# print xTraj[-1]
# setup MHE
nmhe = Nmhe(dae,nk)
# bounds
r = 0.3
nmhe.bound('x',(-2*r,2*r))
nmhe.bound('z',(-2*r,2*r))
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)
if __name__=='__main__':
context = zmq.Context(1)
publisher = context.socket(zmq.PUB)
publisher.bind("tcp://*:5563")
print "reading config..."
# conf = readConfig('stingray.ini','configspec.ini')
conf = readConfig('config.ini','configspec.ini')
print "creating model..."
dae = models.crosswind(conf)#,extraParams=['endTime'])
nk = 30
# setup MHE
mhe = Nmhe(dae,nk)
# bounds
mhe.bound('aileron',(-0.04,0.04))
mhe.bound('elevator',(-0.1,0.3))
mhe.bound('x',(-200,200))
mhe.bound('y',(-200,200))
if 'minAltitude' in conf:
mhe.bound('z',(conf['minAltitude'],200))
else:
mhe.bound('z',(0.5,200))
mhe.bound('r',(1,200))
mhe.bound('dr',(-30,30))
for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
for k in range(nk):
uTraj.append(np.array([5 * C.sin(k / float(nk))]))
# uTraj.append(np.array([0]))
newton.isolver.setInput(xTraj[-1], 0)
newton.isolver.setInput(uTraj[-1], 1)
newton.isolver.setInput(p[-1], 2)
newton.isolver.output().set(1)
newton.isolver.evaluate()
xTraj.append(np.array(newton.isolver.output(1)).squeeze())
# for k in range(nk):
# print xTraj[k],uTraj[k]
# print xTraj[-1]
# setup MHE
nmhe = Nmhe(dae, nk)
# bounds
r = 0.3
nmhe.bound('x', (-2 * r, 2 * r))
nmhe.bound('z', (-2 * r, 2 * r))
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)
