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
