def _setupDynamicsConstraints(self, endTime, traj):
# Todo: add parallelization
# Todo: get endTime right
g = []
nicp = 1
deg = 4
p = self._dvMap.pVec()
for k in range(self.nk):
newton = Newton(LagrangePoly, self.dae, 1, nicp, deg, 'RADAU')
newton.setupStuff(endTime)
X0_i = self._dvMap.xVec(k)
U_i = self._U[k, :].T
# guess
if traj is None:
newton.isolver.setOutput(1, 0)
else:
X = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._xNames])
Z = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._zNames])
newton.isolver.setOutput(C.veccat([X, Z]), 0)
_, Xf_i = newton.isolver.call([X0_i, U_i, p])
X0_i_plus = self._dvMap.xVec(k + 1)
g.append(Xf_i - X0_i_plus)
return g
def advanceState(x):
axes = js.getAxes()
torque = axes[0]
u = C.DMatrix([torque, 0.4])
f.setInput(x, C.INTEGRATOR_X0)
f.setInput(u, C.INTEGRATOR_P)
f.evaluate()
return (C.DMatrix(f.output()), u)
def computeOutputs(self, x, u):
self._outputsFun.setInput(x, 0)
self._outputsFun.setInput(u, 1)
self._outputsFun.evaluate()
outs = [
C.DMatrix(self._outputsFun.output(k))
for k in range(self._outputsFun.getNumOutputs())
]
return numpy.squeeze(numpy.array(C.veccat(outs)))
def advanceState():
js = sim.handleInput()
# saving/loading
fstButton = 13
for k in range(0, 4):
if k + fstButton + 4 in js['buttonsDown'] and k in sim._saves:
print "loading save #" + str(k + 1)
sim.load(k)
sim.default = k
if k + fstButton in js['buttonsDown']:
if k == 0:
print "can't override save0"
else:
print "making save #" + str(k + 1)
sim.save(k)
sim.default = k
if 5 in js['buttonsDown']:
k = sim.default
print "loading save #" + str(k + 1)
sim.load(k)
# play replay
if 3 in js['buttonsDown']:
sim.playReplay(communicator)
sim.loadDefault()
aileron = -js['axes'][0] * 0.05
elevator = js['axes'][1] * 0.2
rudder = -js['axes'][2] * 0.15
tc = 600 * (1 - js['axes'][6])
wind_x = 5 * (1 - js['axes'][7])
wind_x = 0
x = sim.getCurrentState().x
u = C.DMatrix([tc, aileron, elevator, rudder])
p = C.DMatrix([wind_x])
f.setInput(x, C.INTEGRATOR_X0)
f.setInput(C.veccat([u, p]), C.INTEGRATOR_P)
f.evaluate()
xNext = C.DMatrix(f.output())
return ((x, u, p), xNext)
def _calc_der_vals(self):
# Derivatives of all basis polynomials at all interpolation points
der_vals = casadi.DMatrix(self.n + 1, self.n + 1)
for j in xrange(self.n + 1):
for k in xrange(self.n + 1):
der_vals[j, k] = lagrange_derivative_eval(self.p, j,
self.p[k])
# Store derivative values as data attribute
self.der_vals = der_vals
def getSteadyState( self ):
guessVec = C.DMatrix([self.guess[n] for n in self.dvsNames] + [self.dotGuess[n] for n in self.dae.xNames()])
boundsVec = [self.bounds[n] for n in self.dvsNames] + [self.dotBounds[n] for n in self.dae.xNames()]
self.solver.setInput(self.lbg, 'lbg')
self.solver.setInput(self.ubg, 'ubg')
self.solver.setInput(guessVec, 'x0')
lb, ub = zip(*boundsVec)
self.solver.setInput(C.DMatrix(lb), 'lbx')
self.solver.setInput(C.DMatrix(ub), 'ubx')
self.solver.solve()
ret = self.solver.getStat('return_status')
assert ret in ['Solve_Succeeded', 'Solved_To_Acceptable_Level'], 'Solver failed: ' + ret
xOpt = self.solver.output('x')
k = 0
sol = {}
for name in self.dvsNames:
sol[name] = xOpt[k].at(0)
k += 1
dotSol = {}
for name in self.dae.xNames():
dotSol[name] = xOpt[k].at(0)
k += 1
if self.robustVersion is True:
self.zFcn.setInput(C.DMatrix([sol[ n ] for n in self.dvsNames]), 0)
self.zFcn.evaluate()
zEval = self.zFcn.output( 0 )
for k, name in enumerate( self.dae.zNames() ):
sol[ name ] = zEval[ k ].at( 0 )
# Just a quick check
for name in self.dae.xNames():
if abs( dotSol[ name ] ) > 1e-8 and name not in ["sin_delta", "cos_delta"]:
print "Warning: dotSol[ %s ] = %5.3f" % (name, dotSol[ name ])
return sol, dotSol
def getOutputs(self, x, u, p):
if self.outputsFun0 == None:
return {}
(xVec, uVec, pVec) = vectorizeXUP(x, u, p, self.dae)
self.outputsFun0.setInput(xVec, 0)
self.outputsFun0.setInput(uVec, 1)
self.outputsFun0.setInput(pVec, 2)
self.outputsFun0.evaluate()
ret = {}
for k, name in enumerate(self.outputs0names):
ret[name] = maybeToScalar(C.DMatrix(self.outputsFun0.output(k)))
return ret
def step(self, x, u, p):
(xVec, uVec, pVec) = vectorizeXUP(x, u, p, self.dae)
self.integrator.setInput(xVec, C.INTEGRATOR_X0)
self.integrator.setInput(C.veccat([uVec, pVec]), C.INTEGRATOR_P)
self.integrator.evaluate()
xNext = C.DMatrix(self.integrator.output())
if type(x) == dict:
ret = {}
for k, name in enumerate(self.dae.xNames()):
ret[name] = xNext[k].at(0)
else:
ret = xNext
return ret
def qpsolve(H,
g,
lbx,
ubx,
A=NP.zeros((0, 0)),
lba=NP.zeros(0),
uba=NP.zeros(0)):
# Convert to CasADi types
H = C.DMatrix(H)
g = C.DMatrix(g)
lbx = C.DMatrix(lbx)
ubx = C.DMatrix(ubx)
A = C.DMatrix(A)
A = A.reshape((A.size1(), H.size1())) # Make sure matching dimensions
lba = C.DMatrix(lba)
uba = C.DMatrix(uba)
# QP structure
qp = C.qpStruct(h=H.sparsity(), a=A.sparsity())
# Create CasADi solver instance
if False:
solver = C.QpSolver("qpoases", qp)
else:
solver = C.QpSolver("nlp", qp)
solver.setOption("nlp_solver", "ipopt")
# Set options
#solver.setOption("option_name", ...)
# Initialize the solver
solver.init()
# Pass problem data
solver.setInput(H, "h")
solver.setInput(g, "g")
solver.setInput(A, "a")
solver.setInput(lbx, "lbx")
solver.setInput(ubx, "ubx")
solver.setInput(lba, "lba")
solver.setInput(uba, "uba")
# Solver the QP
solver.evaluate()
# Return the solution
return NP.array(solver.getOutput("x"))
# # You should have received a copy of the GNU Lesser General Public # License along with CasADi; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA # # import casadi as ca import numpy as np import matplotlib.pyplot as plt nk = 100 # Control discretization tf = 10.0 # End time coll = False # Use collocation integrator # initial and target points x0 = ca.DMatrix([0, np.pi / 10, 0, 0]) x_target = ca.DMatrix([0, np.pi, 0, 0]) # control limitations u_min = -40. u_max = 40. x_min = -10. x_max = 10. # constants g = 9.81 mc = 1.0 mp = 1. l = 1. mip = 1. mic = 5.
# ----------------------------------------------------------------------------
# Parameters
# ----------------------------------------------------------------------------
# Simulation
t_sim = 2.0 # simulation time
n_sim = 20 # time steps
dt = t_sim / n_sim # time-step length
t = np.linspace(0, t_sim, n_sim + 1)
# Model
nx = 3
nu = 2
# Initial condition
x0 = ca.DMatrix([10, 6, ca.pi])
# Identity matrix
Inx = ca.DMatrix.eye(nx)
# ----------------------------------------------------------------------------
# Dynamics
# ----------------------------------------------------------------------------
# Continuous dynamics
dt_sym = ca.SX.sym('dt')
state = cat.struct_symSX(['x', 'y', 'phi'])
control = cat.struct_symSX(['v', 'w'])
rhs = cat.struct_SX(state)
rhs['x'] = control['v'] * ca.cos(state['phi'])
rhs['y'] = control['v'] * ca.sin(state['phi'])
def check_and_set_initials(self, \
uN = None, \
pinit = None, \
xinit = None):
'''
:param tbd: tbd
:type tbd: tbd
Define structures for the initial values for the
several variables that build up the parameter estimation problem,
and prepare the values provided with the arguments properly.
Afterwards, the values are stored inside the class variable
`Varsinit``.
'''
# Define structures for initial values from the original
# variable struct of the problem
# Set controls values
# (only if the number of controls is not 0, else set them nothing)
if not self.nu == 0:
if uN is None:
uN = np.zeros((self.nu, self.nsteps))
uN = np.atleast_2d(uN)
if uN.shape == (self.nsteps, self.nu):
uN = uN.T
if not uN.shape == (self.nu, self.nsteps):
raise ValueError( \
"Wrong dimension for control values uN.")
self.uN = uN
else:
self.uN = ca.DMatrix(0, self.nsteps)
# Set initials for the parameters
if pinit is None:
pinit = np.zeros(self.np)
pinit = np.atleast_1d(np.squeeze(pinit))
if not pinit.shape == (self.np,):
raise ValueError( \
"Wrong dimension for argument pinit.")
self.Pinit = pinit
# If it's a dynamic problem, set initials and bounds for the states
if type(self.system) is not systems.BasicSystem:
if xinit is None:
xinit = np.zeros((self.nx, self.nsteps + 1))
xinit = np.atleast_2d(xinit)
if xinit.shape == (self.nsteps + 1, self.nx):
xinit = xinit.T
if not xinit.shape == (self.nx, self.nsteps + 1):
raise ValueError( \
"Wrong dimension for argument xinit.")
self.Xinit = ca.repmat(xinit[:,:-1], self.ntauroot+1, 1)
self.XFinit = xinit[:,-1]
else:
self.Xinit = ca.DMatrix(0, 0)
self.XFinit = ca.DMatrix(0, 0)
self.Vinit = np.zeros(self.V.shape)
self.EPS_Einit = np.zeros(self.EPS_E.shape)
self.EPS_Uinit = np.zeros(self.EPS_U.shape)
def __init__(self, system = None, \
tu = None, uN = None, \
ty = None, yN = None,
pinit = None, \
xinit = None, \
scheme = "radau", \
order = 3):
self.tstart_setup = time.time()
SetupsBaseClass.__init__(self)
if not type(system) is systems.ExplODE:
raise TypeError("Setup-method " + self.__class__.__name__ + \
" not allowed for system of type " + str(type(system)) + ".")
self.system = system
# Dimensions
self.nx = system.x.shape[0]
self.nu = system.u.shape[0]
self.np = system.p.shape[0]
self.neps_e = system.eps_e.shape[0]
self.neps_u = system.eps_u.shape[0]
self.nphi = system.phi.shape[0]
if np.atleast_2d(tu).shape[0] == 1:
self.tu = np.asarray(tu)
elif np.atleast_2d(tu).shape[1] == 1:
self.tu = np.squeeze(np.atleast_2d(tu).T)
else:
raise ValueError("Invalid dimension for argument tu.")
if ty == None:
self.ty = self.tu
elif np.atleast_2d(ty).shape[0] == 1:
self.ty = np.asarray(ty)
elif np.atleast_2d(ty).shape[1] == 1:
self.ty = np.squeeze(np.atleast_2d(ty).T)
else:
raise ValueError("Invalid dimension for argument ty.")
self.nsteps = self.tu.shape[0] - 1
self.scheme = scheme
self.order = order
self.tauroot = ca.collocationPoints(order, scheme)
# Degree of interpolating polynomial
self.ntauroot = len(self.tauroot) - 1
# Define the optimization variables
self.P = ca.MX.sym("P", self.np)
self.X = ca.MX.sym("X", (self.nx * (self.ntauroot+1)), self.nsteps)
self.XF = ca.MX.sym("XF", self.nx)
self.V = ca.MX.sym("V", self.nphi, self.nsteps+1)
if self.neps_e != 0:
self.EPS_E = ca.MX.sym("EPS_E", \
(self.neps_e * self.ntauroot), self.nsteps)
else:
self.EPS_E = ca.DMatrix(0, self.nsteps)
if self.neps_u != 0:
self.EPS_U = ca.MX.sym("EPS_U", \
(self.neps_u * self.ntauroot), self.nsteps)
else:
self.EPS_U = ca.DMatrix(0, self.nsteps)
# Define bounds and initial values
self.check_and_set_initials( \
uN = uN, \
pinit = pinit, \
xinit = xinit)
# Set tp the collocation coefficients
# Coefficients of the collocation equation
self.C = np.zeros((self.ntauroot + 1, self.ntauroot + 1))
# Coefficients of the continuity equation
self.D = np.zeros(self.ntauroot + 1)
# Dimensionless time inside one control interval
tau = ca.SX.sym("tau")
# Construct the matrix T that contains all collocation time points
self.T = np.zeros((self.nsteps, self.ntauroot + 1))
for k in range(self.nsteps):
for j in range(self.ntauroot + 1):
self.T[k,j] = self.tu[k] + \
(self.tu[k+1] - self.tu[k]) * self.tauroot[j]
self.T = self.T.T
# For all collocation points
self.lfcns = []
for j in range(self.ntauroot + 1):
# Construct Lagrange polynomials to get the polynomial basis
# at the collocation point
L = 1
for r in range(self.ntauroot + 1):
if r != j:
L *= (tau - self.tauroot[r]) / \
(self.tauroot[j] - self.tauroot[r])
lfcn = ca.SXFunction("lfcn", [tau],[L])
# Evaluate the polynomial at the final time to get the
# coefficients of the continuity equation
[self.D[j]] = lfcn([1])
# Evaluate the time derivative of the polynomial at all
# collocation points to get the coefficients of the
# collocation equation
tfcn = lfcn.tangent()
for r in range(self.ntauroot + 1):
self.C[j,r] = tfcn([self.tauroot[r]])[0]
self.lfcns.append(lfcn)
# Initialize phiN
self.phiN = []
# Initialize measurement function
phifcn = ca.MXFunction("phifcn", \
[system.t, system.u, system.x, system.eps_u, system.p], \
[system.phi])
# Initialzie setup of g
self.g = []
# Initialize ODE right-hand-side
ffcn = ca.MXFunction("ffcn", \
[system.t, system.u, system.x, system.eps_e, system.eps_u, \
system.p], [system.f])
# Collect information for measurement function
# Structs to hold variables for later mapped evaluation
Tphi = []
Uphi = []
Xphi = []
EPS_Uphi = []
for k in range(self.nsteps):
hk = self.tu[k + 1] - self.tu[k]
t_meas = self.ty[np.where(np.logical_and( \
self.ty >= self.tu[k], self.ty < self.tu[k + 1]))]
for t_meas_j in t_meas:
Uphi.append(self.uN[:, k])
EPS_Uphi.append(self.EPS_U[:self.neps_u, k])
if t_meas_j == self.tu[k]:
Tphi.append(self.tu[k])
Xphi.append(self.X[:self.nx, k])
else:
tau = (t_meas_j - self.tu[k]) / hk
x_temp = 0
for r in range(self.ntauroot + 1):
x_temp += self.lfcns[r]([tau])[0] * \
self.X[r*self.nx : (r+1) * self.nx, k]
Tphi.append(t_meas_j)
Xphi.append(x_temp)
if self.tu[-1] in self.ty:
Tphi.append(self.tu[-1])
Uphi.append(self.uN[:,-1])
Xphi.append(self.XF)
EPS_Uphi.append(self.EPS_U[:self.neps_u,-1])
# Mapped calculation of the collocation equations
# Collocation nodes
hc = ca.MX.sym("hc", 1)
tc = ca.MX.sym("tc", self.ntauroot)
xc = ca.MX.sym("xc", self.nx * (self.ntauroot+1))
eps_ec = ca.MX.sym("eps_ec", self.neps_e * self.ntauroot)
eps_uc = ca.MX.sym("eps_uc", self.neps_u * self.ntauroot)
coleqn = ca.vertcat([ \
hc * ffcn([tc[j-1], \
system.u, \
xc[j*self.nx : (j+1)*self.nx], \
eps_ec[(j-1)*self.neps_e : j*self.neps_e], \
eps_uc[(j-1)*self.neps_u : j*self.neps_u], \
system.p])[0] - \
sum([self.C[r,j] * xc[r*self.nx : (r+1)*self.nx] \
for r in range(self.ntauroot + 1)]) \
for j in range(1, self.ntauroot + 1)])
coleqnfcn = ca.MXFunction("coleqnfcn", \
[hc, tc, system.u, xc, eps_ec, eps_uc, system.p], [coleqn])
coleqnfcn = coleqnfcn.expand()
[gcol] = coleqnfcn.map([ \
np.atleast_2d((self.tu[1:] - self.tu[:-1])), self.T[1:,:], \
self.uN, self.X, self.EPS_E, self.EPS_U, self.P])
# Continuity nodes
xnext = ca.MX.sym("xnext", self.nx)
conteqn = xnext - sum([self.D[r] * xc[r*self.nx : (r+1)*self.nx] \
for r in range(self.ntauroot + 1)])
conteqnfcn = ca.MXFunction("conteqnfcn", [xnext, xc], [conteqn])
conteqnfcn = conteqnfcn.expand()
[gcont] = conteqnfcn.map([ \
ca.horzcat([self.X[:self.nx, 1:], self.XF]), self.X])
# Stack equality constraints together
self.g = ca.veccat([gcol, gcont])
# Evaluation of the measurement function
[self.phiN] = phifcn.map( \
[ca.horzcat(k) for k in Tphi, Uphi, Xphi, EPS_Uphi] + \
[self.P])
# self.phiNfcn = ca.MXFunction("phiNfcn", [self.Vars], [self.phiN])
self.tend_setup = time.time()
self.duration_setup = self.tend_setup - self.tstart_setup
print('Initialization of ExplODE system sucessful.')
def __init__(self, system = None, \
tu = None, uN = None, \
pinit = None):
SetupsBaseClass.__init__(self)
if not type(system) is systems.BasicSystem:
raise TypeError("Setup-method " + self.__class__.__name__ + \
" not allowed for system of type " + str(type(system)) + ".")
self.system = system
# Dimensions
self.nu = system.u.shape[0]
self.np = system.p.shape[0]
self.nphi = system.phi.shape[0]
if np.atleast_2d(tu).shape[0] == 1:
self.tu = np.asarray(tu)
elif np.atleast_2d(tu).shape[1] == 1:
self.tu = np.squeeze(np.atleast_2d(tu).T)
else:
raise ValueError("Invalid dimension for argument tu.")
self.nsteps = tu.shape[0]
# Define the struct holding the variables
self.P = ca.MX.sym("P", self.np)
self.X = ca.DMatrix(0, self.nsteps)
self.XF = ca.DMatrix(0, self.nsteps)
self.V = ca.MX.sym("V", self.nphi, self.nsteps)
self.EPS_E = ca.DMatrix(0, self.nsteps)
self.EPS_U = ca.DMatrix(0, self.nsteps)
# Set bounds and initial values
self.check_and_set_initials( \
uN = uN,
pinit = pinit)
# Set up phiN
self.phiN = []
phifcn = ca.MXFunction("phifcn", \
[system.t, system.u, system.p], [system.phi])
for k in range(self.nsteps):
self.phiN.append(phifcn([self.tu[k], \
self.uN[:, k], self.P])[0])
self.phiN = ca.vertcat(self.phiN)
# self.phiNfcn = ca.MXFunction("phiNfcn", [self.Vars], [self.phiN])
# Set up g
# TODO! Can/should/must gfcn depend on uN and/or t?
gfcn = ca.MXFunction("gfcn", [system.p], [system.g])
self.g = gfcn.call([self.P])[0]
self.tend_setup = time.time()
self.duration_setup = self.tend_setup - self.tstart_setup
print('Initialization of BasicSystem system sucessful.')
xSim = np.copy(x0) + ca.vertcat([np.random.normal() * scaleX[k] / 10 for k in range(ocp.x.size())])
zSim = np.copy(z0)
xSh.append(xSim)
xh = []
uh = []
yh = []
yPh = []
Juh_NMPC = []
monitoredOlgah = []
monitoredModelicah = []
Obj_total = 0
Objh_NMPC = []
ObjTotalh = []
simTime = []
xh.append(ca.DMatrix(x0))
ULog = None
monitor = True
if monitor:
measurementsEst = ca.vertcat([ocp.variable(varName).v for varName in measurementsMonitorModelica])
measure_funcEst = ocp.beq(measurementsEst)
HEst = ca.SXFunction(ca.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t), [measure_funcEst])
HEst.init()
if openLoop or controlOlga: # testing state estimation on
if controlOlga and not openLoop:
y0Olga = Olga.OLGA_read_tags(da, measurementTagsOlga)
extendedKalman = True
# u0 = ca.vertcat([ocp.variable(xit.getName()).initialGuess.getValue() for xit in ocp.u])
def runSolver(self, U, trajTrue=None):
# make sure all bounds are set
(xMissing, pMissing) = self._guessMap.getMissing()
msg = []
for name in xMissing:
msg.append("you forgot to set a guess for \"" + name +
"\" at timesteps: " + str(xMissing[name]))
for name in pMissing:
msg.append("you forgot to set a guess for \"" + name + "\"")
if len(msg) > 0:
raise ValueError('\n'.join(msg))
lbx, ubx = zip(*(self._boundMap.vectorize()))
xk = C.DMatrix(list(self._guessMap.vectorize()))
for k in range(100):
############# plot stuff ###############
print "iteration: ", k
# import nmheMaps
# xOpt = np.array(xk).squeeze()
# traj = nmheMaps.VectorizedReadOnlyNmheMap(self.dae,self.nk,xOpt)
#
# xsT = np.array([trajTrue.lookup('x',timestep=kk) for kk in range(self.nk+1)] )
# ysT = np.array([trajTrue.lookup('y',timestep=kk) for kk in range(self.nk+1)] )
# zsT = np.array([trajTrue.lookup('z',timestep=kk) for kk in range(self.nk+1)] )
#
# xs = np.array([traj.lookup('x',timestep=kk) for kk in range(self.nk+1)] )
# ys = np.array([traj.lookup('y',timestep=kk) for kk in range(self.nk+1)] )
# zs = np.array([traj.lookup('z',timestep=kk) for kk in range(self.nk+1)] )
#
# outputMap = nmheMaps.NmheOutputMap(self._outputMapGenerator, xOpt, U)
# c = np.array([outputMap.lookup('c',timestep=kk) for kk in range(self.nk)])
# cdot = np.array([outputMap.lookup('cdot',timestep=kk) for kk in range(self.nk)])
#
# figure()
# title(str(float(k)))
# subplot(3,2,1)
# plot(xs)
# plot(xsT)
# ylabel('x '+str(k))
#
# subplot(3,2,3)
# plot(ys)
# plot(ysT)
# ylabel('y '+str(k))
#
# subplot(3,2,5)
# plot(zs)
# plot(zsT)
# ylabel('z '+str(k))
#
## subplot(2,2,2)
## plot(dxs,-dzs)
## ylabel('vel')
## axis('equal')
#
# subplot(3,2,2)
# plot(c)
# ylabel('c')
#
# subplot(3,2,4)
# plot(cdot)
# ylabel('cdot')
# ##########################################
self.masterFun.setInput(xk, 0)
self.masterFun.setInput(U, 1)
t0 = time.time()
try:
self.masterFun.evaluate()
except RuntimeError as e:
print "ERRRRRRRRRRRRROR"
show()
raise e
t1 = time.time()
masterFunTime = (t1 - t0) * 1000
hessL = self.masterFun.output(0)
gradF = self.masterFun.output(1)
g = self.masterFun.output(2)
jacobG = self.masterFun.output(3)
f = self.masterFun.output(4)
self.qp.setInput(0, C.QP_X_INIT)
self.qp.setInput(hessL, C.QP_H)
self.qp.setInput(jacobG, C.QP_A)
self.qp.setInput(gradF, C.QP_G)
assert all((lbx - xk) <= 0), "lower bounds violation"
assert all((ubx - xk) >= 0), "upper bounds violation"
self.qp.setInput(lbx - xk, C.QP_LBX)
self.qp.setInput(ubx - xk, C.QP_UBX)
self.qp.setInput(self.glb - g, C.QP_LBA)
self.qp.setInput(self.gub - g, C.QP_UBA)
t0 = time.time()
self.qp.evaluate()
t1 = time.time()
# print "gradF: ",gradF
# print 'dim(jacobG): "gra
# print "rank: ",np.linalg.matrix_rank(jacobG)
print "masterFun delta time: %.3f ms" % masterFunTime
print "f: ", f, '\tmax constraint: ', max(C.fabs(g))
print "qp delta time: %.3f ms" % ((t1 - t0) * 1000)
print ""
deltaX = self.qp.output(C.QP_PRIMAL)
# import scipy.io
# scipy.io.savemat('hessL.mat',{'hessL':np.array(hessL),
# 'gradF':np.array(gradF),
# 'x0':0*np.array(deltaX),
# 'xopt':np.array(deltaX),
# 'lbx':np.array(lbx-xk),
# 'ubx':np.array(ubx-xk),
# 'jacobG':np.array(jacobG),
# 'lba':np.array(self.glb-g),
# 'uba':np.array(self.gub-g)})
# import sys; sys.exit()
# print deltaX
xk += deltaX
import joy
#tc0 = 2*389.970797939731
x0 = C.DMatrix([
-10 # position
,
0,
15,
1 # DCM
,
0,
0,
0,
1,
0,
0,
0,
1,
20.0 # vel
,
0.0,
0.0,
0 # ang vel
,
0,
0
])
ts = 0.02
sim = simutils.Sim(ts=ts,
sloMoFactor=4,
def getSteadyState(dae, r0, v0):
# make steady state model
g = Constraints()
g.add(dae.getResidual(), '==', 0, tag=('dae residual', None))
def constrainInvariantErrs():
R_n2b = dae['R_n2b']
makeOrthonormal(g, R_n2b)
g.add(dae['c'], '==', 0, tag=('c(0)==0', None))
g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0', None))
constrainInvariantErrs()
# constrain airspeed
g.add(dae['airspeed'], '>=', v0, tag=('airspeed fixed', None))
g.addBnds(dae['alpha_deg'], (4, 10), tag=('alpha', None))
g.addBnds(dae['beta_deg'], (-10, 10), tag=('beta', None))
dvs = C.veccat(
[dae.xVec(),
dae.zVec(),
dae.uVec(),
dae.pVec(),
dae.xDotVec()])
# ffcn = C.SXFunction([dvs],[sum([dae[n]**2 for n in ['aileron','elevator','y','z']])])
obj = 0
obj += (dae['cL'] - 0.5)**2
obj += dae.ddt('w_bn_b_x')**2
obj += dae.ddt('w_bn_b_y')**2
obj += dae.ddt('w_bn_b_z')**2
ffcn = C.SXFunction([dvs], [obj])
gfcn = C.SXFunction([dvs], [g.getG()])
ffcn.init()
gfcn.init()
guess = {
'x': r0,
'y': 0,
'z': -1,
'r': r0,
'dr': 0,
'e11': 0,
'e12': -1,
'e13': 0,
'e21': 0,
'e22': 0,
'e23': 1,
'e31': -1,
'e32': 0,
'e33': 0,
'dx': 0,
'dy': -20,
'dz': 0,
'w_bn_b_x': 0,
'w_bn_b_y': 0,
'w_bn_b_z': 0,
'aileron': 0,
'elevator': 0,
'rudder': 0,
'daileron': 0,
'delevator': 0,
'drudder': 0,
'nu': 300,
'motor_torque': 10,
'dmotor_torque': 0,
'ddr': 0,
'dddr': 0.0,
'w0': 10.0
}
dotGuess = {
'x': 0,
'y': -20,
'z': 0,
'dx': 0,
'dy': 0,
'dz': 0,
'r': 0,
'dr': 0,
'e11': 0,
'e12': 0,
'e13': 0,
'e21': 0,
'e22': 0,
'e23': 0,
'e31': 0,
'e32': 0,
'e33': 0,
'w_bn_b_x': 0,
'w_bn_b_y': 0,
'w_bn_b_z': 0,
'aileron': 0,
'elevator': 0,
'rudder': 0,
'ddr': 0
}
guessVec = C.DMatrix([
guess[n]
for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames()
] + [dotGuess[n] for n in dae.xNames()])
bounds = {
'x': (0.01, r0 * 2),
'y': (0, 0),
'z': (-r0 * 0.2, -r0 * 0.2),
'dx': (0, 0),
'dy': (-50, 0),
'dz': (0, 0),
'r': (r0, r0),
'dr': (0, 0),
'ddr': (0, 0),
'e11': (-0.5, 0.5),
'e12': (-1.5, -0.5),
'e13': (-0.5, 0.5),
'e21': (-0.5, 0.5),
'e22': (-0.5, 0.5),
'e23': (0.5, 1.5),
'e31': (-1.5, -0.5),
'e32': (-0.5, 0.5),
'e33': (-0.5, 0.5),
'w_bn_b_x': (0, 0),
'w_bn_b_y': (0, 0),
'w_bn_b_z': (0, 0),
# 'aileron':(-0.2,0.2),'elevator':(-0.2,0.2),'rudder':(-0.2,0.2),
'aileron': (0, 0),
'elevator': (0, 0),
'rudder': (0, 0),
'daileron': (0, 0),
'delevator': (0, 0),
'drudder': (0, 0),
'nu': (0, 3000),
'dddr': (0, 0),
'w0': (10, 10)
}
dotBounds = {
'dz': (-C.inf, 0)
} #'dx':(-500,-500),'dy':(-500,500),'dz':(-500,500),
# 'w_bn_b_x':(0,0),'w_bn_b_y':(0,0),'w_bn_b_z':(0,0),
for name in dae.xNames():
if name not in dotBounds:
dotBounds[name] = (-C.inf, C.inf)
boundsVec = [bounds[n] for n in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()]+ \
[dotBounds[n] for n in dae.xNames()]
# gfcn.setInput(guessVec)
# gfcn.evaluate()
# ret = gfcn.output()
# for k,v in enumerate(ret):
# if math.isnan(v):
# print 'index ',k,' is nan: ',g._tags[k]
# import sys; sys.exit()
# context = zmq.Context(1)
# publisher = context.socket(zmq.PUB)
# publisher.bind("tcp://*:5563")
# class MyCallback:
# def __init__(self):
# import rawekite.kiteproto as kiteproto
# import rawekite.kite_pb2 as kite_pb2
# self.kiteproto = kiteproto
# self.kite_pb2 = kite_pb2
# self.iter = 0
# def __call__(self,f,*args):
# x = C.DMatrix(f.input('x'))
# sol = {}
# for k,name in enumerate(dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()):
# sol[name] = x[k].at(0)
# lookup = lambda name: sol[name]
# kp = self.kiteproto.toKiteProto(lookup,
# lineAlpha=0.2)
# mc = self.kite_pb2.MultiCarousel()
# mc.horizon.extend([kp])
# mc.messages.append("iter: "+str(self.iter))
# self.iter += 1
# publisher.send_multipart(["multi-carousel", mc.SerializeToString()])
solver = C.IpoptSolver(ffcn, gfcn)
def addCallback():
nd = len(boundsVec)
nc = g.getLb().size()
c = C.PyFunction(
MyCallback(),
C.nlpsolverOut(x=C.sp_dense(nd, 1),
f=C.sp_dense(1, 1),
lam_x=C.sp_dense(nd, 1),
lam_p=C.sp_dense(0, 1),
lam_g=C.sp_dense(nc, 1),
g=C.sp_dense(nc, 1)), [C.sp_dense(1, 1)])
c.init()
solver.setOption("iteration_callback", c)
# addCallback()
solver.setOption('max_iter', 10000)
solver.setOption('expand', True)
# solver.setOption('tol',1e-14)
# solver.setOption('suppress_all_output','yes')
# solver.setOption('print_time',False)
solver.init()
solver.setInput(g.getLb(), 'lbg')
solver.setInput(g.getUb(), 'ubg')
#guessVec = numpy.load('steady_state_guess.npy')
solver.setInput(guessVec, 'x0')
lb, ub = zip(*boundsVec)
solver.setInput(C.DMatrix(lb), 'lbx')
solver.setInput(C.DMatrix(ub), 'ubx')
solver.solve()
ret = solver.getStat('return_status')
assert ret in ['Solve_Succeeded',
'Solved_To_Acceptable_Level'], 'Solver failed: ' + ret
# publisher.close()
# context.destroy()
xOpt = solver.output('x')
#numpy.save('steady_state_guess',numpy.squeeze(numpy.array(xOpt)))
k = 0
sol = {}
for name in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames():
sol[name] = xOpt[k].at(0)
k += 1
# print name+':\t',sol[name]
dotSol = {}
for name in dae.xNames():
dotSol[name] = xOpt[k].at(0)
k += 1
# print 'DDT('+name+'):\t',dotSol[name]
return sol, dotSol
# 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
mhe.setObj(obj)
# nmhe.constrain(nmhe('dz',timestep=0)**2,'<=',1000)
# get u
uTraj = C.DMatrix([[traj.lookup(name, timestep=k) for k in range(nk)]
for name in dae.uNames()]).T
endTime = traj.tgrid[1, 0, 0] - traj.tgrid[0, 0, 0]
mhe.makeSolver(endTime, traj=traj)
mhe.runSolver(uTraj, traj)
# # callback function
# class MyCallback:
# def __init__(self):
# self.iter = 0
# def __call__(self,f,*args):
# self.iter = self.iter + 1
# xOpt = numpy.array(f.input(C.NLP_X_OPT))
#
# traj = trajectory.Trajectory(ocp,xOpt)
#
# kiteProtos = []
# Start Simulator
da = Olga.OLGA_connect(serverIP=serverIP, modelPrefix=model)
y0Olga = Olga.OLGA_read_tags(da, measurementTagsOlga)
xSh = []
xh = []
uh = []
yh = []
yPh = []
Juh_NMPC = []
monitoredOlgah = []
monitoredModelicah = []
Objh_NMPC = []
simTime = []
ObjTotalh = []
xh.append(ca.DMatrix(x0))
ULog = None
monitor = True
if monitor:
measurementsEst = ca.vertcat([ocp.variable(varName).v for varName in measurementsMonitorModelica])
measure_funcEst = ocp.beq(measurementsEst)
HEst = ca.SXFunction(ca.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t), [measure_funcEst])
HEst.init()
extendedKalman = True
# u0 = ca.vertcat([ocp.variable(xit.getName()).initialGuess.getValue() for xit in ocp.u])
# x0,z0,y0 = daeModel.findSteadyState(u0,None,None,0,consList=['net.w1.z1','net.w2.z1','net.p.z'])
KF.setState(x0, z0)
def run_simulation(self, \
x0 = None, tsim = None, usim = None, psim = None, method = "rk"):
r'''
:param x0: initial value for the states
:math:`x_0 \in \mathbb{R}^{n_x}`
:type x0: list, numpy,ndarray, casadi.DMatrix
:param tsim: optional, switching time points for the controls
:math:`t_{sim} \in \mathbb{R}^{L}` to be used for the
simulation
:type tsim: list, numpy,ndarray, casadi.DMatrix
:param usim: optional, control values
:math:`u_{sim} \in \mathbb{R}^{n_u \times L}`
to be used for the simulation
:type usim: list, numpy,ndarray, casadi.DMatrix
:param psim: optional, parameter set
:math:`p_{sim} \in \mathbb{R}^{n_p}`
to be used for the simulation
:type psim: list, numpy,ndarray, casadi.DMatrix
:param method: optional, CasADi integrator to be used for the
simulation
:type method: str
This function performs a simulation of the system for a given
parameter set :math:`p_{sim}`, starting from a user-provided initial
value for the states :math:`x_0`. If the argument ``psim`` is not
specified, the estimated parameter set :math:`\hat{p}` is used.
For this, a parameter
estimation using :func:`run_parameter_estimation()` has to be
done beforehand, of course.
By default, the switching time points for
the controls :math:`t_u` and the corresponding controls
:math:`u_N` will be used for simulation. If desired, other time points
:math:`t_{sim}` and corresponding controls :math:`u_{sim}`
can be passed to the function.
For the moment, the function can only be used for systems of type
:class:`pecas.systems.ExplODE`.
'''
intro.pecas_intro()
print('\n' + 27 * '-' + \
' PECas system simulation ' + 26 * '-')
print('\nPerforming system simulation, this might take some time ...')
if not type(self.pesetup.system) is systems.ExplODE:
raise NotImplementedError("Until now, this function can only " + \
"be used for systems of type ExplODE.")
if x0 == None:
raise ValueError("You have to provide an initial value x0 " + \
"to run the simulation.")
x0 = np.squeeze(np.asarray(x0))
if np.atleast_1d(x0).shape[0] != self.pesetup.nx:
raise ValueError("Wrong dimension for initial value x0.")
if tsim == None:
tsim = self.pesetup.tu
if usim == None:
usim = self.pesetup.uN
if psim == None:
try:
psim = self.phat
except AttributeError:
errmsg = '''
You have to either perform a parameter estimation beforehand to obtain a
parameter set that can be used for simulation, or you have to provide a
parameter set in the argument psim.
'''
raise AttributeError(errmsg)
else:
if not np.atleast_1d(np.squeeze(psim)).shape[0] == self.pesetup.np:
raise ValueError("Wrong dimension for parameter set psim.")
fp = ca.MXFunction("fp", \
[self.pesetup.system.t, self.pesetup.system.u, \
self.pesetup.system.x, self.pesetup.system.eps_e, \
self.pesetup.system.eps_u, self.pesetup.system.p], \
[self.pesetup.system.f])
fpeval = fp([\
self.pesetup.system.t, self.pesetup.system.u, \
self.pesetup.system.x, np.zeros(self.pesetup.neps_e), \
np.zeros(self.pesetup.neps_u), psim])[0]
fsim = ca.MXFunction("fsim", \
ca.daeIn(t = self.pesetup.system.t, \
x = self.pesetup.system.x, \
p = self.pesetup.system.u), \
ca.daeOut(ode = fpeval))
Xsim = []
Xsim.append(x0)
u0 = ca.DMatrix()
for k, e in enumerate(tsim[:-1]):
try:
integrator = ca.Integrator("integrator", method, \
fsim, {"t0": e, "tf": tsim[k+1]})
except RuntimeError as err:
errmsg = '''
It seems like you want to use an integration method that is not currently
supported by CasADi. Please refer to the CasADi documentation for a list
of supported integrators, or use the default RK4-method by not setting the
method-argument of the function.
'''
raise RuntimeError(errmsg)
if not self.pesetup.nu == 0:
u0 = usim[:, k]
Xk_end = itemgetter('xf')(integrator({'x0': x0, 'p': u0}))
Xsim.append(Xk_end)
x0 = Xk_end
self.Xsim = ca.horzcat(Xsim)
print( \
'''System simulation finished.''')
t = time()
dummy = c.mul(x.T, x)
print "CasADi pure = %.4f s" % (time() - t)
X = MX("X", x.sparsity())
t = time()
f = MXFunction([X], [c.mul(X.T, X)])
f.init()
print "CasADi MX wrapped init overhead = %.4f s" % (time() - t)
f.setInput(x)
t = time()
f.evaluate()
print "CasADi MX wrapped = %.4f s" % (time() - t)
t = time()
# Create the sparsity pattern for the matrix-matrix product
spres = x.sparsity().patternProduct(x.sparsity())
print "CasADi generating procuct sparsity pattern = %.4f s" % (time() - t)
t = time()
# Create the return object with correct sparsity
ret = c.DMatrix(spres)
print "CasADi allocating resulting = %.4f s" % (time() - t)
t = time()
# Carry out the matrix product
c.DMatrix.mul_no_alloc(x, x, ret)
print "CasADi actual multiplication = %.4f s" % (time() - t)
# along with PECas. If not, see <http://www.gnu.org/licenses/>.
# This example is an adapted version of the system identification example
# included in CasADi, for the original file see:
# https://github.com/casadi/casadi/blob/master/docs/examples/python/sysid.py
import casadi as ca
import pylab as pl
import pecas
import os
N = 1000
fs = 610.1
p_true = ca.DMatrix([5.625e-6, 2.3e-4, 1, 4.69])
p_guess = ca.DMatrix([5, 3, 1, 5])
scale = ca.vertcat([1e-6, 1e-4, 1, 1])
x = ca.MX.sym("x", 2)
u = ca.MX.sym("u", 1)
p = ca.MX.sym("p", 4)
f = ca.vertcat([
x[1], \
(u - scale[3] * p[3] * x[0]**3 - scale[2] * p[2] * x[0] - \
scale[1] * p[1] * x[1]) / (scale[0] * p[0]), \
])
phi = x
yN = y_randn, wv = wv)
lsqpe_test.run_parameter_estimation()
p_test.append(lsqpe_test.phat)
p_mean = pl.mean(p_test)
p_std = pl.std(p_test, ddof=0)
lsqpe_test.compute_covariance_matrix()
# Generate report
print("\np_mean = " + str(ca.DMatrix(p_mean)))
print("phat_last_exp = " + str(ca.DMatrix(lsqpe_test.phat)))
print("\np_sd = " + str(ca.DMatrix(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(lsqpe_test.Covp))))
print("beta = " + str(lsqpe_test.beta))
print("\ndelta_abs_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \
ca.diag(ca.sqrt(lsqpe_test.Covp)))))
print("delta_rel_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \
ca.diag(ca.sqrt(lsqpe_test.Covp))) / ca.DMatrix(p_std)))
fname = os.path.basename(__file__)[:-3] + ".rst"
report = open(fname, "w")
import pickle
import rawe
import rawekite
x0 = C.DMatrix( [ 1.154244772411
, -0.103540608242
, -0.347959211327
, 0.124930983341
, 0.991534857363
, 0.035367725910
, 0.316039689643
, -0.073559821379
, 0.945889986864
, 0.940484536806
, -0.106993361072
, -0.322554269411
, 0.000000000000
, 0.000000000000
, 0.000000000000
, 0.137035790811
, 3.664945343102
, -1.249768772258
, 3.874600000000
, 0.0
])
x0=C.veccat([x0,C.sqrt(C.sumAll(x0[0:2]*x0[0:2])),0,0,0])
def setupOcp(dae,conf,nk=50,nicp=1,deg=4):
ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)
ocp.setupCollocation(ocp.lookup('endTime'))
# 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
# nmhe.constrain(nmhe('dz',timestep=0)**2,'<=',1000)
nmhe.setObj(obj)
uTraj = C.DMatrix(np.concatenate(uTraj))
nmhe.makeSolver(dt)
nmhe.runSolver(uTraj)
## newton.isolver.input(0).set(x0)
# newton.isolver.setInput(x0,0)
# newton.isolver.setInput(0,1) # torque
# newton.isolver.setInput(0.3,2) # mass
# newton.isolver.evaluate()
# newton.isolver.evaluate()
#
# j = newton.isolver.jacobian(0,1)
# j.init()
# j.setInput(x0,0)
# j.setInput(0,1)
# j.setInput(0.3,2)
cY += conf['cY_rudder'] * dae['rudder']
cD += conf['cD_rudder2'] * dae['rudder'] * dae['rudder'] + conf[
'cD_B_rudder'] * beta * dae['rudder'] + conf['cD_rudder'] * dae[
'rudder']
dae['cL'] = cL
dae['cD'] = cD
dae['cY'] = cY
dae['cD_tether'] = 0.25 * dae['r'] * 0.001 / sref
dae['L_over_D'] = cL / cD
cD = dae['cD'] + dae['cD_tether']
dae['L_over_D_with_tether'] = cL / cD
######## moment coefficients #######
# offset
dae['momentCoeffs0'] = C.DMatrix([0, conf['cm0'], 0])
# with roll rates
# non-dimensionalized angular velocity
w_bn_b_hat = C.veccat([
0.5 * conf['bref'] / dae['airspeed'] * w_bn_b[0],
0.5 * conf['cref'] / dae['airspeed'] * w_bn_b[1],
0.5 * conf['bref'] / dae['airspeed'] * w_bn_b[2]
])
momentCoeffs_pqr = C.mul(
C.vertcat([
C.horzcat([conf['cl_p'], conf['cl_q'], conf['cl_r']]),
C.horzcat([conf['cm_p'], conf['cm_q'], conf['cm_r']]),
C.horzcat([conf['cn_p'], conf['cn_q'], conf['cn_r']])
]), w_bn_b_hat)
dae['momentCoeffs_pqr'] = momentCoeffs_pqr
ocp.makeSemiExplicit()
ocp.eliminateIndependentParameters()
ocp.eliminateDependentParameters()
ocp.eliminateDependentParameterInterdependencies()
ocp.eliminateAlgebraic()
DT = 10 # for simulation and kalman filtering
scaleXModelica = ca.vertcat(
[ocp.variable(ocp.x[k].getName()).nominal for k in range(ocp.x.size())])
scaleZModelica = ca.vertcat(
[ocp.variable(ocp.z[k].getName()).nominal for k in range(ocp.z.size())])
scaleUModelica = ca.vertcat(
[ocp.variable(ocp.u[k].getName()).nominal for k in range(ocp.u.size())])
u = ca.DMatrix([1.2963, 1.3245, 70000, 200000, 200000])
daeModel = DaeModel.DaeModel()
daeModel.init(ocp, DT, measurementTagsModelica)
x0, z0, y_0 = daeModel.findSteadyState(
u, None, None, 0, consList=['net.w1.z1', 'net.w2.z1', 'net.p.z'])
uOpt, xOpt, zOpt, yOpt = daeModel.findOptimalPoint(
u,
x0=x0,
z0=z0,
simCount=0,
consList=['net.w1.z1', 'net.w2.z1', 'net.p.z'])
x0 = xOpt
## Start Simulator
openLoop = False
controlOlga = False
xh = []
uh = []
yh = []
yPh = []
simTime = []
Juh_NMPC = []
monitoredSimh = []
monitoredModelicah = []
Objh_NMPC = []
ObjTotalh = []
xh.append(ca.DMatrix(x0))
ULog = None
monitor = True
if monitor:
measurementsEst = ca.vertcat([ocp.variable(varName).v for varName in measurementsMonitorModelica])
measure_funcEst = ocp.beq(measurementsEst)
HEst = ca.SXFunction(ca.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t), [measure_funcEst])
HEst.init()
extendedKalman = True
KF.setState(x0, z0)
x_hat = KF.getState()
# z_hat= ca.vertcat([ocp.variable(ocp.z[k].getName()).start for k in range(ocp.z.size())])
# You should have received a copy of the GNU Lesser General Public License
# along with PECas. If not, see <http://www.gnu.org/licenses/>.
# This example is an adapted version of the system identification example
# included in CasADi, for the original file see:
# https://github.com/casadi/casadi/blob/master/docs/examples/python/sysid.py
import pylab as pl
import casadi as ca
import pecas
N = 10000
fs = 610.1
p_true = ca.DMatrix([5.625e-6, 2.3e-4, 1, 4.69])
p_guess = ca.DMatrix([5, 3, 1, 5])
scale = ca.vertcat([1e-6, 1e-4, 1, 1])
x = ca.MX.sym("x", 2)
u = ca.MX.sym("u", 1)
p = ca.MX.sym("p", 4)
f = ca.vertcat([
x[1], \
(u - scale[3] * p[3] * x[0]**3 - scale[2] * p[2] * x[0] - \
scale[1] * p[1] * x[1]) / (scale[0] * p[0]), \
])
phi = x
