def setupfun(self, N):
s = Sparsity.diag(N)
s[-1, 0] = 1
H = MX.sym("H", s)
X = MX.sym("X", N, 1)
f = Function('f', [H, X], [c.prod(H, X)])
return {'f': f}
def setupfun(self, N):
s = Sparsity.diag(N)
s[-1, 0] = 1
H = MX.sym("H", s)
X = MX.sym("X", N, 1)
f = MXFunction("f", [H, X], [c.prod(H, X)])
return {"f": f}
def setupfun(self,N):
s = Sparsity.diag(N)
s[-1,0]=1
H = MX.sym("H",s)
X = MX.sym("X",N,1)
f = MXFunction([H,X],[c.prod(H,X)])
f.init()
return {'f':f}
def setupfun(self,N):
s = sp_diag(N)
s[-1,0]=1
H = MX("H",s)
X = MX("X",N,1)
f = MXFunction([H,X],[c.prod(H,X)])
f.init()
return {'f':f}
def setupfun(self,N):
s = Sparsity.diag(N)
s[-1,0]=1
H = MX.sym("H",s)
s = Sparsity.diag(N)
s[-1,0]=1
X = MX.sym("X",s)
f = Function('f', [H,X],[c.prod(H,X)])
return {'f':f}
def setupfun(self, N):
s = sp_diag(N)
s[-1, 0] = 1
H = MX("H", s)
s = sp_diag(N)
s[-1, 0] = 1
X = MX("X", s)
f = MXFunction([H, X], [c.prod(H, X)])
f.init()
return {'f': f}
def setupfun(self, N):
s = Sparsity.diag(N)
s[-1, 0] = 1
H = MX.sym("H", s)
s = Sparsity.diag(N)
s[-1, 0] = 1
X = MX.sym("X", s)
f = MXFunction([H, X], [c.prod(H, X)])
f.init()
return {'f': f}
def idSystem(makePlots=False):
# parameters
k = 5.0 # spring constant
b = 0.4 # viscous damping
# noise
Q = N.matrix( N.diag( [5e-3, 1e-2] )**2 )
R = N.matrix( N.diag( [0.03, 0.06] )**2 )
# observation function
def h(x):
yOut = x
return yOut
# Time length
T = 20.0
# Shooting length
nu = 150 # Number of control segments
DT = N.double(T)/nu
# Create integrator
#integrator = create_integrator_cvodes()
integrator = create_integrator_rk4()
# Initialize the integrator
integrator.init()
############# simulation
# initial state
s0 = 0 # initial position
v0 = 1 # initial speed
xNext = [s0,v0]
Utrue = []
Xtrue = [[s0,v0]]
Y = [[s0,v0]]
time = [0]
for j in range(nu):
u = N.sin(N.double(j)/10.0)
integrator.setInput( j*DT, C.INTEGRATOR_T0 )
integrator.setInput( (j+1)*DT, C.INTEGRATOR_TF )
integrator.setInput( [u, k, b], C.INTEGRATOR_P )
integrator.setInput( xNext, C.INTEGRATOR_X0 )
integrator.evaluate()
xNext = list(integrator.getOutput())
x = list(integrator.getOutput())
y = list(integrator.getOutput())
# state record
w = N.random.multivariate_normal([0,0], Q)
x[0] += w[0]
x[1] += w[1]
# measurement
v = N.random.multivariate_normal( [0,0], R )
y[0] += v[0]
y[1] += v[1]
Xtrue.append(x)
Y.append(y)
Utrue.append(u)
time.append(time[-1]+DT)
############## parameter estimation #################
# Integrate over all intervals
T0 = C.MX(0) # Beginning of time interval (changed from k*DT due to probable Sundials bug)
TF = C.MX(DT) # End of time interval (changed from (k+1)*DT due to probable Sundials bug)
design_variables = C.MX('design_variables', 2 + 2*(nu+1))
k_hat = design_variables[0]
b_hat = design_variables[1]
x_hats = []
for j in range(nu+1):
x_hats.append(design_variables[2+2*j:2+2*j+2])
logLiklihood = C.MX(0)
# logLiklihood += sensor error
for j in range(nu+1):
yj = C.MX(2,1)
yj[0,0] = C.MX(Y[j][0])
yj[1,0] = C.MX(Y[j][1])
xj = x_hats[j]
# ll = 1/2 * err^T * R^-1 * err
err = yj-h(xj)
ll = -C.MX(0.5)*C.prod( err.T, C.prod(C.MX(R.I), err) )
logLiklihood += ll
# logLiklihood += dynamics constraint
xdot = C.MX('xdot', 2,1)
for j in range(nu):
xj = x_hats[j]
Xnext = integrator( [T0, TF, xj, C.vertcat([Utrue[j], k_hat, b_hat]), xdot, C.MX()])
err = Xnext - x_hats[j+1]
ll = -C.MX(0.5)*C.prod( err.T, C.prod( C.MX(Q.I), err) )
logLiklihood += ll
# Objective function
F = -logLiklihood
# Terminal constraints
G = k_hat - C.MX(20.0)
# Create the NLP
ffcn = C.MXFunction([design_variables],[F]) # objective function
gfcn = C.MXFunction([design_variables],[G]) # constraint function
# Allocate an NLP solver
#solver = C.IpoptSolver(ffcn,gfcn)
solver = C.IpoptSolver(ffcn)
# Set options
solver.setOption("tol",1e-10)
solver.setOption("hessian_approximation","limited-memory");
solver.setOption("derivative_test","first-order")
# initialize the solver
solver.init()
# Bounds on u and initial condition
kb_min = [1, 0.1] + [-10 for j in range(2*(nu+1))] # lower bound
solver.setInput(kb_min, C.NLP_SOLVER_LBX)
kb_max = [10, 1] + [10 for j in range(2*(nu+1))] # upper bound
solver.setInput(kb_max, C.NLP_SOLVER_UBX)
guess = []
for y in Y:
guess.append(y[0])
guess.append(y[1])
kb_sol = [5, 0.4] + guess # initial guess
solver.setInput(kb_sol, C.NLP_SOLVER_X0)
# Bounds on g
#Gmin = Gmax = [] #[10, 0]
#solver.setInput(Gmin,C.NLP_SOLVER_LBG)
#solver.setInput(Gmax,C.NLP_SOLVER_UBG)
# Solve the problem
solver.solve()
# Get the solution
xopt = solver.getOutput(C.NLP_SOLVER_X)
# estimated parameters:
print ""
print "(estimated, real) k = ("+str(k)+", "+str(xopt[0])+"),\t"+str(100.0*(k-xopt[0])/k)+" % error"
print "(estimated, real) b = ("+str(b)+", "+str(xopt[1])+"),\t"+str(100.0*(b-xopt[1])/b)+" % error"
# estimated state:
s_est = []
v_est = []
for j in range(0, nu+1 ):
s_est.append(xopt[2+2*j])
v_est.append(xopt[2+2*j+1])
# make plots
if makePlots:
plt.figure()
plt.clf()
plt.subplot(2,1,1)
plt.xlabel('time')
plt.ylabel('position')
plt.plot(time, [x[0] for x in Xtrue])
plt.plot(time, [y[0] for y in Y], '.')
plt.plot(time, s_est)
plt.legend(['true','meas','est'])
plt.subplot(2,1,2)
plt.xlabel('time')
plt.ylabel('velocity')
plt.plot(time, [x[1] for x in Xtrue])
plt.plot(time, [y[1] for y in Y], '.')
plt.plot(time, v_est)
plt.legend(['true','meas','est'])
# plt.subplot(3,1,3)
# plt.xlabel('time')
# plt.ylabel('control input')
# plt.plot(time[:-1], Utrue, '.')
plt.show()
return {'k':xopt[0], 'b':xopt[1]}
def setupfun(self, N):
G = MX.sym("G", N, 1)
X = MX.sym("X", N, 1)
f = MXFunction("f", [G, X], [c.prod(G.T, X)])
return {"f": f}
def setupfun(self,N):
G = MX.sym("G",N,1)
X = MX.sym("X",N,1)
f = Function('f', [G,X],[c.prod(G.T,X)])
return {'f':f}
def setupfun(self,N):
G = MX.sym("G",N,1)
X = MX.sym("X",N,1)
f = MXFunction([G,X],[c.prod(G.T,X)])
f.init()
return {'f':f}
def setupfun(self, N):
G = MX.sym("G", N, 1)
X = MX.sym("X", N, 1)
f = Function('f', [G, X], [c.prod(G.T, X)])
return {'f': f}
def idSystem(makePlots=False):
# parameters
k = 5.0 # spring constant
b = 0.4 # viscous damping
# noise
Q = N.matrix(N.diag([5e-3, 1e-2])**2)
R = N.matrix(N.diag([0.03, 0.06])**2)
# observation function
def h(x):
yOut = x
return yOut
# Time length
T = 20.0
# Shooting length
nu = 150 # Number of control segments
DT = N.double(T) / nu
# Create integrator
#integrator = create_integrator_cvodes()
integrator = create_integrator_rk4()
# Initialize the integrator
integrator.init()
############# simulation
# initial state
s0 = 0 # initial position
v0 = 1 # initial speed
xNext = [s0, v0]
Utrue = []
Xtrue = [[s0, v0]]
Y = [[s0, v0]]
time = [0]
for j in range(nu):
u = N.sin(N.double(j) / 10.0)
integrator.setInput(j * DT, C.INTEGRATOR_T0)
integrator.setInput((j + 1) * DT, C.INTEGRATOR_TF)
integrator.setInput([u, k, b], C.INTEGRATOR_P)
integrator.setInput(xNext, C.INTEGRATOR_X0)
integrator.evaluate()
xNext = list(integrator.getOutput())
x = list(integrator.getOutput())
y = list(integrator.getOutput())
# state record
w = N.random.multivariate_normal([0, 0], Q)
x[0] += w[0]
x[1] += w[1]
# measurement
v = N.random.multivariate_normal([0, 0], R)
y[0] += v[0]
y[1] += v[1]
Xtrue.append(x)
Y.append(y)
Utrue.append(u)
time.append(time[-1] + DT)
############## parameter estimation #################
# Integrate over all intervals
T0 = C.MX(
0
) # Beginning of time interval (changed from k*DT due to probable Sundials bug)
TF = C.MX(
DT
) # End of time interval (changed from (k+1)*DT due to probable Sundials bug)
design_variables = C.MX('design_variables', 2 + 2 * (nu + 1))
k_hat = design_variables[0]
b_hat = design_variables[1]
x_hats = []
for j in range(nu + 1):
x_hats.append(design_variables[2 + 2 * j:2 + 2 * j + 2])
logLiklihood = C.MX(0)
# logLiklihood += sensor error
for j in range(nu + 1):
yj = C.MX(2, 1)
yj[0, 0] = C.MX(Y[j][0])
yj[1, 0] = C.MX(Y[j][1])
xj = x_hats[j]
# ll = 1/2 * err^T * R^-1 * err
err = yj - h(xj)
ll = -C.MX(0.5) * C.prod(err.T, C.prod(C.MX(R.I), err))
logLiklihood += ll
# logLiklihood += dynamics constraint
xdot = C.MX('xdot', 2, 1)
for j in range(nu):
xj = x_hats[j]
Xnext = integrator(
[T0, TF, xj,
C.vertcat([Utrue[j], k_hat, b_hat]), xdot,
C.MX()])
err = Xnext - x_hats[j + 1]
ll = -C.MX(0.5) * C.prod(err.T, C.prod(C.MX(Q.I), err))
logLiklihood += ll
# Objective function
F = -logLiklihood
# Terminal constraints
G = k_hat - C.MX(20.0)
# Create the NLP
ffcn = C.MXFunction([design_variables], [F]) # objective function
gfcn = C.MXFunction([design_variables], [G]) # constraint function
# Allocate an NLP solver
#solver = C.IpoptSolver(ffcn,gfcn)
solver = C.IpoptSolver(ffcn)
# Set options
solver.setOption("tol", 1e-10)
solver.setOption("hessian_approximation", "limited-memory")
solver.setOption("derivative_test", "first-order")
# initialize the solver
solver.init()
# Bounds on u and initial condition
kb_min = [1, 0.1] + [-10 for j in range(2 * (nu + 1))] # lower bound
solver.setInput(kb_min, C.NLP_SOLVER_LBX)
kb_max = [10, 1] + [10 for j in range(2 * (nu + 1))] # upper bound
solver.setInput(kb_max, C.NLP_SOLVER_UBX)
guess = []
for y in Y:
guess.append(y[0])
guess.append(y[1])
kb_sol = [5, 0.4] + guess # initial guess
solver.setInput(kb_sol, C.NLP_SOLVER_X0)
# Bounds on g
#Gmin = Gmax = [] #[10, 0]
#solver.setInput(Gmin,C.NLP_SOLVER_LBG)
#solver.setInput(Gmax,C.NLP_SOLVER_UBG)
# Solve the problem
solver.solve()
# Get the solution
xopt = solver.getOutput(C.NLP_SOLVER_X)
# estimated parameters:
print ""
print "(estimated, real) k = (" + str(k) + ", " + str(
xopt[0]) + "),\t" + str(100.0 * (k - xopt[0]) / k) + " % error"
print "(estimated, real) b = (" + str(b) + ", " + str(
xopt[1]) + "),\t" + str(100.0 * (b - xopt[1]) / b) + " % error"
# estimated state:
s_est = []
v_est = []
for j in range(0, nu + 1):
s_est.append(xopt[2 + 2 * j])
v_est.append(xopt[2 + 2 * j + 1])
# make plots
if makePlots:
plt.figure()
plt.clf()
plt.subplot(2, 1, 1)
plt.xlabel('time')
plt.ylabel('position')
plt.plot(time, [x[0] for x in Xtrue])
plt.plot(time, [y[0] for y in Y], '.')
plt.plot(time, s_est)
plt.legend(['true', 'meas', 'est'])
plt.subplot(2, 1, 2)
plt.xlabel('time')
plt.ylabel('velocity')
plt.plot(time, [x[1] for x in Xtrue])
plt.plot(time, [y[1] for y in Y], '.')
plt.plot(time, v_est)
plt.legend(['true', 'meas', 'est'])
# plt.subplot(3,1,3)
# plt.xlabel('time')
# plt.ylabel('control input')
# plt.plot(time[:-1], Utrue, '.')
plt.show()
return {'k': xopt[0], 'b': xopt[1]}
def setupfun(self, N):
G = MX.sym("G", N, 1)
X = MX.sym("X", N, 1)
f = MXFunction([G, X], [c.prod(G.T, X)])
f.init()
return {'f': f}
dy = repmat(y,1,N)-repmat(y.T,N,1)
# distance^2 matrix
N_ = dx**2+dy**2
# Denominator of Coulomb force
D = N_**(-3/2.0)
# k-indices to get diagonal elements
diagonal_k = list(getNZDense(sp_diag(N)))
# No self-interaction
D[diagonal_k]=MX(0)
# Summing all force contributions
n = MX.ones(N,1)
Fx_ = c.prod((D*dx),n)
Fy_ = c.prod((D*dy),n)
# Projecting the forces on the local tangents
F = Fx_*tx+Fy_*ty
f = MXFunction([phi],[F])
f.init()
J = Jacobian(f)
J.setOption("ad_mode","forward")
J.init()
print "duration: %f [s]" % (time()-t0)
# initial guess: evenly spaced
x_ = array(numpy.linspace(0,2*pi*(1-1.0/N),N),ndmin=2).T # decision variables
dy = repmat(y, 1, N) - repmat(y.T, N, 1)
# distance^2 matrix
N_ = dx**2 + dy**2
# Denominator of Coulomb force
D = N_**(-3 / 2.0)
# k-indices to get diagonal elements
diagonal_k = list(getNZDense(sp_diag(N)))
# No self-interaction
D[diagonal_k] = MX(0)
# Summing all force contributions
n = MX.ones(N, 1)
Fx_ = c.prod((D * dx), n)
Fy_ = c.prod((D * dy), n)
# Projecting the forces on the local tangents
F = Fx_ * tx + Fy_ * ty
f = MXFunction([phi], [F])
f.init()
J = Jacobian(f)
J.setOption("ad_mode", "forward")
J.init()
print "duration: %f [s]" % (time() - t0)
# initial guess: evenly spaced
x_ = array(numpy.linspace(0, 2 * pi * (1 - 1.0 / N), N),
