def __init__(self, model, **kwargs): OptimalControlProblem.__init__(self, model, name=model.name + "_stabilization", obj={ "Q": DM.eye(2), "R": DM.eye(2) }, x_0=[1, 1], **kwargs)
def create_2x2_mimo(): a = DM([[-1, -2], [5, -1]]) b = DM([[1, 0], [0, 1]]) model = _create_linear_system(n_x=2, n_u=2, a=a, b=b, name="MIMO_2x2") problem = OptimalControlProblem(model, obj={ "Q": DM.eye(2), "R": DM.eye(2) }, x_0=[1, 1]) return model, problem
def create_siso(): a = DM([-1]) b = DM([1]) model = _create_linear_system(n_x=1, n_u=1, a=a, b=b, name="SISO") problem = OptimalControlProblem(model, obj={ "Q": DM.eye(1), "R": DM.eye(1) }, x_0=[1]) return model, problem
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 test_expm(self): # Test for eye correct_res = diag(exp(DM.ones(3))) a = expm(DM.eye(3)) self.assertAlmostEqual(norm_fro(a - correct_res), 0, 3) # Test for -magic(3) (compared with MATLAB solution) a = DM([[-8, -1, -6], [-3, -5, -7], [-4, -9, -2]]) correct_res = DM( [[3.646628887990924, 32.404567030885005, -36.051195612973601], [5.022261973341555, 44.720086474306093, -49.742348141745325], [-8.668890555430160, -77.124653199288772, 85.793544060621244]]) self.assertAlmostEqual(norm_fro(expm(a) - correct_res), 0, 2)
def expm(a_matrix): """Since casadi does not have native support for matrix exponential, this is a trick to computing it. It can be quite expensive, specially for large matrices. THIS ONLY SUPPORT NUMERIC MATRICES AND MX VARIABLES, DOES NOT SUPPORT SX SYMBOLIC VARIABLES. :param DM a_matrix: matrix :return: """ dim = a_matrix.shape[1] # Create the integrator x_mx = MX.sym('x', a_matrix.shape[1]) a_mx = MX.sym('x', a_matrix.shape) ode = mtimes(a_mx, x_mx) dae_system_dict = {'x': x_mx, 'ode': ode, 'p': vec(a_mx)} integrator_ = integrator("integrator", "cvodes", dae_system_dict, {'tf': 1}) integrator_map = integrator_.map(a_matrix.shape[1], 'thread') res = integrator_map(x0=DM.eye(dim), p=repmat(vec(a_matrix), (1, a_matrix.shape[1])))['xf'] return res
sys.path.append(abspath(dirname(dirname(__file__)))) model = SystemModel() x = model.create_state("x", 2) u = model.create_control("u", 2) a = DM([[-1, -2], [5, -1]]) b = DM([[1, 0], [0, 1]]) model.include_equations(ode=vertcat(mtimes(a, x) + mtimes(b, u))) problem = OptimalControlProblem( model, obj={ "Q": DM.eye(2), "R": DM.eye(2) }, x_0=[1, 1], ) problem.delta_u_max = [0.05, 0.05] problem.delta_u_min = [-0.05, -0.05] problem.last_u = [0, 0] solution_method = DirectMethod( problem, degree=3, degree_control=1, finite_elements=20, # integrator_type='implicit',
problem, finite_elements=20, discretization_scheme="collocation", ) ###################### # Estimator # ###################### # Create a copy of the model for the estimator estimator_model = model.get_copy() # Create the estimator estimator = ExtendedKalmanFilter( model=estimator_model, t_s=t_s, p_k=0.0001 * DM.eye(estimator_model.n_x), x_mean=x_0 * 1.1, c_matrix=c_matrix, r_v=0.00001 * DM.eye(estimator_model.n_x), r_n=0.00001 * DM.eye(1), ) ###################### # Plant # ###################### # Create a copy of the model for the plant plant_model = model.get_copy() # create the plant plant = PlantSimulation(model=plant_model, x_0=x_0, c_matrix=c_matrix, u=0.1)
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 __init__(self, model, **kwargs): """Extended Kalman Filter for ODE and DAE systems implementation based on (1) (1) Mandela, R. K., Narasimhan, S., & Rengaswamy, R. (2009). Nonlinear State Estimation of Differential Algebraic Systems. Proceedings of the 2009 ADCHEM (Vol. 42). IFAC. http://doi.org/10.3182/20090712-4-TR-2008.00129 :param SystemModel model: filter model :param float t_s: sampling time :param float t: current estimator time :param DM x_mean: current state estimation :param DM p_k: current covariance estimation :param DM r_v: process noise covariance matrix :param DM r_n: measurement noise covariance matrix :param DM y_guess: initial guess of the algebraic variables for the model simulation """ self.model = model self.h_function = None # casadi.Function self.c_matrix = None self.d_matrix = None self.implementation = "standard" self.x_mean = None self.p_k = None self.r_v = 0.0 self.r_n = 0.0 self.p = None self.theta = None self.y_guess = None self.verbosity = 1 self._types_fixed = False self._checked = False EstimatorAbstract.__init__(self, **kwargs) self._p_k_aug = DM.eye(self.model.n_x + self.model.n_y) self._p_k_aug[:self.model.n_x, :self.model.n_x] = self.p_k if self.model.n_y > 0: if self.y_guess is not None: self._x_aug = vertcat(self.x_mean, self.y_guess) else: raise ValueError( "If the model has algebraic it is necessary to provide a initial guess for it " "('y_guess').") else: self._x_aug = self.x_mean ( self.augmented_model, self.a_aug_matrix_func, self.gamma_func, self.p_update_func, ) = self._create_augmented_model_and_p_update(model) # Data set self.dataset = DataSet(name=self.model.name) self.dataset.data["x"]["size"] = self.model.n_x self.dataset.data["x"]["names"] = [ "ekf_" + self.model.x_sym[i].name() for i in range(self.model.n_x) ] self.dataset.data["y"]["size"] = self.model.n_y self.dataset.data["y"]["names"] = [ "ekf_" + self.model.y_sym[i].name() for i in range(self.model.n_y) ] self.dataset.data["P"]["size"] = self.model.n_x**2 self.dataset.data["P"]["names"] = [ "ekf_P_" + str(i) + str(j) for i in range(self.model.n_x) for j in range(self.model.n_x) ] self.dataset.data["P_y"]["size"] = self.model.n_y**2 self.dataset.data["P_y"]["names"] = [ "ekf_P_y_" + str(i) + str(j) for i in range(self.model.n_y) for j in range(self.model.n_y) ] self.dataset.data["meas"]["size"] = self.n_meas self.dataset.data["meas"]["names"] = [ "ekf_meas_" + str(i) for i in range(self.n_meas) ]
def _create_augmented_model_and_p_update(self, model): """ :type model: SystemModel """ aug_model = SystemModel("aug_linearized_EKF_model") aug_model.include_state(self.model.x_sym) aug_model.include_state(self.model.y_sym) aug_model.include_control(self.model.u_sym) aug_model.include_parameter(self.model.p_sym) aug_model.include_theta(self.model.theta_sym) # remove u_par (self.model.u_par model.u_par) all_sym = list(self.model.all_sym) # Mean a_func = Function("A_matrix", all_sym, [jacobian(self.model.ode, self.model.x_sym)]) b_func = Function("B_matrix", all_sym, [jacobian(self.model.ode, self.model.y_sym)]) c_func = Function("C_matrix", all_sym, [jacobian(self.model.alg, self.model.x_sym)]) d_func = Function("D_matrix", all_sym, [jacobian(self.model.alg, self.model.y_sym)]) x_lin = aug_model.create_parameter("x_lin", self.model.n_x) y_lin = aug_model.create_parameter("y_lin", self.model.n_y) all_sym[1] = x_lin all_sym[2] = y_lin a_matrix = a_func(*all_sym) b_matrix = b_func(*all_sym) c_matrix = c_func(*all_sym) d_matrix = d_func(*all_sym) a_aug_matrix = vertcat( horzcat(a_matrix, b_matrix), horzcat( mtimes(-solve(d_matrix, c_matrix), a_matrix), mtimes(-solve(d_matrix, c_matrix), b_matrix), ), ) x_aug = vertcat(model.x_sym, model.y_sym) aug_model.include_equations(ode=[mtimes(a_aug_matrix, x_aug)]) # Covariance gamma = vertcat(DM.eye(self.model.n_x), -solve(d_matrix, c_matrix)) trans_matrix_sym = SX.sym("trans_matrix", a_aug_matrix.shape) p_matrix_sym = SX.sym("p_matrix", (model.n_x + model.n_y, model.n_x + model.n_y)) gamma_matrix_sym = SX.sym("gamma_matrix", gamma.shape) q_matrix_sym = SX.sym("q_matrix", (self.model.n_x, self.model.n_x)) p_kpp = mtimes(trans_matrix_sym, mtimes(p_matrix_sym, trans_matrix_sym.T)) + mtimes( gamma_matrix_sym, mtimes(q_matrix_sym, gamma_matrix_sym.T)) a_aug_matrix_func = Function("trans_matrix", all_sym, [a_aug_matrix]) gamma_func = Function("gamma", all_sym, [gamma]) p_update_func = Function( "p_update", [trans_matrix_sym, p_matrix_sym, gamma_matrix_sym, q_matrix_sym], [p_kpp], ) return aug_model, a_aug_matrix_func, gamma_func, p_update_func
def estimate(self, t_k, y_k, u_k): if not self._checked: self._check() self._checked = True if not self._types_fixed: self._fix_types() self._types_fixed = True # Simulate the system to obtain the prediction for the states and algebraic variables sim_results = self.model.simulate( x_0=self.x_mean, t_0=self.t, t_f=self.t + self.t_s, u=u_k, p=self.p, theta=self.theta, y_0=self.y_guess, integrator_type="implicit", ) x_pred, y_pred, u_f = sim_results.final_condition() x_aug_f = vertcat(x_pred, y_pred) # Covariance all_sym_values = self.model.put_values_in_all_sym_format( t=self.t, x=x_pred, y=y_pred, p=self.p, theta=self.theta) all_sym_values = all_sym_values a_aug_matrix = self.a_aug_matrix_func(*all_sym_values) transition_matrix = expm(a_aug_matrix * self.t_s) gamma_matrix = self.gamma_func(*all_sym_values) p_pred = self.p_update_func(transition_matrix, self._p_k_aug, gamma_matrix, self.r_v) # Obtain the matrix G (linearized measurement model) measurement_sym = self._get_measurement_from_prediction( self.model.x_sym, self.model.y_sym, self.model.u_sym) dh_dx_aug_sym = jacobian(measurement_sym, vertcat(self.model.x_sym, self.model.y_sym)) f_dh_dx_aug = Function( "dh_dx_aug", [self.model.x_sym, self.model.y_sym, self.model.u_sym], [dh_dx_aug_sym], ) g_matrix = f_dh_dx_aug(x_pred, y_pred, u_f) # Obtain the filter gain k_gain = mtimes( mtimes(p_pred, g_matrix.T), inv(mtimes(g_matrix, mtimes(p_pred, g_matrix.T)) + self.r_n), ) # Get measurement prediction meas_pred = self._get_measurement_from_prediction(x_pred, y_pred, u_f) # Correct prediction of the state estimation x_mean = x_aug_f + mtimes(k_gain, (y_k - meas_pred)) meas_corr = self._get_measurement_from_prediction( x_mean[:self.model.n_x], x_mean[self.model.n_x:], u_f) if self.verbosity > 1: print("Predicted state: {}".format(x_pred)) print("Prediction error: {}".format(y_k - meas_pred)) print("Correction: {}".format(mtimes(k_gain, (y_k - meas_pred)))) print("Corrected state: {}".format(x_mean)) print("Measurement: {}".format(y_k)) print("Corrected Meas.: {}".format(meas_corr)) # Correct covariance prediction p_k = mtimes( DM.eye(self.augmented_model.n_x) - mtimes(k_gain, g_matrix), p_pred) # Save variables in local object self._x_aug = x_mean self.x_mean = x_mean[:self.model.n_x] self._p_k_aug = p_k self.p_k = p_k[:self.model.n_x, :self.model.n_x] # Save in the data set self.dataset.insert_data("x", t_k, self.x_mean) self.dataset.insert_data("y", t_k, x_mean[self.model.n_x:]) self.dataset.insert_data("meas", t_k, meas_corr) self.dataset.insert_data("P", t_k, vec(self.p_k)) self.dataset.insert_data("P_y", t_k, p_k[self.model.n_x:, self.model.n_x:]) return self.x_mean, self.p_k
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