def __init__(self,
n_x,
n_y,
n_u,
x_names=None,
y_names=None,
u_names=None,
**kwargs):
self.model_name = ''
self.delta_t = 1
self.t_0 = 0
self.t_f = 1
self.finite_elements = 0
DataSet.__init__(self, **kwargs)
self.plot_style = 'plot'
self.create_entry('x', size=n_x, names=x_names)
self.create_entry('y', size=n_y, names=y_names)
self.create_entry('u', size=n_u, names=u_names)
def extend(self, other_sim_result):
"""Extend this SimulationResult with other SimulationResult.
It is only implemented for the case where the other simulation result starts at the end of this
simulation. That is this.t_f == other_sim_result.t_0 .
:param SimulationResult other_sim_result:
:return:
"""
list_of_attributes_to_check = ['n_x', 'n_y', 'n_u']
for attr in list_of_attributes_to_check:
if not getattr(self, attr) == getattr(other_sim_result, attr):
raise Exception(
"Attribute {} is no equal for both SimulationResults: {}!={}"
.format(attr, getattr(self, attr),
getattr(other_sim_result, attr)))
self.t_f = max(self.t_f, other_sim_result.t_f)
self.t_0 = min(self.t_0, other_sim_result.t_0)
self.finite_elements += other_sim_result.finite_elements
DataSet.extend(self, other_sim_result)
def __init__(self, model, **kwargs):
"""
:param SystemModel model: estimator model
:param x_mean: a initial guess for the mean
:param p_k: a initial guess for the covariance of the estimator
:param h_function: a function that receives 3 parameters (x, y_algebraic, u) and returns an measurement.
:param c_matrix: if h_function is not give, c_matrix is "C" from the measurement equation:
y_meas = C*[x y_alg] + D*u. note that it has to have n_x + n_y (algebraic) columns.
:param DM r_v: process noise matrix
:param DM r_n: measurement noise matrix
:param int pc_order: Polynomial chaos order
:param int n_samples: Number of samples to be used
"""
self.model = model
self.h_function = None
self.c_matrix = None
self.d_matrix = None
self.implementation = 'standard'
self.x_mean = None
self.p_k = None
self.r_v = DM(0.)
self.r_n = DM(0.)
self.p = None
self.theta = None
self.y_guess = None
self.n_samples = None
self.n_uncertain = None
self.pc_order = 4
self._types_fixed = False
self._checked = False
EstimatorAbstract.__init__(self, **kwargs)
self._fix_types()
if self.n_uncertain is None:
self.n_uncertain = self.x_mean.numel()
if self.n_samples is None:
self.n_samples = factorial(self.n_uncertain + self.pc_order) // (
factorial(self.n_uncertain) * factorial(self.pc_order))
if self.n_samples < self.n_pol_parameters:
raise ValueError(
'Number of samples has to greater or equal to the number of polynomial parameters'
'"n_samples"={}, n_pol_parameter={}'.format(
self.n_samples, self.n_pol_parameters))
self._ls_factor, _, _ = get_ls_factor(self.n_uncertain, self.n_samples,
self.pc_order)
self.dataset = DataSet(name=self.model.name)
self.dataset.data['x']['size'] = self.model.n_x
self.dataset.data['x']['names'] = [
'est_' + self.model.x_sym[i].name() for i in range(self.model.n_x)
]
self.dataset.data['P']['size'] = self.model.n_x**2
self.dataset.data['P']['names'] = [
'P_' + str(i) + str(j) for i in range(self.model.n_x)
for j in range(self.model.n_x)
]
def to_dataset(self):
"""
Return a dataset with the data of x, y, and u
:rtype: DataSet
"""
dataset = DataSet(name=self.problem_name + '_dataset')
dataset.max_delta_t = (self.t_f - self.t_0) / self.finite_elements
dataset.plot_style = 'plot'
dataset.create_entry('x', size=len(self.x_names), names=self.x_names)
dataset.create_entry('y', size=len(self.y_names), names=self.y_names)
dataset.create_entry('u', size=len(self.u_names), names=self.u_names)
dataset.create_entry('theta_opt',
size=len(self.theta_opt_names),
names=self.theta_opt_names,
plot_style='step')
for entry in self.other_data:
size = self.other_data[entry]['values'][0][0].shape[0]
dataset.create_entry(
entry,
size=size,
names=[entry + '_' + str(i) for i in range(size)])
if self.degree_control > 1:
dataset.data['u']['plot_style'] = 'plot'
else:
dataset.data['u']['plot_style'] = 'step'
x_times = self.x_data['time'] + [[self.t_f]]
for el in range(self.finite_elements + 1):
time = horzcat(*x_times[el])
values = horzcat(*self.x_data['values'][el])
dataset.insert_data('x', time, values)
for el in range(self.finite_elements):
time_y = horzcat(*self.y_data['time'][el])
values_y = horzcat(*self.y_data['values'][el])
dataset.insert_data('y', time_y, values_y)
time_u = horzcat(*self.u_data['time'][el])
values_u = horzcat(*self.u_data['values'][el])
dataset.insert_data('u', time_u, values_u)
dataset.insert_data('theta_opt',
time=horzcat(*self.time_breakpoints[:-1]),
value=horzcat(*self.theta_opt))
for entry in self.other_data:
for el in range(self.finite_elements):
dataset.insert_data(
entry,
time=horzcat(*self.other_data[entry]['time'][el]),
value=horzcat(*self.other_data[entry]['values'][el]))
return dataset
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)
class PCEKalmanFilter(EstimatorAbstract):
def __init__(self, model, **kwargs):
"""
:param SystemModel model: estimator model
:param x_mean: a initial guess for the mean
:param p_k: a initial guess for the covariance of the estimator
:param h_function: a function that receives 3 parameters (x, y_algebraic, u) and returns an measurement.
:param c_matrix: if h_function is not give, c_matrix is "C" from the measurement equation:
y_meas = C*[x y_alg] + D*u. note that it has to have n_x + n_y (algebraic) columns.
:param DM r_v: process noise matrix
:param DM r_n: measurement noise matrix
:param int pc_order: Polynomial chaos order
:param int n_samples: Number of samples to be used
"""
self.model = model
self.h_function = None
self.c_matrix = None
self.d_matrix = None
self.implementation = 'standard'
self.x_mean = None
self.p_k = None
self.r_v = DM(0.)
self.r_n = DM(0.)
self.p = None
self.theta = None
self.y_guess = None
self.n_samples = None
self.n_uncertain = None
self.pc_order = 4
self._types_fixed = False
self._checked = False
EstimatorAbstract.__init__(self, **kwargs)
self._fix_types()
if self.n_uncertain is None:
self.n_uncertain = self.x_mean.numel()
if self.n_samples is None:
self.n_samples = factorial(self.n_uncertain + self.pc_order) // (
factorial(self.n_uncertain) * factorial(self.pc_order))
if self.n_samples < self.n_pol_parameters:
raise ValueError(
'Number of samples has to greater or equal to the number of polynomial parameters'
'"n_samples"={}, n_pol_parameter={}'.format(
self.n_samples, self.n_pol_parameters))
self._ls_factor, _, _ = get_ls_factor(self.n_uncertain, self.n_samples,
self.pc_order)
self.dataset = DataSet(name=self.model.name)
self.dataset.data['x']['size'] = self.model.n_x
self.dataset.data['x']['names'] = [
'est_' + self.model.x_sym[i].name() for i in range(self.model.n_x)
]
self.dataset.data['P']['size'] = self.model.n_x**2
self.dataset.data['P']['names'] = [
'P_' + str(i) + str(j) for i in range(self.model.n_x)
for j in range(self.model.n_x)
]
@property
def n_pol_parameters(self):
n_pol_parameters = factorial(self.n_uncertain + self.pc_order) / (
factorial(self.n_uncertain) * factorial(self.pc_order))
return n_pol_parameters
def _fix_types(self):
self.x_mean = vertcat(self.x_mean)
self.p_k = vertcat(self.p_k)
self.r_v = vertcat(self.r_v)
self.r_n = vertcat(self.r_n)
if self.p is not None:
self.p = vertcat(self.p)
def _check(self):
if self.x_mean is None:
raise ValueError(
'A initial condition for the "x_mean" must be provided')
if self.p_k is None:
raise ValueError(
'A initial condition for the "p_k" must be provided')
if self.h_function is None and self.c_matrix is None:
raise ValueError(
'Neither a measurement function "h_function" or a measurement matrix "c_matrix" was given'
)
return True
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
x_mean = self.x_mean
x_cov = self.p_k
(x_hat_k_minus, p_k_minus, y_hat_k_minus, p_yk_yk,
k_gain) = self._priori_update(x_mean,
x_cov,
u=u_k,
p=self.p,
theta=self.theta)
x_hat_k = x_hat_k_minus + mtimes(k_gain, (y_k - y_hat_k_minus))
p_k = p_k_minus - mtimes(k_gain, mtimes(p_yk_yk, k_gain.T))
self.x_mean = x_hat_k
self.p_k = p_k
self.dataset.insert_data('x', t_k, self.x_mean)
self.dataset.insert_data('P', t_k, vec(self.p_k))
return x_hat_k, p_k
def _get_measurement_from_prediction(self, x, y, u):
if self.h_function is not None:
measurement_prediction = self.h_function(x, y, u)
elif self.c_matrix is not None:
measurement_prediction = mtimes(self.c_matrix, vertcat(x, y))
if self.d_matrix is not None:
measurement_prediction = measurement_prediction + mtimes(
self.d_matrix, u)
else:
raise ValueError(
'Neither a measurement function "h_function" or a measurement matrix "c_matrix" was given'
)
return measurement_prediction
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 _get_sampled_states(self, x_mean, x_cov):
x_samples = sample_parameter_normal_distribution_with_sobol(
x_mean, x_cov, self.n_samples)
return [x_samples[:, i] for i in range(self.n_samples)]
class PlantSimulation(Plant):
"""
Simulates a plant using a model.
"""
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 _initialize_dataset(self):
self.dataset.data['x']['size'] = self.model.n_x
self.dataset.data['x']['names'] = [
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'] = [
self.model.y_sym[i].name() for i in range(self.model.n_y)
]
self.dataset.data['u']['size'] = self.model.n_u
self.dataset.data['u']['names'] = [
self.model.u_sym[i].name() for i in range(self.model.n_u)
]
self.dataset.data['meas']['size'] = self.c_matrix.size1()
self.dataset.data['meas']['names'] = [
'meas_' + str(i) for i in range(self.model.n_x)
]
def get_measurement(self):
"""Return the plant measurement of a simulated model and advance time by 't_s'.
Return the measurement time, the measurement [x; y], and the controls.
:rtype: tuple
:return: (timestamp, measuremnt, control)
"""
# perform the simulation
if self.has_noise:
v_rand = DM(
numpy.random.multivariate_normal([0] * self.r_v.shape[0],
self.r_v))
else:
v_rand = 0
# Simulation (Try to do the simulation with disturbance, if not able to use the disturbance, try again without)
sim_result = self.model.simulate(
x_0=self.x,
t_0=self.t,
t_f=self.t + self.t_s,
y_0=self.y_guess,
u=self.u,
p=self.p,
theta=self.theta,
integrator_options=self.integrator_options)
x, y, u = sim_result.final_condition()
if self.has_noise:
x = x + v_rand
self.t += self.t_s
self.x = x
measurement_wo_noise = mtimes(self.c_matrix, vertcat(x, y))
if self.has_noise:
n_rand = DM(
numpy.random.multivariate_normal([0] * self.r_n.shape[0],
self.r_n))
measurement = measurement_wo_noise + n_rand
else:
measurement = measurement_wo_noise
self.dataset.insert_data('x', self.t, x)
self.dataset.insert_data('y', self.t, y)
self.dataset.insert_data('u', self.t, u)
self.dataset.insert_data('meas', self.t, measurement)
self.dataset.insert_data('meas_wo_noise', self.t, measurement_wo_noise)
if self.verbosity >= 1:
print('Real state: {}'.format(x))
return self.t, measurement, self.u
def set_control(self, u):
"""set a new control for the plant
:param DM u: new control vector
"""
if isinstance(u, (list, int, float)):
u = vertcat(u)
if not u.shape[0] == self.model.n_u:
raise ValueError(
"Given control does not have the same size of the plant."
"Plant control size: {}, given control size: {}".format(
self.model.n_u, u.shape[0]))
self.u = u
def __init__(self, model, **kwargs):
"""
Unscented Kalman Filter. Two versions are implemented standard and square-root.
Implemented based on [1] and [2].
References:
[1] Wan, E. A., & Van Der Merwe, R. (2000). The unscented Kalman filter for nonlinear estimation.
In Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium
(Cat. No.00EX373) (Vol. v, pp. 153–158). IEEE. http://doi.org/10.1109/ASSPCC.2000.882463
[2] Merwe, R. Van Der, & Wan, E. a. (2001). The square-root unscented Kalman filter for state and
parameter-estimation. 2001 IEEE International Conference on Acoustics, Speech, and Signal Processing.
Proceedings (Cat. No.01CH37221), 6, 1–4. http://doi.org/10.1109/ICASSP.2001.940586
:param SystemModel model: estimator model
:param x_mean: a initial guess for the mean
:param p_k: a initial guess for the covariance of the estimator
:param h_function: a function that receives 3 parameters (x, y_algebraic, u) and returns an measurement.
:param c_matrix: if h_function is not give, c_matrix is "C" from the measurement equation:
y_meas = C*[x y_alg] + D*u. note that it has to have n_x + n_y (algebraic) columns.
:param DM r_v: process noise matrix
:param DM r_n: measurement noise matrix
:param implementation: options: 'standard' or 'square-root'. (default: 'standard')
"""
self.model = model
self._ell = self.model.n_x
self.n_sigma_points = 2 * self._ell + 1
self.h_function = None
self.c_matrix = None
self.d_matrix = None
self.implementation = 'standard'
self.x_mean = None
self.p_k = None
self.r_v = 0.
self.r_n = 0.
self.p = None
self.theta = None
self.y_guess = None
self._types_fixed = False
self._checked = False
EstimatorAbstract.__init__(self, **kwargs)
# Data set
self.dataset = DataSet(name=self.model.name)
self.dataset.data['x']['size'] = self.model.n_x
self.dataset.data['x']['names'] = [
'ukf_' + 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'] = [
'ukf_' + 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'] = [
'ukf_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'] = [
'ukf_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'] = [
'ukf_meas_' + str(i) for i in range(self.n_meas)
]
# Choose the UKF implementation
if self.implementation == 'standard':
self.estimate = self._estimate_standard_ukf
else:
self.estimate = self._estimate_square_root_ukf
class UnscentedKalmanFilter(EstimatorAbstract):
def __init__(self, model, **kwargs):
"""
Unscented Kalman Filter. Two versions are implemented standard and square-root.
Implemented based on [1] and [2].
References:
[1] Wan, E. A., & Van Der Merwe, R. (2000). The unscented Kalman filter for nonlinear estimation.
In Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium
(Cat. No.00EX373) (Vol. v, pp. 153–158). IEEE. http://doi.org/10.1109/ASSPCC.2000.882463
[2] Merwe, R. Van Der, & Wan, E. a. (2001). The square-root unscented Kalman filter for state and
parameter-estimation. 2001 IEEE International Conference on Acoustics, Speech, and Signal Processing.
Proceedings (Cat. No.01CH37221), 6, 1–4. http://doi.org/10.1109/ICASSP.2001.940586
:param SystemModel model: estimator model
:param x_mean: a initial guess for the mean
:param p_k: a initial guess for the covariance of the estimator
:param h_function: a function that receives 3 parameters (x, y_algebraic, u) and returns an measurement.
:param c_matrix: if h_function is not give, c_matrix is "C" from the measurement equation:
y_meas = C*[x y_alg] + D*u. note that it has to have n_x + n_y (algebraic) columns.
:param DM r_v: process noise matrix
:param DM r_n: measurement noise matrix
:param implementation: options: 'standard' or 'square-root'. (default: 'standard')
"""
self.model = model
self._ell = self.model.n_x
self.n_sigma_points = 2 * self._ell + 1
self.h_function = None
self.c_matrix = None
self.d_matrix = None
self.implementation = 'standard'
self.x_mean = None
self.p_k = None
self.r_v = 0.
self.r_n = 0.
self.p = None
self.theta = None
self.y_guess = None
self._types_fixed = False
self._checked = False
EstimatorAbstract.__init__(self, **kwargs)
# Data set
self.dataset = DataSet(name=self.model.name)
self.dataset.data['x']['size'] = self.model.n_x
self.dataset.data['x']['names'] = [
'ukf_' + 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'] = [
'ukf_' + 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'] = [
'ukf_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'] = [
'ukf_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'] = [
'ukf_meas_' + str(i) for i in range(self.n_meas)
]
# Choose the UKF implementation
if self.implementation == 'standard':
self.estimate = self._estimate_standard_ukf
else:
self.estimate = self._estimate_square_root_ukf
@property
def n_meas(self):
""" Number of measurements
:rtype: int
:return: Number of measurements
"""
if self.h_function is not None:
return self.h_function.numel_out()
elif self.c_matrix is not None:
return self.c_matrix.shape[0]
else:
raise Exception(
"The estimator has no measurements information, neither 'h_function' or 'c_matrix' "
"were given.")
def _fix_types(self):
self.x_mean = vertcat(self.x_mean)
self.p_k = vertcat(self.p_k)
self.r_v = vertcat(self.r_v)
self.r_n = vertcat(self.r_n)
if self.p is not None:
self.p = vertcat(self.p)
def _check(self):
if self.x_mean is None:
raise ValueError(
'A initial condition for the "x_mean" must be provided')
if self.p_k is None:
raise ValueError(
'A initial condition for the "p_k" must be provided')
if self.h_function is None and self.c_matrix is None:
raise ValueError(
'Neither a measurement function "h_function" or a measurement matrix "c_matrix" was given'
)
return True
def estimate(self, t_k, y_k, u_k):
raise Exception('This function should have been replaced')
def _estimate_standard_ukf(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
x_mean = self.x_mean
x_cov = self.p_k
# obtain the weights
sigma_points, weights_m, weights_c = self._get_sigma_points_and_weights(
x_mean, x_cov)
# Obtain the unscented transformation points via simulation
simulation_results = []
x_ut_list = []
y_ut_list = []
x_aug_ut_list = []
meas_ut_list = []
for i in range(self.n_sigma_points):
x_0_i = sigma_points[i]
simulation_results_i = self.model.simulate(x_0=x_0_i,
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)
simulation_results.append(simulation_results_i)
x_ut_list.append(simulation_results_i.final_condition()[0])
y_ut_list.append(simulation_results[i].final_condition()[1])
x_aug_ut_list.append(vertcat(x_ut_list[-1], y_ut_list[-1]))
meas_ut_list.append(
self._get_measurement_from_prediction(x_ut_list[i],
y_ut_list[i], u_k))
# Obtain the means
x_aug_pred = sum([
weights_m[i] * x_aug_ut_list[i] for i in range(self.n_sigma_points)
])
x_pred = x_aug_pred[:self.model.n_x]
meas_pred = sum([
weights_m[i] * meas_ut_list[i] for i in range(self.n_sigma_points)
])
# Compute the covariances
cov_x_aug_pred = sum([
weights_c[i] * mtimes((x_aug_ut_list[i] - x_aug_pred),
(x_aug_ut_list[i] - x_aug_pred).T)
for i in range(self.n_sigma_points)
])
cov_x_aug_pred[:self.model.n_x, :self.model.n_x] += self.r_v
cov_meas_pred = sum([
weights_c[i] * mtimes((meas_ut_list[i] - meas_pred),
(meas_ut_list[i] - meas_pred).T)
for i in range(self.n_sigma_points)
]) + self.r_n
cov_xmeas_pred = sum([
weights_c[i] * mtimes((x_aug_ut_list[i] - x_aug_pred),
(meas_ut_list[i] - meas_pred).T)
for i in range(self.n_sigma_points)
])
# Calculate the gain
k_gain = solve(cov_meas_pred.T, cov_xmeas_pred.T).T
# Correct prediction of the state estimation
x_mean = x_aug_pred + 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_k)
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
cov_x_aug = cov_x_aug_pred - mtimes(k_gain,
mtimes(cov_meas_pred, k_gain.T))
self.x_mean = x_mean
self.p_k = cov_x_aug
# Save variables in local object
self._x_aug = x_mean
self.x_mean = x_mean[:self.model.n_x]
self._p_k_aug = cov_x_aug_pred
self.p_k = cov_x_aug[: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, cov_x_aug[self.model.n_x:,
self.model.n_x:])
return x_mean, cov_x_aug
def _estimate_square_root_ukf(self, t_k, y_k, u_k):
raise NotImplementedError
@staticmethod
def cholupdate(r_matrix, x, sign):
import numpy as np
p = np.size(x)
x = x.T
for k in range(p):
if sign == '+':
r = np.sqrt(r_matrix[k, k]**2 + x[k]**2)
elif sign == '-':
r = np.sqrt(r_matrix[k, k]**2 - x[k]**2)
else:
raise ValueError(
"sign can be '-' or '+', value given = {}".format(sign))
c = r / r_matrix[k, k]
s = x[k] / r_matrix[k, k]
r_matrix[k, k] = r
if sign == '+':
r_matrix[k,
k + 1:p] = (r_matrix[k, k + 1:p] + s * x[k + 1:p]) / c
elif sign == '-':
r_matrix[k,
k + 1:p] = (r_matrix[k, k + 1:p] - s * x[k + 1:p]) / c
x[k + 1:p] = c * x[k + 1:p] - s * r_matrix[k, k + 1:p]
return r_matrix
def _get_sigma_points_and_weights(self, x_mean, x_cov):
# Initialize variables
sigma_points = []
weights_m = []
weights_c = []
ell = self._ell
# Tuning parameters
alpha = 1e-3
kappa = 0
beta = 2
lamb = alpha**2 * (ell + kappa)
# Sigma points
sqr_root_matrix = chol((ell + lamb) * x_cov).T
sigma_points.append(x_mean)
for i in range(self.n_sigma_points - 1):
ind = i % ell
sign = 1 if i < ell else -1
sigma_points.append(x_mean + sign * sqr_root_matrix[:, ind])
# Weights
weights_m.append(lamb / (ell + lamb))
weights_c.append(lamb / (ell + lamb) + (1 - alpha**2 + beta))
for i in range(self.n_sigma_points - 1):
weights_m.append(1 / (2 * (ell + lamb)))
weights_c.append(1 / (2 * (ell + lamb)))
return sigma_points, weights_m, weights_c
def _get_measurement_from_prediction(self, x, y, u):
if self.h_function is not None:
measurement_prediction = self.h_function(x, y, u)
elif self.c_matrix is not None:
measurement_prediction = mtimes(self.c_matrix, vertcat(x, y))
if self.d_matrix is not None:
measurement_prediction = measurement_prediction + mtimes(
self.d_matrix, u)
else:
raise ValueError(
'Neither a measurement function "h_function" or a measurement matrix "c_matrix" was given'
)
return measurement_prediction
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)
]
class ExtendedKalmanFilter(EstimatorAbstract):
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)
]
@property
def n_meas(self):
""" Number of measurements
:rtype: int
:return: Number of measurements
"""
if self.h_function is not None:
return self.h_function.numel_out()
elif self.c_matrix is not None:
return self.c_matrix.shape[0]
else:
raise Exception(
"The estimator has no measurements information, neither 'h_function' or 'c_matrix' "
"were given.")
def _fix_types(self):
self.x_mean = vertcat(self.x_mean)
self.p_k = vertcat(self.p_k)
self.r_v = vertcat(self.r_v)
self.r_n = vertcat(self.r_n)
if self.p is not None:
self.p = vertcat(self.p)
def _check(self):
if self.x_mean is None:
raise ValueError(
'A initial condition for the "x_mean" must be provided')
if self.p_k is None:
raise ValueError(
'A initial condition for the "p_k" must be provided')
if self.h_function is None and self.c_matrix is None:
raise ValueError(
'Neither a measurement function "h_function" or a measurement matrix "c_matrix" was given'
)
return True
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 _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 _get_measurement_from_prediction(self, x, y, u):
if self.h_function is not None:
measurement_prediction = self.h_function(x, y, u)
elif self.c_matrix is not None:
measurement_prediction = mtimes(self.c_matrix, vertcat(x, y))
if self.d_matrix is not None:
measurement_prediction = measurement_prediction + mtimes(
self.d_matrix, u)
else:
raise ValueError(
'Neither a measurement function "h_function" or a measurement matrix "c_matrix" was given'
)
return measurement_prediction