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),