def _apply_parameters_to_objective(self):
# As mentioned above, the objective does not depend on the actual
# values of V, but on the values of P and EPS_E and EPS_U, while
# P is fed from pdata, and EPS_E, EPS_u are supposed to be 0
objective_free_variables = ci.veccat([ \
self._discretization.optimization_variables["P"],
self._discretization.optimization_variables["U"],
self._discretization.optimization_variables["Q"],
self._discretization.optimization_variables["X"],
self._discretization.optimization_variables["EPS_U"],
])
objective_free_variables_parameters_applied = ci.veccat([ \
self._pdata,
self._discretization.optimization_variables["U"],
self._discretization.optimization_variables["Q"],
self._discretization.optimization_variables["X"],
ci.mx(*self._discretization.optimization_variables["EPS_U"].shape),
])
objective_fcn = ci.mx_function("objective_fcn", \
[objective_free_variables], [self._objective_parameters_free])
[self._objective] = objective_fcn( \
[objective_free_variables_parameters_applied])
def _apply_parameters_to_equality_constraints(self):
optimization_variables_for_equality_constraints = ci.veccat([ \
self._discretization.optimization_variables["U"],
self._discretization.optimization_variables["Q"],
self._discretization.optimization_variables["X"],
self._discretization.optimization_variables["EPS_U"],
self._discretization.optimization_variables["P"],
])
optimization_variables_parameters_applied = ci.veccat([ \
self._discretization.optimization_variables["U"],
self._discretization.optimization_variables["Q"],
self._discretization.optimization_variables["X"],
ci.mx(*self._discretization.optimization_variables["EPS_U"].shape),
self._pdata,
])
equality_constraints_fcn = ci.mx_function( \
"equality_constraints_fcn", \
[optimization_variables_for_equality_constraints], \
[self._discretization.equality_constraints])
[self._equality_constraints_parameters_applied] = \
equality_constraints_fcn([optimization_variables_parameters_applied])
def _apply_parameters_to_objective(self):
# As mentioned above, the objective does not depend on the actual
# values of V, but on the values of P and EPS_U, while
# P is fed from pdata, and EPS_U is supposed to be 0
objective_free_variables = ci.veccat([ \
self._discretization.optimization_variables["P"],
self._discretization.optimization_variables["U"],
self._discretization.optimization_variables["Q"],
self._discretization.optimization_variables["X"],
self._discretization.optimization_variables["EPS_U"],
])
objective_free_variables_parameters_applied = ci.veccat([ \
self._pdata,
self._discretization.optimization_variables["U"],
self._discretization.optimization_variables["Q"],
self._discretization.optimization_variables["X"],
ci.mx(*self._discretization.optimization_variables["EPS_U"].shape),
])
objective_fcn = ci.mx_function("objective_fcn", \
[objective_free_variables], [self._objective_parameters_free])
self._objective = objective_fcn( \
objective_free_variables_parameters_applied)
def _apply_parameters_to_equality_constraints(self):
optimization_variables_for_equality_constraints = ci.veccat([ \
self._discretization.optimization_variables["U"],
self._discretization.optimization_variables["Q"],
self._discretization.optimization_variables["X"],
self._discretization.optimization_variables["EPS_U"],
self._discretization.optimization_variables["P"],
])
optimization_variables_parameters_applied = ci.veccat([ \
self._discretization.optimization_variables["U"],
self._discretization.optimization_variables["Q"],
self._discretization.optimization_variables["X"],
ci.mx(*self._discretization.optimization_variables["EPS_U"].shape),
self._pdata,
])
equality_constraints_fcn = ci.mx_function( \
"equality_constraints_fcn", \
[optimization_variables_for_equality_constraints], \
[self._discretization.equality_constraints])
self._equality_constraints_parameters_applied = \
equality_constraints_fcn(optimization_variables_parameters_applied)
def _setup_gauss_newton_lagrangian_hessian(self):
gauss_newton_lagrangian_hessian_diag = ci.vertcat([ \
ci.mx(self._cov_matrix_derivative_directions.shape[0] - \
self._weightings_vectorized.shape[0], 1), \
self._weightings_vectorized])
self._gauss_newton_lagrangian_hessian = ci.diag( \
gauss_newton_lagrangian_hessian_diag)
def _setup_gauss_newton_lagrangian_hessian(self):
gauss_newton_lagrangian_hessian_diag = ci.vertcat([ \
ci.mx(self._cov_matrix_derivative_directions.shape[0] - \
self._weightings_vectorized.shape[0], 1), \
self._weightings_vectorized])
self._gauss_newton_lagrangian_hessian = ci.diag( \
gauss_newton_lagrangian_hessian_diag)
def gauss_newton_lagrangian_hessian(self):
gauss_newton_lagrangian_hessian_diag = ci.vertcat([ \
ci.mx(self._optimization_variables.shape[0] - \
self._weightings_vectorized.shape[0], 1), \
self._weightings_vectorized])
gauss_newton_lagrangian_hessian = ci.diag( \
gauss_newton_lagrangian_hessian_diag)
return gauss_newton_lagrangian_hessian
def gauss_newton_lagrangian_hessian(self):
gauss_newton_lagrangian_hessian_diag = ci.vertcat([ \
ci.mx(self._optimization_variables.shape[0] - \
self._weightings_vectorized.shape[0], 1), \
self._weightings_vectorized])
gauss_newton_lagrangian_hessian = ci.diag( \
gauss_newton_lagrangian_hessian_diag)
return gauss_newton_lagrangian_hessian
def _setup_covariance_matrix(self, kkt_matrix, \
number_of_unknown_parameters):
I = ci.mx_eye(number_of_unknown_parameters)
O = ci.mx(kkt_matrix.shape[0] - number_of_unknown_parameters, \
number_of_unknown_parameters)
Z_p = ci.vertcat([I, O])
self._covariance_matrix = ci.solve(kkt_matrix, Z_p, "csparse")[ \
:number_of_unknown_parameters, :number_of_unknown_parameters]
def _setup_covariance_matrix(self, kkt_matrix, \
number_of_unknown_parameters):
I = ci.mx_eye(number_of_unknown_parameters)
O = ci.mx(kkt_matrix.shape[0] - number_of_unknown_parameters, \
number_of_unknown_parameters)
Z_p = ci.vertcat([I,O])
self._covariance_matrix = ci.solve(kkt_matrix, Z_p, "csparse")[ \
:number_of_unknown_parameters, :number_of_unknown_parameters]
def _setup_kkt_matrix(self, equality_constraints, optimization_variables):
# Construct the KKT matrix from the Hessian of the Langrangian and the
# Jacobian of the equality constraints
kkt_matrix_A = self.hess_lag
kkt_matrix_B = ci.jacobian(equality_constraints, \
optimization_variables)
kkt_matrix_C = ci.mx(equality_constraints.shape[0], \
equality_constraints.shape[0])
self._kkt_matrix = ci.blockcat( \
kkt_matrix_A, kkt_matrix_B.T, \
kkt_matrix_B, kkt_matrix_C)
def _setup_kkt_matrix(self, equality_constraints, optimization_variables):
# Construct the KKT matrix from the Hessian of the Langrangian and the
# Jacobian of the equality constraints
kkt_matrix_A = self.hess_lag
kkt_matrix_B = ci.jacobian(equality_constraints, \
optimization_variables)
kkt_matrix_C = ci.mx(equality_constraints.shape[0], \
equality_constraints.shape[0])
self._kkt_matrix = ci.blockcat( \
kkt_matrix_A, kkt_matrix_B.T, \
kkt_matrix_B, kkt_matrix_C)
