def test_tau_max_from_actuators(value, threshold): ocp = prepare_test_ocp(with_actuator=True) x = [DM.zeros((6, 1)), DM.zeros((6, 1))] u = [DM.ones((3, 1)) * value, DM.ones((3, 1)) * value] penalty_type = ConstraintFcn.TORQUE_MAX_FROM_ACTUATORS penalty = Constraint(penalty_type) if threshold and threshold < 0: with pytest.raises( ValueError, match="min_torque cannot be negative in tau_max_from_actuators" ): penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []), min_torque=threshold), else: penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []), min_torque=threshold) val = [] for i in range(len(ocp.nlp[0].g[0])): val.append(ocp.nlp[0].g[0][i]["val"]) for res in val: if threshold: np.testing.assert_almost_equal( res, np.repeat([value + threshold, value - threshold], 3)[:, np.newaxis]) else: np.testing.assert_almost_equal( res, np.repeat([value + 5, value - 10], 3)[:, np.newaxis])
def create_quadratic_cost(self, par_dict): self.L = DM(0) self.V = DM(0) if 'x_ref' not in par_dict: par_dict['x_ref'] = DM.zeros(self.model.n_x) if 'u_ref' not in par_dict: par_dict['u_ref'] = DM.zeros(self.model.n_u) if 'Q' in par_dict: self.L += mtimes( mtimes((self.model.x - par_dict['x_ref']).T, par_dict['Q']), (self.model.x - par_dict['x_ref'])) if 'R' in par_dict: self.L += mtimes( mtimes((self.model.u - par_dict['u_ref']).T, par_dict['R']), (self.model.u - par_dict['u_ref'])) if 'Qv' in par_dict: self.V += mtimes( mtimes((self.model.x - par_dict['x_ref']).T, par_dict['Qv']), (self.model.x - par_dict['x_ref'])) if 'Rv' in par_dict: self.V += mtimes(mtimes(self.model.x.T, par_dict['Rv']), self.model.x)
def test_tau_max_from_actuators(value, threshold): ocp = prepare_test_ocp(with_actuator=True) t = [0] x = [DM.zeros((6, 1)), DM.zeros((6, 1))] u = [DM.ones((3, 1)) * value, DM.ones((3, 1)) * value] penalty_type = ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT penalty = Constraint(penalty_type, min_torque=threshold) if threshold and threshold < 0: with pytest.raises( ValueError, match="min_torque cannot be negative in tau_max_from_actuators" ): get_penalty_value(ocp, penalty, t, x, u, []) return else: res = get_penalty_value(ocp, penalty, t, x, u, []) if threshold: np.testing.assert_almost_equal( res, np.repeat([value + threshold, value - threshold], 3)[:, np.newaxis]) else: np.testing.assert_almost_equal( res, np.repeat([value + 5, value - 10], 3)[:, np.newaxis])
def _sample_parameters(self): n_samples = self.n_samples n_uncertain = self.n_uncertain mean = vertcat(self.socp.p_unc_mean, self.socp.uncertain_initial_conditions_mean) if self.socp.n_p_unc > 0 and self.socp.n_uncertain_initial_condition > 0: covariance = diagcat(self.socp.p_unc_cov, self.socp.uncertain_initial_conditions_cov) elif self.socp.n_p_unc > 0: covariance = self.socp.p_unc_cov elif self.socp.n_uncertain_initial_condition > 0: covariance = self.socp.uncertain_initial_conditions_cov else: raise ValueError("No uncertainties found n_p_unc = {}, " "n_uncertain_initial_condition={}".format( self.socp.n_p_unc, self.socp.n_uncertain_initial_condition)) dist = self.socp.p_unc_dist + self.socp.uncertain_initial_conditions_distribution for d in dist: if not d == 'normal': raise NotImplementedError( 'Distribution "{}" not implemented, only "normal" is available.' .format(d)) sampled_epsilon = sample_parameter_normal_distribution_with_sobol( DM.zeros(mean.shape), DM.eye(covariance.shape[0]), n_samples) sampled_parameters = SX.zeros(n_uncertain, n_samples) for s in range(n_samples): sampled_parameters[:, s] = mean + mtimes(sampled_epsilon[:, s].T, chol(covariance)).T return sampled_parameters
def sample_parameter_normal_distribution_with_sobol(mean, covariance, n_samples=1): """Sample parameter using Sobol sampling with a normal distribution. :param mean: :param covariance: :param n_samples: :return: """ if isinstance(mean, list): mean = vertcat(*mean) n_uncertain = mean.size1() # Uncertain parameter design sobol_design = sobol_seq.i4_sobol_generate(n_uncertain, n_samples, math.ceil(np.log2(n_samples))) sobol_samples = DM(sobol_design.T) for i in range(n_uncertain): sobol_samples[i, :] = norm(loc=0., scale=1).ppf(sobol_samples[i, :]) unscaled_sample = DM.zeros(n_uncertain, n_samples) for i in range(n_samples): unscaled_sample[:, i] = mean + mtimes( chol(covariance).T, sobol_samples[:, i]) return unscaled_sample
def blockdiag(*matrices_list): """Receives a list of matrices and return a block diagonal. :param DM|MX|SX matrices_list: list of matrices """ size_1 = sum([m.size1() for m in matrices_list]) size_2 = sum([m.size2() for m in matrices_list]) matrix_types = [type(m) for m in matrices_list] if SX in matrix_types and MX in matrix_types: raise ValueError( "Can not mix MX and SX types. Types give: {}".format(matrix_types)) if SX in matrix_types: matrix = SX.zeros(size_1, size_2) elif MX in matrix_types: matrix = MX.zeros(size_1, size_2) else: matrix = DM.zeros(size_1, size_2) index_1 = 0 index_2 = 0 for m in matrices_list: matrix[index_1:index_1 + m.size1(), index_2:index_2 + m.size2()] = m index_1 += m.size1() index_2 += m.size2() return matrix
def include_algebraic(self, var, alg=None, y_min=None, y_max=None, y_guess=None): if y_min is None: y_min = -DM.inf(var.numel()) if y_max is None: y_max = DM.inf(var.numel()) if isinstance(y_min, list): y_min = vertcat(*y_min) if isinstance(y_max, list): y_max = vertcat(*y_max) if not var.numel() == y_min.numel(): raise ValueError( "Given 'var' and 'y_min' does not have the same size, {}!={}". format(var.numel(), y_min.numel())) if not var.numel() == y_max.numel(): raise ValueError( "Given 'var' and 'y_max' does not have the same size, {}!={}". format(var.numel(), y_max.numel())) if self.y_guess is not None: if y_guess is None: self.y_guess = vertcat(self.y_guess, DM.zeros(var.numel())) else: self.y_guess = vertcat(self.y_guess, y_guess) self.model.include_algebraic(var, alg) self.y_min = vertcat(self.y_min, y_min) self.y_max = vertcat(self.y_max, y_max)
def create_initial_guess(self, p=None, theta=None): """Create an initial guess for the optimal control problem using problem.x_0, problem.y_guess, problem.u_guess, and a given p and theta (for p_opt and theta_opt) if they are given. If y_guess or u_guess are None the initial guess uses a vector of zeros of appropriate size. :param p: Optimization parameters :param theta: Optimization theta :return: """ x_init = repmat(self.problem.x_0, (self.degree + 1) * self.finite_elements) if self.problem.y_guess is not None: y_init = repmat(self.problem.y_guess, self.degree * self.finite_elements) else: y_init = repmat(DM.zeros(self.model.n_y), self.degree * self.finite_elements) if self.model.n_u_par > 0: if self.problem.u_guess is not None: u_init = repmat(self.problem.u_guess, self.degree_control * self.finite_elements) else: u_init = repmat(DM.zeros(self.model.n_u), self.degree_control * self.finite_elements) else: u_init = [] eta_init = DM.zeros(self.problem.n_eta, 1) p_opt_init = DM.zeros(self.problem.n_p_opt, 1) theta_opt_init = DM.zeros( self.problem.n_theta_opt * self.finite_elements, 1) if p is not None: for k, ind in enumerate(self.problem.get_p_opt_indices()): p_opt_init[k] = p[ind] if theta is not None: for el in range(self.finite_elements): for k, ind in enumerate(self.problem.get_theta_opt_indices()): theta_opt_init[k + el * self.problem.n_theta_opt] = theta[el][ind] return vertcat(x_init, y_init, u_init, eta_init, p_opt_init, theta_opt_init)
def include_time_chance_inequality(self, ineq, prob, rhs=None, when='default'): r"""Include time dependent chance inequality. Prob[ineq(..., t) <= rhs] <= prob, for t \in [t_0, t_f] The inequality is concatenated to "g_stochastic_ineq" :param ineq: inequality :param list|DM rhs: Right-hand size of the inequality :param list|DM prob: Chance/probability of the constraint being satisfied :param str when: Can be 'default', 'end', 'start'. 'start' - the constraint will be evaluated at the start of every finite element 'end' - the constraint will be evaluated at the end of every finite element 'default' - will be evaluated at each collocation point of every finite element. For the multiple shooting, the constraint will be evaluated at the end of each finite element """ if isinstance(ineq, list): ineq = vertcat(*ineq) if isinstance(rhs, list): rhs = vertcat(*rhs) if isinstance(prob, list): prob = vertcat(*prob) if isinstance(rhs, (float, int)): rhs = vertcat(rhs) if isinstance(prob, (float, int)): prob = vertcat(prob) if rhs is None: rhs = DM.zeros(ineq.shape) if not ineq.numel() == rhs.numel(): raise ValueError( 'Given inequality does not have the same dimensions of the provided "rhs". ' 'Size of ineq: {}, size of rhs: {}'.format( ineq.shape, rhs.shape)) if not ineq.numel() == rhs.numel(): raise ValueError( 'Given inequality does not have the same dimensions of the provided "prob". ' 'Size of ineq: {}, size of prob: {}'.format( ineq.shape, prob.shape)) self.g_stochastic_ineq = vertcat(self.g_stochastic_ineq, ineq) self.g_stochastic_rhs = vertcat(self.g_stochastic_rhs, rhs) self.g_stochastic_prob = vertcat(self.g_stochastic_prob, prob)
def include_state(self, var, ode=None, x_0=None, x_min=None, x_max=None, h_initial=None, x_0_sym=None, suppress=False): if x_0 is not None: x_0 = vertcat(x_0) if x_min is None: x_min = -DM.inf(var.numel()) if x_max is None: x_max = DM.inf(var.numel()) if x_0 is None and h_initial is None and not suppress: raise Exception('No initial condition given') var = vertcat(var) x_min = vertcat(x_min) x_max = vertcat(x_max) if not var.numel() == x_max.numel(): raise ValueError('Size of "x" and "x_max" differ. x.numel()={} ' 'and x_max.numel()={}'.format( var.numel(), x_max.numel())) if not var.numel() == x_min.numel(): raise ValueError('Size of "x" and "x_min" differ. x.numel()={} ' 'and x_min.numel()={}'.format( var.numel(), x_min.numel())) if not var.numel() == x_min.numel(): raise ValueError('Size of "x" and "x_0" differ. x.numel()={} ' 'and x_0.numel()={}'.format( var.numel(), x_0.numel())) x_0_sym = self.model.include_state(var, ode, x_0_sym) if x_0 is not None: self.x_0 = vertcat(self.x_0, x_0) h_initial = x_0_sym - var else: x_0 = DM.zeros(var.shape) self.x_0 = vertcat(self.x_0, x_0) if h_initial is not None: self.h_initial = vertcat(self.h_initial, h_initial) self.x_min = vertcat(self.x_min, x_min) self.x_max = vertcat(self.x_max, x_max)
def get_states(self, t_k, y_k, u_k): """Get states out of a measurement. :param DM t_k: time of the measurement :param DM y_k: measurements :param DM u_k: controls :return: DM """ if self.estimator is None: x_k = y_k[:self.solution_method.model.n_x] cov_x_k = DM.zeros(x_k.shape) else: x_k, cov_x_k = self.estimator.estimate(t_k, y_k, u_k) if self.include_cost_in_state_vector: x_k = vertcat(x_k, 0) return x_k, cov_x_k
def calculate_optimal_control(self): dd_h_dudu, d_h_du = hessian(self.problem.H, self.model.u) if is_equal(dd_h_dudu, DM.zeros(self.model.n_u, self.model.n_u)): # TODO: Implement the case where the controls are linear on the Hamiltonian ("Bang-Bang" control) raise Exception( 'The Hamiltonian "H" is not strictly convex with respect to the control "u". ' + 'The obtained hessian d^2 H/du^2 = 0') # if not ddH_dudu.is_constant(): # raise NotImplementedError('The Hessian of the Hamiltonian with respect to "u" is not constant, # this case has not been implemented') u_opt = -mtimes(inv(dd_h_dudu), substitute(d_h_du, self.model.u, 0)) for i in range(self.model.n_u): if not self.problem.u_min[i] == -inf: u_opt[i] = fmax(u_opt[i], self.problem.u_min[i]) if not self.problem.u_max[i] == inf: u_opt[i] = fmin(u_opt[i], self.problem.u_max[i]) return u_opt
def runge_kutta_4th_order( x0=DM.zeros(n_states, 1), p=None, n_iter=iterations): if n_iter < 1: raise Exception( "The given number of Runge Kutta iterations is less than one, given {}" .format(n_iter)) if p is None: p = [] x_f = x0 h = (t_f - t_0) / n_iter t = t_0 for it in range(n_iter): k1 = h * f(t, x0, p) k2 = h * f(t + 0.5 * h, x0 + 0.5 * k1, p) k3 = h * f(t + 0.5 * h, x0 + 0.5 * k2, p) k4 = h * f(t + h, x0 + k3, p) x_f = x0 + 1 / 6. * k1 + 1 / 3. * k2 + 1 / 3. * k3 + 1 / 6. * k4 x0 = x_f t += h return {'xf': x_f, 'zf': DM([])}
def include_equality(self, expr, rhs=None): """Include a equality with the following form expr = rhs :param expr: expression, this is the only term that should contain symbolic variables :param rhs: right hand side, by default it is a vector of zeros with same size of expr. If the 'expr' size is greater than one but a scalar is passed as 'rhs', a vector of 'rhs' with size of 'expr' will be used as right hand side. (default = [0]*size) """ if isinstance(expr, list): expr = vertcat(expr) if expr.size2() > 1: raise Exception( "Given expression is not a vector, number of columns is {}". format(expr.size2())) if rhs is None: rhs = DM.zeros(expr.shape) else: rhs = vertcat(rhs) if rhs.numel() == 1 and expr.numel() > 1: rhs = repmat(rhs, expr.numel()) if not expr.shape == rhs.shape: msg = "Expression and the right hand side does not have the same size: " \ "expr.shape={}, rhs.shape=={}".format(expr.shape, rhs.shape) raise ValueError(msg) # check for if rhs have 'x's and 'p's if depends_on(rhs, vertcat(self.x, self.p)): raise ValueError( "Right-hand side cannot contain variables from the optimization problem. " "RHS = {}".format(rhs)) self.g = vertcat(self.g, expr) self.g_lb = vertcat(self.g_lb, rhs) self.g_ub = vertcat(self.g_ub, rhs)
def custom_with_bounds(ocp, nlp, t, x, u, p): my_values = DM.zeros((12, 1)) + x[0] return -10, my_values, 10
def create_initial_guess_with_simulation(self, u=None, p=None, theta=None): """Create an initial guess for the optimal control problem using by simulating with a given control u, and a given p and theta (for p_opt and theta_opt) if they are given. If no u is given the value of problem.u_guess is used, or problem.last_u, then a vector of zeros of appropriate size is used. If no p or theta is given, an vector of zeros o appropriate size is used. :param u: Control initial guess :param p: Optimization parameters :param theta: Optimization theta :return: """ x_init = [] y_init = [] u_init = [] # Simulation if u is None: if self.problem.u_guess is not None: u = self.problem.u_guess elif self.problem.last_u is not None: u = self.problem.last_u else: u = DM.zeros(self.model.n_u) if self.model.n_u_par > 0: u = vec(horzcat(*[u] * self.degree_control)) else: u = [] x_0 = self.problem.x_0 y_guess = self.problem.y_guess x_init.append([x_0]) for el in range(self.finite_elements): el_x = [] el_y = [] # get DAE system and remove tau dae_sys = self.model.get_dae_system() dae_sys.convert_from_tau_to_time(t_k=self.time_breakpoints[el], t_kp1=self.time_breakpoints[el + 1]) # Prepare for loop t_init = self.time_breakpoints[el] p_el = vertcat(p, theta[el], u) for t in [self.time_breakpoints[el + 1]]: res = dae_sys.simulate(x_0=x_0, t_0=t_init, t_f=t, p=p_el, y_0=y_guess) el_x.append(res['xf']) el_y.append(res['zf']) t_init = t x_0 = res['xf'] x_init.append(el_x) y_init.append(el_y) u_init.append(u) x_init = self.vectorize(x_init) y_init = self.vectorize(y_init) u_init = self.vectorize(u_init) # Other variables eta_init = DM.zeros(self.problem.n_eta, 1) p_opt_init = DM.zeros(self.problem.n_p_opt, 1) theta_opt_init = DM.zeros( self.problem.n_theta_opt * self.finite_elements, 1) if p is not None: for k, ind in enumerate(self.problem.get_p_opt_indices()): p_opt_init[k] = p[ind] if theta is not None: for el in range(self.finite_elements): for k, ind in enumerate(self.problem.get_theta_opt_indices()): theta_opt_init[k + el * self.problem.n_theta_opt] = theta[el][ind] return vertcat(x_init, y_init, u_init, eta_init, p_opt_init, theta_opt_init)
def __init__(self, model, x_0, **kwargs): """ Plant which uses a SystemModel.simulate to obtain the measurements. :param SystemModel model: simulation model :param DM x_0: initial condition :param DM t_s: (default: 1) sampling time :param DM u: (default: 0) initial control :param DM y_guess: initial guess for algebraic variables for simulation :param DM t_0: (default: 0) initial time :param dict integrator_options: integrator options :param bool has_noise: Turn on/off the process/measurement noise :param DM r_n: Measurement noise covariance matrix :param DM r_v: Process noise covariance matrix :param noise_seed: Seed for the random number generator used to create noise. Use the same seed for the repeatability in the experiments. """ Plant.__init__(self) self.model = model self.name = self.model.name self.x = x_0 self.u = DM.zeros(self.model.n_u) self.y_guess = None self.t = 0. self.t_s = 1. self.c_matrix = None self.d_matrix = None self.p = None self.theta = None # Noise self.has_noise = False self.r_n = DM(0.) self.r_v = DM(0.) self.noise_seed = None # Options self.integrator_options = None self.verbosity = 1 self.dataset = DataSet(name='Plant') if 't_0' in kwargs: self.t = kwargs['t_0'] for (k, v) in kwargs.items(): setattr(self, k, v) if self.c_matrix is None: self.c_matrix = DM.eye(self.model.n_x + self.model.n_y) if self.d_matrix is None: self.d_matrix = DM(0.) self._initialize_dataset() if self.has_noise: if self.noise_seed is not None: numpy.random.seed(self.noise_seed)
def custom_with_bounds(pn): my_values = DM.zeros((12, 1)) + x[0] return -10, my_values, 10
def custom(pn, mult): my_values = DM.zeros((12, 1)) + pn.x[0] * mult return my_values
def call_solver(self, initial_guess=None, p=None, theta=None, x_0=None, last_u=None, initial_guess_dict=None): if self.opt_problem is None: self.create_optimization_problem() # initial conditions if x_0 is None: x_0 = self.problem.x_0 if isinstance(x_0, list): x_0 = vertcat(x_0) if not vertcat(x_0).numel() == self.model.n_x: raise Exception( 'Size of given x_0 (or obtained from problem.x_0) is different from model.n_x, ' 'x_0.numel() = {}, model.n_x = {}'.format( vertcat(x_0).numel(), self.model.n_x)) # parameters if p is None: if self.problem.n_p_opt == self.model.n_p: p = DM.zeros(self.problem.n_p_opt) elif self.problem.model.n_p > 0: raise Exception( "A parameter 'p' of size {} should be given".format( self.problem.model.n_p)) if isinstance(p, list): p = DM(p) # theta if theta is None: if self.problem.n_theta_opt == self.model.n_theta: theta = create_constant_theta(0, self.problem.n_theta_opt, self.finite_elements) elif self.problem.model.n_theta > 0: raise Exception( "A parameter 'theta' of size {} should be given".format( self.problem.model.n_theta)) # Prepare NLP parameter vector theta_vector, par_x_0, par_last_u = [], [], [] if theta is not None: theta_vector = vertcat( *[theta[i] for i in range(self.finite_elements)]) if self.initial_condition_as_parameter: par_x_0 = x_0 # last control if not self.last_control_as_parameter and last_u is not None: raise warnings.warn( 'solution_method.last_control_as_parameter is False, but last_u was passed. last_u will' ' be ignored.') else: if last_u is not None: if isinstance(last_u, list): last_u = vertcat(*last_u) elif self.problem.last_u is not None: last_u = self.problem.last_u elif self.last_control_as_parameter and last_u is None: raise Exception( 'last_control_as_parameter is True, but no "last_u" was passed and the "ocp.last_u" is ' 'None.') if self.last_control_as_parameter: par_last_u = last_u par = vertcat(p, theta_vector, par_x_0, par_last_u) if initial_guess_dict is None: if initial_guess is None: if self.initial_guess_heuristic == 'simulation': initial_guess = self.discretizer.create_initial_guess_with_simulation( p=p, theta=theta) elif self.initial_guess_heuristic == 'problem_info': initial_guess = self.discretizer.create_initial_guess( p, theta) else: raise ValueError( 'initial_guess_heuristic did not recognized, available options: "simulation" and ' '"problem_info". Given: {}'.format( self.initial_guess_heuristic)) args = dict(initial_guess=initial_guess, p=par) else: args = dict(initial_guess=initial_guess_dict['x'], p=par, lam_x=initial_guess_dict['lam_x'], lam_g=initial_guess_dict['lam_g']) sol = self.opt_problem.solve(**args) return sol, p, theta, x_0, last_u
nl['lbg'][:3 * (N_pred + 1):3] = [h_lb] * (N_pred + 1) nl['ubg'][:3 * (N_pred + 1):3] = [h_ub] * (N_pred + 1) nl['lbg'][1:3 * (N_pred + 1):3] = [-inf] * (N_pred + 1) nl['ubg'][1:3 * (N_pred + 1):3] = [inf] * (N_pred + 1) nl['lbg'][2:3 * (N_pred + 1):3] = [eta_lb] * (N_pred + 1) nl['ubg'][2:3 * (N_pred + 1):3] = [eta_ub] * (N_pred + 1) nl['lbg'][-1] = -inf nl['ubg'][-1] = 500 print(nl['ubg']) nl['p'] = [0, 0, 49, 0.9, 0, 48] nl['x0'] = [0 for i in range(N_pred)] sol = solver(**nl) temp_xn = DM.zeros(3, 1) temp_xn[0, 0] = sol['g'][3 * (N_pred)] temp_xn[1, 0] = sol['g'][3 * (N_pred) + 1] temp_xn[2, 0] = sol['g'][3 * (N_pred) + 2] print(temp_xn) print(temp_xn - nl['p'][3:6]) print((temp_xn - nl['p'][3:6]).T @ P_f @ (temp_xn - nl['p'][3:6])) print(sol['g'][3 * (N_pred + 1)]) # print(type(float((sol['g'][2])))) print(sol) # print(sol['x'].shape) eta_out = [] h_out = [] t = [T * i for i in range(N_pred)]
def _priori_update(self, x_mean, x_cov, u, p, theta): x_samples = self._get_sampled_states(x_mean, x_cov) # Perform predictions via simulation simulation_results = [] x_cal_x_k_at_k_minus_1 = [] y_alg_cal_x_k_at_k_minus_1 = [] for s in range(self.n_samples): x_0_i = DM(x_samples[s]) simulation_results_i = self.model.simulate(x_0=x_0_i, t_0=self.t, t_f=self.t + self.t_s, u=u, p=p, theta=theta, y_0=self.y_guess) simulation_results.append(simulation_results_i) x_cal_x_k_at_k_minus_1.append( simulation_results_i.final_condition()[0]) y_alg_cal_x_k_at_k_minus_1.append( simulation_results[s].final_condition()[1]) # fit the polynomial for x a_x = [] x_hat_k_minus = [] for i in range(self.model.n_x): x_i_vector = vertcat( *[x_cal_x_k_at_k_minus_1[s][i] for s in range(self.n_samples)]) a_x.append(mtimes(self._ls_factor, x_i_vector)) # get the mean for x for i in range(self.model.n_x): x_hat_k_minus.append(a_x[i][0]) x_hat_k_minus = vertcat(*x_hat_k_minus) # get the covariance for x p_k_minus = DM.zeros(self.model.n_x, self.model.n_x) for i in range(self.model.n_x): for j in range(self.model.n_x): p_k_minus[i, j] = sum( [a_x[i][k] * a_x[j][k] for k in range(1, self.n_samples)]) + self.r_v[i, j] # calculate the measurement for each sample y_cal_k_at_k_minus_1 = [] for s in range(self.n_samples): y_cal_k_at_k_minus_1.append( self._get_measurement_from_prediction( x_cal_x_k_at_k_minus_1[s], y_alg_cal_x_k_at_k_minus_1[s], u)) n_meas = y_cal_k_at_k_minus_1[0].numel() # find the measurements estimate a_meas = [] y_meas_hat_k_minus = [] for i in range(n_meas): y_meas_i_vector = vertcat( *[y_cal_k_at_k_minus_1[s][i] for s in range(self.n_samples)]) a_meas.append(mtimes(self._ls_factor, y_meas_i_vector)) # get the mean for the measurement for i in range(n_meas): y_meas_hat_k_minus.append(a_meas[i][0]) y_meas_hat_k_minus = vertcat(*y_meas_hat_k_minus) # get the covariance for the meas p_yk_yk = DM.zeros(n_meas, n_meas) for i in range(n_meas): for j in range(n_meas): p_yk_yk[i, j] = sum([ a_meas[i][k] * a_meas[j][k] for k in range(1, self.n_samples) ]) p_yk_yk = p_yk_yk + self.r_n # get cross-covariance p_xk_yk = DM.zeros(self.model.n_x, n_meas) for i in range(self.model.n_x): for j in range(n_meas): p_xk_yk[i, j] = sum([ a_x[i][k] * a_meas[j][k] for k in range(1, self.n_samples) ]) # k_gain = mtimes(p_xk_yk, inv(p_yk_yk)) k_gain = solve(p_yk_yk.T, p_xk_yk.T).T return x_hat_k_minus, p_k_minus, y_meas_hat_k_minus, p_yk_yk, k_gain
def custom_no_mult(ocp, nlp, t, x, u, p): my_values = DM.zeros((12, 1)) + x[0] return my_values
def objective(x,y,p,N,params): nPrimal = x.numel() #number of primal sln nDual = y['lam_g'].numel() #number of dual nParam = p.numel() #number of parameters #Loading initial states and controls data = spio.loadmat('CstrDistXinit.mat', squeeze_me = True) Xinit = data['Xinit'] xf = Xinit[0:84] u_opt = Xinit[84:] #Model parameters NT = params['dist']['NT'] #Stages in column Uf = 0.3 #Feed rate to CSTR F_0 _,state,xdot,inputs = ColCSTR_model(Uf,params) sf = Function('sf',[state,inputs],[xdot]) params['model']['sf'] = sf params['model']['xdot_val_rf_ss'] = xf params['model']['x'] = x params['model']['u_opt'] = u_opt nx = params['prob']['nx'] nu = params['prob']['nu'] nk = params['prob']['nk'] tf = params['prob']['tf'] ns = params['prob']['ns'] h = params['prob']['h'] #Preparing collocation matrices _, C, D, d = collocationSetup() params['prob']['d'] = d colloc = {'C':C,'D':D, 'h':h} params['colloc'] = colloc #NLP variable vector V = MX() #Decision variables (control + state) obj = 0 #Objective function cons = MX() #Nonlinear Constraints delta_time = 1 alpha = 1 beta = 1 gamma = 1 params['weight']['delta_time'] = delta_time params['weight']['alpha'] = alpha params['weight']['beta'] = beta params['weight']['gamma'] = gamma #Initial states and Controls data_init = spio.loadmat('CstrDistXinit.mat', squeeze_me = True) xf = data_init['Xinit'][0:84] u_opt = data_init['Xinit'][84:89] #"Lift" Initial conditions X0 = MX.sym('X0', nx) V = vertcat(V,X0) #Decision variables cons = vertcat(cons, X0 - x[0:nx,0]) #Nonlinear constraints cons_x0 = X0 - x[0:nx,0] #Formulating the NLP Xk = X0 data = spio.loadmat('Qmax.mat', squeeze_me = True) params['Qmax'] = data['Qmax'] ssoftc = 0 for i in range(0,N): obj, cons, V, Xk, params, ssoftc = itPredHorizon_pf(Xk, V, cons, obj, params, i, ssoftc) V = vertcat(V[:]) cons = vertcat(cons[:]) #Objective function and constraint functions f = Function('f', [V], [obj], ['V'], ['objective']) c = Function('c', [V], [cons], ['V'], ['constraint']) cx0 = Function('cx0',[X0], [cons_x0], ['X0'], ['constraint']) #Constructing Lagrangian lag_expr = obj + mtimes(transpose(y['lam_g']),cons) g = Function('g',[V],[jacobian(obj,V),obj]) lagr = Function('lagr', [V], [lag_expr], ['V'], ['lag_expr']) [H,gg] = hessian(lag_expr,V) H = Function('H',[V],[H,gg]) [Hobj,gobj] = hessian(obj,V) Hobj = Function('Hobj', [V], [Hobj,gobj]) J = Function('J',[V],[jacobian(cons,V),cons]) Jp = Function('Jp',[X0],[jacobian(cons_x0,X0),cons_x0]) #Evaluating functions at current point f = f(x) g = g(x) g = g[0] g = transpose(g) H = H(x) H = H[0] Lxp = H[0:nPrimal,0:nParam] J = J(x) J = J[0] Jtemp = DM.zeros((nDual,nParam)) cp = Jp(x[0:nParam]) cp = cp[0] Jtemp[0:nParam, 0:nParam] = cp.full() cp = Jtemp.sparse() cst = c(x) #Evaluation of objective function used for Greshgorin bound Hobj = Hobj(x) Hobj = Hobj[0].sparse() f = f.full() g = g.sparse() Lxp = Lxp.sparse() cst = cst.full() #Equality constraint Jeq = J dpe = cp return f, g, H, Lxp, cst, J, cp, Jeq, dpe, Hobj
def custom_with_mult(ocp, nlp, t, x, u, p, mult): my_values = DM.zeros((12, 1)) + x[0] * mult return my_values
def get_ls_factor(n_uncertain, n_samples, pc_order, lamb=0.0): # Uncertain parameter design sobol_design = sobol_seq.i4_sobol_generate(n_uncertain, n_samples, ceil(np.log2(n_samples))) sobol_samples = np.transpose(sobol_design) for i in range(n_uncertain): sobol_samples[i, :] = norm(loc=0., scale=1).ppf(sobol_samples[i, :]) # Polynomial function definition x = SX.sym('x') he0fcn = Function('He0fcn', [x], [1.]) he1fcn = Function('He1fcn', [x], [x]) he2fcn = Function('He2fcn', [x], [x**2 - 1]) he3fcn = Function('He3fcn', [x], [x**3 - 3 * x]) he4fcn = Function('He4fcn', [x], [x**4 - 6 * x**2 + 3]) he5fcn = Function('He5fcn', [x], [x**5 - 10 * x**3 + 15 * x]) he6fcn = Function('He6fcn', [x], [x**6 - 15 * x**4 + 45 * x**2 - 15]) he7fcn = Function('He7fcn', [x], [x**7 - 21 * x**5 + 105 * x**3 - 105 * x]) he8fcn = Function('He8fcn', [x], [x**8 - 28 * x**6 + 210 * x**4 - 420 * x**2 + 105]) he9fcn = Function('He9fcn', [x], [x**9 - 36 * x**7 + 378 * x**5 - 1260 * x**3 + 945 * x]) he10fcn = Function( 'He10fcn', [x], [x**10 - 45 * x**8 + 640 * x**6 - 3150 * x**4 + 4725 * x**2 - 945]) helist = [ he0fcn, he1fcn, he2fcn, he3fcn, he4fcn, he5fcn, he6fcn, he7fcn, he8fcn, he9fcn, he10fcn ] # Calculation of factor for least-squares xu = SX.sym("xu", n_uncertain) exps = (p for p in product(range(pc_order + 1), repeat=n_uncertain) if sum(p) <= pc_order) next(exps) exps = list(exps) psi = SX.ones( int( factorial(n_uncertain + pc_order) / (factorial(n_uncertain) * factorial(pc_order)))) for i in range(len(exps)): for j in range(n_uncertain): psi[i + 1] *= helist[exps[i][j]](xu[j]) psi_fcn = Function('PSIfcn', [xu], [psi]) nparameter = SX.size(psi)[0] psi_matrix = SX.zeros(n_samples, nparameter) for i in range(n_samples): psi_a = psi_fcn(sobol_samples[:, i]) for j in range(SX.size(psi)[0]): psi_matrix[i, j] = psi_a[j] psi_t_psi = mtimes(psi_matrix.T, psi_matrix) + lamb * DM.eye(nparameter) chol_psi_t_psi = chol(psi_t_psi) inv_chol_psi_t_psi = solve(chol_psi_t_psi, SX.eye(nparameter)) inv_psi_t_psi = mtimes(inv_chol_psi_t_psi, inv_chol_psi_t_psi.T) ls_factor = mtimes(inv_psi_t_psi, psi_matrix.T) ls_factor = DM(ls_factor) # Calculation of expectations for variance function n_sample_expectation_vector = 100000 x_sample = np.random.multivariate_normal(np.zeros(n_uncertain), np.eye(n_uncertain), n_sample_expectation_vector) psi_squared_sum = DM.zeros(SX.size(psi)[0]) for i in range(n_sample_expectation_vector): psi_squared_sum += psi_fcn(x_sample[i, :])**2 expectation_vector = psi_squared_sum / n_sample_expectation_vector return ls_factor, expectation_vector, psi_fcn