diff --git a/src/deepc/__init__.py b/src/deepc/__init__.py index 145c728..ba3c514 100644 --- a/src/deepc/__init__.py +++ b/src/deepc/__init__.py @@ -1,5 +1,4 @@ from .lti import DiscreteLTI, RandomNoiseDiscreteLTI -from .deepc import deePC from .controller import Controller -__all__ = ["DiscreteLTI", "RandomNoiseDiscreteLTI", "deePC", "Controller"] +__all__ = ["DiscreteLTI", "RandomNoiseDiscreteLTI", "Controller"] diff --git a/src/deepc/controller.py b/src/deepc/controller.py index 1b6ffad..76796e4 100644 --- a/src/deepc/controller.py +++ b/src/deepc/controller.py @@ -2,7 +2,18 @@ from typing import Callable import numpy as np from .math import hankel_matrix, projected_gradient_method -from .deepc import as_column_vector, check_dimensions + + +def as_column_vector(v: list | np.ndarray) -> np.ndarray: + v = np.array(v) + if v.ndim == 1: + return v.reshape(-1, 1) + return v + + +def check_dimensions(var: np.ndarray, name: str, size: int, dims: int) -> None: + "Checks the dimensions of a variable." + assert var.shape == (size, dims), f"{name}.shape={var.shape} but should be ({size}, {dims})." class Controller: diff --git a/src/deepc/deepc.py b/src/deepc/deepc.py deleted file mode 100644 index 0a65cce..0000000 --- a/src/deepc/deepc.py +++ /dev/null @@ -1,140 +0,0 @@ -from typing import Callable -import numpy as np -from .math import hankel_matrix, projected_gradient_method - - -def as_column_vector(v: list | np.ndarray) -> np.ndarray: - v = np.array(v) - if v.ndim == 1: - return v.reshape(-1, 1) - return v - - -def check_dimensions(var: np.ndarray, name: str, size: int, dims: int) -> None: - "Checks the dimensions of a variable." - assert var.shape == (size, dims), f"{name}.shape={var.shape} but should be ({size}, {dims})." - - -def deePC( - u_d: list | np.ndarray, - y_d: list | np.ndarray, - u_ini: list | np.ndarray, - y_ini: list | np.ndarray, - target: list | np.ndarray, - u_0: list | np.ndarray | None = None, - Q: np.ndarray | int | float | None = None, - R: np.ndarray | int | float | None = None, - input_constrain_fkt: Callable | None = None, - max_pgm_iterations=300, - pgm_tolerance=1e-6, -) -> np.ndarray: - """ - Returns the optimal control for a given system and target system outputs. - According to the paper Data-Enabled Predictive Control: In the Shallows of the DeePC - https://arxiv.org/abs/1811.05890 - Args: - u_d: Control inputs from an offline procedure. - y_d: System outputs from an offline procedure. - u_ini: Control inputs to initialize the state. - y_ini: System outputs to initialize the state. - target: Target system outputs, optimal control tries to match. - u_0: Control input offset, defaults to zero vector. - Q: Output cost matrix. Defaults to identity matrix. - If int or float, diagonal matrix with this value. - R: Control cost matrix. Defaults to zero matrix. - If int or float, diagonal matrix with this value. - input_constrain_fkt: Function that constrains the system inputs. - max_pgm_iterations: Maximum number of iterations of the projected gradient method (PGM) - used to solve the constrained optimization problem. - pgm_tolerance: Tolerance for the PGM algorithm. - """ - u_d = as_column_vector(u_d) - y_d = as_column_vector(y_d) - u_ini = as_column_vector(u_ini) - y_ini = as_column_vector(y_ini) - target = as_column_vector(target) - - offline_len = len(u_d) - T_ini = len(u_ini) - target_len = len(target) - input_dims = u_d.shape[1] - output_dims = y_d.shape[1] - - check_dimensions(u_d, "u_d", offline_len, input_dims) - check_dimensions(y_d, "y_d", offline_len, output_dims) - check_dimensions(u_ini, "u_ini", T_ini, input_dims) - check_dimensions(y_ini, "y_ini", T_ini, output_dims) - check_dimensions(target, "target", target_len, output_dims) - - if u_0 is None: - u_0 = np.zeros((target_len, input_dims)) - else: - u_0 = as_column_vector(u_0) - check_dimensions(u_0, "u_0", target_len, input_dims) - - Q_size = target_len * output_dims - if isinstance(Q, (int, float)): - Q = np.eye(Q_size) * Q - if Q is None: - Q = np.eye(Q_size) - check_dimensions(Q, "Q", Q_size, Q_size) - - R_size = target_len * input_dims - if isinstance(R, (int, float)): - R = np.eye(R_size) * R - if R is None: - R = np.zeros((R_size, R_size)) - check_dimensions(R, "R", R_size, R_size) - - # Flatten - u_ini = np.concatenate(u_ini).reshape(-1, 1) - y_ini = np.concatenate(y_ini).reshape(-1, 1) - target = np.concatenate(target).reshape(-1, 1) - u_0 = np.concatenate(u_0).reshape(-1, 1) - - U = hankel_matrix(T_ini + target_len, u_d) - U_p = U[: T_ini * input_dims, :] # past - U_f = U[T_ini * input_dims :, :] # future - Y = hankel_matrix(T_ini + target_len, y_d) - Y_p = Y[: T_ini * output_dims, :] # past - Y_f = Y[T_ini * output_dims :, :] # future - - # Now solving - # minimize: ||y - r||_Q^2 + ||u||_R^2 - # subject to: [U_p; Y_p; U_f; Y_f] * g = [u_ini; y_ini; u; y] - - # We define - A = np.block([[U_p], [Y_p], [U_f]]) - x = np.concatenate([u_ini, y_ini]).reshape(-1, 1) - # to get - # A * g = [x; u] (1) - # and - # Y_f * g = y (2). - - # We multiply (1) from the left with the pseudo inverse of A. - # Since pinv(A) * A = I, we get g = pinv(A) * [x; u]. - # Substituting g in (2) gives Y_f * pinv(A) * [x; u] = y. - - # We define - M = Y_f @ np.linalg.pinv(A) - # and get M * [x; u] = y. - - # We define [M_x, M_u] := M such that M_x * x + M_u * u = y. - M_x = M[:, : len(x)] - M_u = M[:, len(x) :] - - # We can now solve the unconstrained problem. - # This is a ridge regression problem with generalized Tikhonov regularization. - # https://en.wikipedia.org/wiki/Ridge_regression#Generalized_Tikhonov_regularization - # minimize: ||y - r||_Q^2 + ||u||_R^2 - # subject to: M_u * u = y - M_x * x - # This has an explicit solution u_star = (M_u^T * Q * M_u + R)^-1 * (M_u^T * Q * y + R * u_0). - - G = M_u.T @ Q @ M_u + R - w = M_u.T @ Q @ (target - M_x @ x) + R @ u_0 - u_star = np.linalg.lstsq(G, w)[0] - - if input_constrain_fkt is not None: - u_star = projected_gradient_method(G, u_star, w, input_constrain_fkt, max_pgm_iterations, pgm_tolerance) - - return u_star.reshape(-1, input_dims) diff --git a/src/deepc/math.py b/src/deepc/math.py index 039c4cd..e253227 100644 --- a/src/deepc/math.py +++ b/src/deepc/math.py @@ -1,4 +1,3 @@ -import math from typing import Callable import numpy as np diff --git a/src/deepcpp/CMakeLists.txt b/src/deepcpp/CMakeLists.txt index 30dc8ab..3d0cf54 100644 --- a/src/deepcpp/CMakeLists.txt +++ b/src/deepcpp/CMakeLists.txt @@ -4,8 +4,6 @@ add_library( algorithm.cpp controller.h controller.cpp - deepc.h - deepc.cpp finite_queue.h finite_queue.cpp lti.h diff --git a/src/deepcpp/deepc.cpp b/src/deepcpp/deepc.cpp deleted file mode 100644 index 9308f4c..0000000 --- a/src/deepcpp/deepc.cpp +++ /dev/null @@ -1,136 +0,0 @@ -#include "deepc.h" -#include "algorithm.h" -#include -#include -#include - -static void check_dimensions(const std::vector &var, std::string name, int size, int dims) -{ - if (var.size() != size) - throw std::invalid_argument(name + ".size()=" + std::to_string(var.size()) + " but should be " + std::to_string(size) + "."); - for (int i = 0; i < size; ++i) - if (var[i].size() != dims) - throw std::invalid_argument(name + "[" + std::to_string(i) + "].size()=" + std::to_string(var[i].size()) + " but should be " + std::to_string(dims) + "."); -} - -std::vector deePC( - const std::vector &u_d, - const std::vector &y_d, - const std::vector &u_ini, - const std::vector &y_ini, - const std::vector &target, - std::variant Q_, - std::variant R_, - std::function input_constrain_fkt, - int max_pgm_iterations, - double pgm_tolerance) -{ - const int offline_size = u_d.size(); - const int T_ini = u_ini.size(); - const int target_size = target.size(); - const int input_dims = u_d.front().size(); - const int output_dims = y_d.front().size(); - - check_dimensions(u_d, "u_d", offline_size, input_dims); - check_dimensions(y_d, "y_d", offline_size, output_dims); - check_dimensions(u_ini, "u_ini", T_ini, input_dims); - check_dimensions(y_ini, "y_ini", T_ini, output_dims); - check_dimensions(target, "target", target_size, output_dims); - - MatrixXd Q; - const int Q_size = target_size * output_dims; - if (std::holds_alternative(Q_)) - Q = std::get(Q_) * MatrixXd::Identity(Q_size, Q_size); - else - Q = std::get(Q_); - - MatrixXd R; - const int R_size = target_size * input_dims; - if (std::holds_alternative(R_)) - R = std::get(R_) * MatrixXd::Identity(R_size, R_size); - else - R = std::get(R_); - - // Check Q - if (Q.rows() != Q.cols()) - throw std::invalid_argument("Q must be a square matrix. Q.rows()=" + std::to_string(Q.rows()) + ", Q.cols()=" + std::to_string(Q.cols())); - if (Q.rows() != Q_size) - throw std::invalid_argument("Q.rows()=" + std::to_string(Q.rows()) + " but should be " + std::to_string(Q_size) + "."); - - // Check R - if (R.rows() != R.cols()) - throw std::invalid_argument("R must be a square matrix. R.rows()=" + std::to_string(R.rows()) + ", R.cols()=" + std::to_string(R.cols())); - if (R.rows() != target_size * input_dims) - throw std::invalid_argument("R.rows()=" + std::to_string(R.rows()) + " but should be " + std::to_string(target_size * input_dims) + "."); - - auto U = HankelMatrix(T_ini + target_size, u_d); - auto U_p = U.block(0, 0, T_ini * input_dims, U.cols()); // past - auto U_f = U.block(T_ini * input_dims, 0, U.rows() - T_ini * input_dims, U.cols()); // future - auto Y = HankelMatrix(T_ini + target_size, y_d); - auto Y_p = Y.block(0, 0, T_ini * output_dims, Y.cols()); // past - auto Y_f = Y.block(T_ini * output_dims, 0, Y.rows() - T_ini * output_dims, Y.cols()); // future - - // Now solving - // minimize: ||y - target||_Q^2 + ||u||_R^2 - // subject to: [U_p; Y_p; U_f; Y_f] * g = [u_ini; y_ini; u; y] - - // We define - auto A = vstack(U_p, Y_p, U_f); - auto x = concat(u_ini, y_ini); - // to get - // A * g = [x; u] (1) - // and - // Y_f * g = y (2). - - // We multiply (1) from the left with the pseudo inverse of A. - // Since pinv(A) * A = I, we get g = pinv(A) * [x; u]. - // Substituting g in (2) gives Y_f * pinv(A) * [x; u] = y. - - // We define - auto M = Y_f * A.completeOrthogonalDecomposition().pseudoInverse(); - // and get M * [x; u] = y. - - // We define [M_x; M_u] := M - // such that M_x * x + M_u * u = y. - auto M_x = M.block(0, 0, M.rows(), x.size()); - auto M_u = M.block(0, x.size(), M.rows(), M.cols() - x.size()); - auto M_u_T = M_u.transpose(); - - // We can now solve the unconstrained problem. - // This is a ridge regression problem with generalized Tikhonov regularization. - // https://en.wikipedia.org/wiki/Ridge_regression//Generalized_Tikhonov_regularization - // minimize: ||y - target||_Q^2 + ||u||_R^2 - // subject to: M_u * u = y - M_x * x - // This has an explicit solution u_star = (M_u^T * Q * M_u + R)^-1 * (M_u^T * Q * y). - - // Flatten - VectorXd target_ = concat(target); - - auto G = M_u_T * Q * M_u + R; - auto w = M_u_T * Q * (target_ - M_x * x); - auto u_star = G.ldlt().solve(w).eval(); - - if (input_constrain_fkt != nullptr) - u_star = projected_gradient_method( - G, - u_star, - w, - input_constrain_fkt, - max_pgm_iterations, - pgm_tolerance); - - return split(u_star, target_size); -} - -std::vector deePC( - const std::vector& u_d, - const std::vector& y_d, - const std::vector& u_ini, - const std::vector& y_ini, - const std::vector& target, - std::function input_constrain_fkt, - int max_pgm_iterations, - double pgm_tolerance) -{ - return deePC(u_d, y_d, u_ini, y_ini, target, 1.0, 0.0, input_constrain_fkt, max_pgm_iterations, pgm_tolerance); -} diff --git a/src/deepcpp/deepc.h b/src/deepcpp/deepc.h deleted file mode 100644 index 1fc4696..0000000 --- a/src/deepcpp/deepc.h +++ /dev/null @@ -1,60 +0,0 @@ -#pragma once -#include -#include -#include -#include - -using Eigen::MatrixXd; -using Eigen::VectorXd; - -// Returns the optimal control for a given system and target system outputs. -// According to the paper Data-Enabled Predictive Control: In the Shallows of the DeePC -// https://arxiv.org/abs/1811.05890 -// Args: -// u_d: Control inputs from an offline procedure. -// y_d: System outputs from an offline procedure. -// u_ini: Control inputs to initialize the state. -// y_ini: System outputs to initialize the state. -// target: Target system outputs, optimal control tries to match. -// Q: Output cost matrix. Defaults to identity matrix. -// If double, diagonal matrix with this value. -// R: Control cost matrix. Defaults to zero matrix. -// If double, diagonal matrix with this value. -// input_constrain_fkt: Function that constrains the system inputs. -// max_pgm_iterations: Maximum number of iterations of the projected gradient method (PGM) -// used to solve the constrained optimization problem. -// pgm_tolerance: Tolerance for the PGM algorithm. -std::vector deePC( - const std::vector& u_d, - const std::vector& y_d, - const std::vector& u_ini, - const std::vector& y_ini, - const std::vector& target, - std::variant Q, - std::variant R, - std::function input_constrain_fkt = nullptr, - int max_pgm_iterations = 300, - double pgm_tolerance = 1e-6); - -// Returns the optimal control for a given system and target system outputs. -// According to the paper Data-Enabled Predictive Control: In the Shallows of the DeePC -// https://arxiv.org/abs/1811.05890 -// Args: -// u_d: Control inputs from an offline procedure. -// y_d: System outputs from an offline procedure. -// u_ini: Control inputs to initialize the state. -// y_ini: System outputs to initialize the state. -// target: Target system outputs, optimal control tries to match. -// input_constrain_fkt: Function that constrains the system inputs. -// max_pgm_iterations: Maximum number of iterations of the projected gradient method (PGM) -// used to solve the constrained optimization problem. -// pgm_tolerance: Tolerance for the PGM algorithm. -std::vector deePC( - const std::vector& u_d, - const std::vector& y_d, - const std::vector& u_ini, - const std::vector& y_ini, - const std::vector& target, - std::function input_constrain_fkt = nullptr, - int max_pgm_iterations = 300, - double pgm_tolerance = 1e-6); diff --git a/tests/deepc/test_controller.py b/tests/deepc/test_controller.py index 3d7c7be..2f279f6 100644 --- a/tests/deepc/test_controller.py +++ b/tests/deepc/test_controller.py @@ -26,68 +26,214 @@ def control_system( class Test_1D_in_1D_out_LTI(unittest.TestCase): - def test_unconstrained(self): - system = create_1D_in_1D_out_LTI() - u_d, y_d = gather_offline_data(system, samples=1_000) - T_ini = 20 - target = [[10], [10], [10]] - - controller = Controller(u_d, y_d, T_ini, len(target)) - warm_up_controller(controller, system, u=[1]) - y = control_system(controller, system, target, time_steps=2 * T_ini) + def setUp(self): + self.system = create_1D_in_1D_out_LTI() + self.u_d, self.y_d = gather_offline_data(self.system, samples=1_000) # Offline data + self.T_ini = 20 + self.target = [[3], [3]] - np.testing.assert_array_almost_equal(y, target[0]) + def test_unconstrained(self): + controller = Controller(self.u_d, self.y_d, self.T_ini, len(self.target)) + warm_up_controller(controller, self.system, u=[1]) + y = control_system(controller, self.system, self.target, time_steps=2 * self.T_ini) + np.testing.assert_array_almost_equal(y, self.target[0]) def test_constrained(self): - system = create_1D_in_1D_out_LTI() - u_d, y_d = gather_offline_data(system, samples=1_000) - T_ini = 20 - target = [[10]] - - controller = Controller(u_d, y_d, T_ini, len(target), input_constrain_fkt=lambda u: np.clip(u, 0, 25)) - warm_up_controller(controller, system, u=[1]) - y = control_system(controller, system, target, time_steps=2 * T_ini) + controller = Controller(self.u_d, self.y_d, self.T_ini, len(self.target), input_constrain_fkt=lambda u: np.clip(u, 0, 25)) + warm_up_controller(controller, self.system, u=[1]) + y = control_system(controller, self.system, self.target, time_steps=2 * self.T_ini) + np.testing.assert_array_almost_equal(y, self.target[0]) - np.testing.assert_array_almost_equal(y, target[0]) + def test_offset(self): + controller = Controller(self.u_d, self.y_d, self.T_ini, len(self.target), R=0.001) + warm_up_controller(controller, self.system, u=[1]) + y = control_system(controller, self.system, self.target, time_steps=2 * self.T_ini, u_0=[[10], [10]]) + np.testing.assert_array_almost_equal(y, self.target[0], decimal=1) class Test_2D_in_3D_out_LTI(unittest.TestCase): - def test_unconstrained(self): - system = create_2D_in_3D_out_LTI() - u_d, y_d = gather_offline_data(system, samples=1_000) - T_ini = 20 - target = [(0.19, 0.92, 0.24)] - - controller = Controller(u_d, y_d, T_ini, len(target)) - warm_up_controller(controller, system, u=(1, 1)) - y = control_system(controller, system, target, time_steps=2 * T_ini) + def setUp(self): + self.system = create_2D_in_3D_out_LTI() + self.u_d, self.y_d = gather_offline_data(self.system, samples=1_000) + self.T_ini = 20 + self.target = [(0.19, 0.92, 0.24)] - np.testing.assert_array_almost_equal(y, target[0], decimal=2) + def test_unconstrained(self): + controller = Controller(self.u_d, self.y_d, self.T_ini, len(self.target)) + warm_up_controller(controller, self.system, u=(1, 1)) + y = control_system(controller, self.system, self.target, time_steps=2 * self.T_ini) + np.testing.assert_array_almost_equal(y, self.target[0], decimal=2) def test_constrained(self): - system = create_2D_in_3D_out_LTI() - u_d, y_d = gather_offline_data(system, samples=1_000) - T_ini = 20 - target = [(0.19, 0.92, 0.24)] - - controller = Controller(u_d, y_d, T_ini, len(target), input_constrain_fkt=lambda u: np.clip(u, 0, 25)) - warm_up_controller(controller, system, u=(1, 1)) - y = control_system(controller, system, target, time_steps=2 * T_ini) - - np.testing.assert_array_almost_equal(y, target[0], decimal=2) + controller = Controller(self.u_d, self.y_d, self.T_ini, len(self.target), input_constrain_fkt=lambda u: np.clip(u, -15, 15)) + warm_up_controller(controller, self.system, u=(1, 1)) + y = control_system(controller, self.system, self.target, time_steps=2 * self.T_ini) + np.testing.assert_array_almost_equal(y, self.target[0], decimal=2) def test_offset(self): - system = create_2D_in_3D_out_LTI() - u_d, y_d = gather_offline_data(system, samples=1_000) - T_ini = 20 - target = [(0.32, 0.95, 0.24)] - - controller = Controller(u_d, y_d, T_ini, len(target), R=0.01) - warm_up_controller(controller, system, u=(1, 1)) - y = control_system(controller, system, target, time_steps=2 * T_ini, u_0=[(1, 1)]) - - np.testing.assert_array_almost_equal(y, target[0], decimal=2) - + controller = Controller(self.u_d, self.y_d, self.T_ini, len(self.target), R=0.001) + warm_up_controller(controller, self.system, u=(1, 1)) + y = control_system(controller, self.system, self.target, time_steps=2 * self.T_ini, u_0=[(1, 1)]) + np.testing.assert_array_almost_equal(y, self.target[0], decimal=1) + + +class Test_types(unittest.TestCase): + "Test the different types of input data." + + def test_int(self): + u_d = [1, 2, 3, 4, -5, 6, 7, 8, 9, 10] + y_d = [1, 2, 3, 4, -5, 6, 7, 8, 9, 10] + u_ini = [2] + y_ini = [2] + u = [[4]] + u_0 = [3] + target = [4] + + controller = Controller(u_d, y_d, 1, 1) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_int_with_2_targets(self): + u_d = [1, 2, 3, 4, -5, 6, 7, 8, 9, 10] + y_d = [1, 2, 3, 4, -5, 6, 7, 8, 9, 10] + u_ini = [2] + y_ini = [2] + u = [[4], [4]] + u_0 = [[3], [3]] + target = [4, 4] + + controller = Controller(u_d, y_d, 1, 2) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_tuple(self): + u_d = [(1,), (2,), (3,), (4,), (-5,), (6,), (7,), (8,), (9,), (10,)] + y_d = [(1,), (2,), (3,), (4,), (-5,), (6,), (7,), (8,), (9,), (10,)] + u_ini = [(2,)] + y_ini = [(2,)] + u = [(4,)] + u_0 = [(3,)] + target = [(4,)] + + controller = Controller(u_d, y_d, 1, 1) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_tuple_with_2_targets(self): + u_d = [(1,), (2,), (3,), (4,), (-5,), (6,), (7,), (8,), (9,), (10,)] + y_d = [(1,), (2,), (3,), (4,), (-5,), (6,), (7,), (8,), (9,), (10,)] + u_ini = [(2,)] + y_ini = [(2,)] + u = [(4,), (4,)] + u_0 = [(3,), (3,)] + target = [(4,), (4,)] + + controller = Controller(u_d, y_d, 1, 2) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_list(self): + u_d = [[1], [2], [3], [4], [-5], [6], [7], [8], [9], [10]] + y_d = [[1], [2], [3], [4], [-5], [6], [7], [8], [9], [10]] + u_ini = [[2]] + y_ini = [[2]] + u = [[4]] + u_0 = [[3]] + target = [[4]] + + controller = Controller(u_d, y_d, 1, 1) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_list_with_2_targets(self): + u_d = [[1], [2], [3], [4], [-5], [6], [7], [8], [9], [10]] + y_d = [[1], [2], [3], [4], [-5], [6], [7], [8], [9], [10]] + u_ini = [[2]] + y_ini = [[2]] + u = [[4], [4]] + u_0 = [[3], [3]] + target = [[4], [4]] + + controller = Controller(u_d, y_d, 1, 2) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_nparray(self): + u_d = np.array([1, 2, 3, 4, -5, 6, 7, 8, 9, 10]) + y_d = np.array([1, 2, 3, 4, -5, 6, 7, 8, 9, 10]) + u_ini = np.array([2]) + y_ini = np.array([2]) + u = np.array([[4]]) + u_0 = np.array([3]) + target = np.array([4]) + + controller = Controller(u_d, y_d, 1, 1) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_nparray_with_2_targets(self): + u_d = np.array([1, 2, 3, 4, -5, 6, 7, 8, 9, 10]) + y_d = np.array([1, 2, 3, 4, -5, 6, 7, 8, 9, 10]) + u_ini = np.array([2]) + y_ini = np.array([2]) + u = np.array([[4], [4]]) + u_0 = np.array([[3], [3]]) + target = np.array([4, 4]) + + controller = Controller(u_d, y_d, 1, 2) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_tuple_multidim(self): + u_d = [(x, y) for x in range(3) for y in range(3)] + y_d = [(x, y, x + y) for x, y in u_d] + u_ini = [(2, 2)] + y_ini = [(x, y, x + y) for x, y in u_ini] + u = [(1, 3)] + u_0 = [(2, 2)] + target = [(x, y, x + y) for x, y in u] + + controller = Controller(u_d, y_d, 1, 1) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_list_multidim(self): + u_d = [[x, y] for x in range(3) for y in range(3)] + y_d = [[x, y, x + y] for x, y in u_d] + u_ini = [[2, 2]] + y_ini = [[x, y, x + y] for x, y in u_ini] + u = [[1, 3]] + u_0 = [[2, 2]] + target = [[x, y, x + y] for x, y in u] + + controller = Controller(u_d, y_d, 1, 1) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + + def test_nparray_multidim(self): + u_d = np.array([[x, y] for x in range(3) for y in range(3)]) + y_d = np.array([[x, y, x + y] for x, y in u_d]) + u_ini = np.array([[2, 2]]) + y_ini = np.array([[x, y, x + y] for x, y in u_ini]) + u = np.array([[1, 3]]) + u_0 = np.array([[2, 2]]) + target = np.array([[x, y, x + y] for x, y in u]) + + controller = Controller(u_d, y_d, 1, 1) + controller.update(u_ini, y_ini) + u_star = controller.apply(target, u_0) + np.testing.assert_array_almost_equal(u_star, u) + if __name__ == "__main__": unittest.main(verbosity=2) diff --git a/tests/deepc/test_deepc.py b/tests/deepc/test_deepc.py deleted file mode 100644 index 8bc3f07..0000000 --- a/tests/deepc/test_deepc.py +++ /dev/null @@ -1,245 +0,0 @@ -import unittest -import numpy as np -from deepc import deePC -from helpers import create_1D_in_1D_out_LTI, create_2D_in_3D_out_LTI, gather_offline_data - - -class Test_1D_in_1D_out_LTI(unittest.TestCase): - def setUp(self): - self.system = create_1D_in_1D_out_LTI() - - # Offline data - self.u_d, self.y_d = gather_offline_data(self.system, samples=1_000) - - # Initial conditions - self.u_ini = [1] * 20 - self.y_ini = self.system.apply_multiple(self.u_ini) - - # Reference trajectory - self.target = [[3], [3]] - - def test_unconstrained(self): - u_star = deePC(self.u_d, self.y_d, self.u_ini, self.y_ini, self.target) - - # Apply the control input to the system - # to see if the output matches the target - y_star = self.system.apply_multiple(u_star) - np.testing.assert_array_almost_equal(y_star, self.target) - - def test_constrained(self): - u_star = deePC( - self.u_d, - self.y_d, - self.u_ini, - self.y_ini, - self.target, - input_constrain_fkt=lambda u: np.clip(u, -15, 15), - ) - - # Apply the control input to the system - # to see if the output matches the target - y_star = self.system.apply_multiple(u_star) - np.testing.assert_array_almost_equal(y_star, self.target) - - def test_offset(self): - u_star = deePC(self.u_d, self.y_d, self.u_ini, self.y_ini, self.target, u_0=[[10], [10]], R=0.001) - - # Apply the control input to the system - # to see if the output matches the target - y_star = self.system.apply_multiple(u_star) - np.testing.assert_array_almost_equal(y_star, self.target, decimal=1) - - -class Test_2D_in_3D_out_LTI(unittest.TestCase): - def setUp(self): - self.system = create_2D_in_3D_out_LTI() - - # Offline data - self.u_d, self.y_d = gather_offline_data(self.system, samples=1_000) - - # Initial conditions - self.u_ini = [(1, 1)] * 20 - self.y_ini = self.system.apply_multiple(self.u_ini) - - # Reference trajectory - self.target = [(0.21, 0.9, 0.36)] - - def test_unconstrained(self): - u_star = deePC(self.u_d, self.y_d, self.u_ini, self.y_ini, self.target) - - # Apply the control input to the system - # to see if the output matches the target - y_star = self.system.apply_multiple(u_star) - np.testing.assert_array_almost_equal(y_star, self.target, decimal=2) - - def test_constrained(self): - u_star = deePC( - self.u_d, - self.y_d, - self.u_ini, - self.y_ini, - self.target, - input_constrain_fkt=lambda u: np.clip(u, -15, 15), - ) - - # Apply the control input to the system - # to see if the output matches the target - y_star = self.system.apply_multiple(u_star) - np.testing.assert_array_almost_equal(y_star, self.target, decimal=2) - - -class Test_types(unittest.TestCase): - """ - Test the different types of input data that can be used with deePC - on a trivial 1D input 1D output system. - """ - - def test_int(self): - u_d = [1, 2, 3, 4, -5, 6, 7, 8, 9, 10] - y_d = [1, 2, 3, 4, -5, 6, 7, 8, 9, 10] - u_ini = [2] - y_ini = [2] - u = [[4]] - r = [4] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_int_with_2_targets(self): - u_d = [1, 2, 3, 4, -5, 6, 7, 8, 9, 10] - y_d = [1, 2, 3, 4, -5, 6, 7, 8, 9, 10] - u_ini = [2] - y_ini = [2] - u = [[4], [4]] - r = [4, 4] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_tuple(self): - u_d = [(1,), (2,), (3,), (4,), (5,)] - y_d = [(1,), (2,), (3,), (4,), (5,)] - u_ini = [(2,)] - y_ini = [(2,)] - u = [(4,)] - r = [(4,)] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_tuple_with_2_targets(self): - u_d = [(1,), (2,), (3,), (4,), (-5,), (6,), (7,), (8,), (9,), (10,)] - y_d = [(1,), (2,), (3,), (4,), (-5,), (6,), (7,), (8,), (9,), (10,)] - u_ini = [(2,)] - y_ini = [(2,)] - u = [(4,), (4,)] - r = [(4,), (4,)] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_list(self): - u_d = [[1], [2], [3], [4], [5]] - y_d = [[1], [2], [3], [4], [5]] - u_ini = [[2]] - y_ini = [[2]] - u = [[4]] - r = [[4]] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_list_with_3_targets(self): - u_d = [[1], [2], [3], [4], [-5], [6], [7], [8], [9], [10]] - y_d = [[1], [2], [3], [4], [-5], [6], [7], [8], [9], [10]] - u_ini = [[2]] - y_ini = [[2]] - u = [[4], [4], [4]] - r = [[4], [4], [4]] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_list_of_nparray(self): - u_d = [np.array([1]), np.array([2]), np.array([3]), np.array([4]), np.array([5])] - y_d = [np.array([1]), np.array([2]), np.array([3]), np.array([4]), np.array([5])] - u_ini = [np.array([2])] - y_ini = [np.array([2])] - u = [np.array([4])] - r = [np.array([4])] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_nparray(self): - u_d = np.array([1, 2, 3, 4, 5]) - y_d = np.array([1, 2, 3, 4, 5]) - u_ini = np.array([2]) - y_ini = np.array([2]) - u = np.array([[4]]) - r = np.array([4]) - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - -class Test_2D_types(unittest.TestCase): - def test_tuple(self): - u_d = [(x, y) for x in range(3) for y in range(3)] - y_d = [(x, y, x + y) for x, y in u_d] - u_ini = [(2, 2)] - y_ini = [(x, y, x + y) for x, y in u_ini] - u = [(1, 3)] - r = [(x, y, x + y) for x, y in u] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_list(self): - u_d = [[x, y] for x in range(3) for y in range(3)] - y_d = [[x, y, x + y] for x, y in u_d] - u_ini = [[2, 2]] - y_ini = [[x, y, x + y] for x, y in u_ini] - u = [[1, 3]] - r = [[x, y, x + y] for x, y in u] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_list_of_nparray(self): - u_d = [np.array([x, y]) for x in range(3) for y in range(3)] - y_d = [np.array([x, y, x + y]) for x, y in u_d] - u_ini = [np.array([2, 2])] - y_ini = [np.array([x, y, x + y]) for x, y in u_ini] - u = [np.array([1, 3])] - r = [np.array([x, y, x + y]) for x, y in u] - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - def test_nparray(self): - u_d = np.array([[x, y] for x in range(3) for y in range(3)]) - y_d = np.array([[x, y, x + y] for x, y in u_d]) - u_ini = np.array([[2, 2]]) - y_ini = np.array([[x, y, x + y] for x, y in u_ini]) - u = np.array([[1, 3]]) - r = np.array([[x, y, x + y] for x, y in u]) - - u_star = deePC(u_d, y_d, u_ini, y_ini, r) - - np.testing.assert_array_almost_equal(u_star, u) - - -if __name__ == "__main__": - unittest.main(verbosity=2) diff --git a/tests/deepcpp/CMakeLists.txt b/tests/deepcpp/CMakeLists.txt index a8846bd..887cbc0 100644 --- a/tests/deepcpp/CMakeLists.txt +++ b/tests/deepcpp/CMakeLists.txt @@ -3,7 +3,7 @@ include_directories(${CMAKE_SOURCE_DIR}/src/deepcpp) add_executable( DeePCppTest test_algorithm.cpp - test_deepc.cpp + test_controller.cpp test_helpers.cpp test_lti.cpp ) diff --git a/tests/deepcpp/test_controller.cpp b/tests/deepcpp/test_controller.cpp new file mode 100644 index 0000000..ca8e3c6 --- /dev/null +++ b/tests/deepcpp/test_controller.cpp @@ -0,0 +1,152 @@ +#include "gtest/gtest.h" +#include "controller.h" +#include "algorithm.h" +#include "lti.h" +#include "helpers.h" +#include +#include +#include + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +// LTI system with 1D input and 1D output +DiscreteLTI create_1D_in_1D_out_LTI() +{ + DiscreteLTI system( + Matrix({0.9, -0.2}, {0.7, 0.1}), + Matrix({0.1}, {0}), + Matrix({1, 0}), + Matrix({0.1}), + Vector(1, 1)); + assert(system.input_dim() == 1); + assert(system.output_dim() == 1); + assert(system.is_controllable()); + assert(system.is_observable()); + assert(system.is_stable()); + return system; +} + +// LTI system with 3D input and 2D output +DiscreteLTI create_2D_in_3D_out_LTI() +{ + DiscreteLTI system( + Matrix({0.5, 0.1, 0}, {0.1, 0.5, 0.1}, {0, 0.1, 0.5}), + Matrix({0.1, 0}, {0.1, 0.5}, {0, 0.1}), + Matrix({1, 0, 0}, {0, 1, 1}, {0, 0, 1}), + Matrix({0, 0}, {0, 0}, {0, 0}), + Vector(0, 0, 0)); + assert(system.input_dim() == 2); + assert(system.output_dim() == 3); + assert(system.is_controllable()); + assert(system.is_observable()); + assert(system.is_stable()); + return system; +} + +std::tuple, std::vector> gather_offline_data(DiscreteLTI& system, int samples) +{ + std::vector u_d; + for (int i = 0; i < samples; ++i) + u_d.push_back(VectorXd::Random(system.input_dim())); + std::vector y_d = system.apply_multiple(u_d); + return {u_d, y_d}; +} + +void expect_near(const VectorXd& value, const VectorXd& expected, double abs_error) +{ + // Equal dimensions + EXPECT_EQ(value.size(), expected.size()); + const int dim = value.size(); + + // Near values + for (int i = 0; i < dim; ++i) + EXPECT_NEAR(value(i), expected(i), abs_error); +} + +// Warm up the controller until it is initialized. +void warm_up_controller(Controller& controller, DiscreteLTI& system, const VectorXd& u) +{ + while (!controller.is_initialized()) + { + VectorXd y = system.apply(u); + controller.update(u, y); + } +} + +// Control the system for a given number of time steps. +// Returns the output of the system after the last time step. +VectorXd control_system(Controller& controller, DiscreteLTI& system, const std::vector& target, int time_steps) +{ + VectorXd u, y; + for (int i = 0; i < time_steps; ++i) + { + u = controller.apply(target).front(); + y = system.apply(u); + controller.update(u, y); + } + return y; +} + +class Test_1D_in_1D_out_LTI : public ::testing::Test +{ +protected: + DiscreteLTI system = create_1D_in_1D_out_LTI(); + std::vector u_d, y_d, u_ini, y_ini; + int T_ini = 20; + std::vector target = std::vector(2, Vector(3)); + + void SetUp() override + { + std::tie(u_d, y_d) = gather_offline_data(system, 1'000); + } +}; + +TEST_F(Test_1D_in_1D_out_LTI, Unconstrained) +{ + Controller controller{u_d, y_d, T_ini, target.size()}; + warm_up_controller(controller, system, Vector(1)); + VectorXd y = control_system(controller, system, target, 2 * T_ini); + expect_near(y, target[0], 1e-5); +} + +TEST_F(Test_1D_in_1D_out_LTI, Constrained) +{ + Controller controller{u_d, y_d, T_ini, target.size(), + [](const VectorXd &u) { return clamp(u, 0, 25); }}; + warm_up_controller(controller, system, Vector(1)); + VectorXd y = control_system(controller, system, target, 2 * T_ini); + expect_near(y, target[0], 1e-5); +} + + +class Test_2D_in_3D_out_LTI : public ::testing::Test +{ +protected: + DiscreteLTI system = create_2D_in_3D_out_LTI(); + std::vector u_d, y_d, u_ini, y_ini; + int T_ini = 20; + std::vector target = Vectors({0.19, 0.92, 0.24}); + + void SetUp() override + { + std::tie(u_d, y_d) = gather_offline_data(system, 1'000); + } +}; + +TEST_F(Test_2D_in_3D_out_LTI, Unconstrained) +{ + Controller controller{u_d, y_d, T_ini, target.size()}; + warm_up_controller(controller, system, Vector(1, 1)); + VectorXd y = control_system(controller, system, target, 2 * T_ini); + expect_near(y, target[0], 0.05); +} + +TEST_F(Test_2D_in_3D_out_LTI, Constrained) +{ + Controller controller{u_d, y_d, T_ini, target.size(), + [](const VectorXd &u) { return clamp(u, -15, 15); }}; + warm_up_controller(controller, system, Vector(1, 1)); + VectorXd y = control_system(controller, system, target, 2 * T_ini); + expect_near(y, target[0], 0.05); +} diff --git a/tests/deepcpp/test_deepc.cpp b/tests/deepcpp/test_deepc.cpp deleted file mode 100644 index 86a2f67..0000000 --- a/tests/deepcpp/test_deepc.cpp +++ /dev/null @@ -1,256 +0,0 @@ -#include "gtest/gtest.h" -#include "deepc.h" -#include "controller.h" -#include "algorithm.h" -#include "lti.h" -#include "helpers.h" -#include -#include -#include - -using Eigen::MatrixXd; -using Eigen::VectorXd; - -// LTI system with 1D input and 1D output -DiscreteLTI lti_1D_input_1D_output() -{ - DiscreteLTI system( - Matrix({0.9, -0.2}, {0.7, 0.1}), - Matrix({0.1}, {0}), - Matrix({1, 0}), - Matrix({0.1}), - Vector(1, 1)); - assert(system.input_dim() == 1); - assert(system.output_dim() == 1); - assert(system.is_controllable()); - assert(system.is_observable()); - assert(system.is_stable()); - return system; -} - -// LTI system with 3D input and 2D output -DiscreteLTI lti_2D_input_3D_output() -{ - DiscreteLTI system( - Matrix({0.5, 0.1, 0}, {0.1, 0.5, 0.1}, {0, 0.1, 0.5}), - Matrix({0.1, 0}, {0.1, 0.5}, {0, 0.1}), - Matrix({1, 0, 0}, {0, 1, 1}, {0, 0, 1}), - Matrix({0, 0}, {0, 0}, {0, 0}), - Vector(0, 0, 0)); - assert(system.input_dim() == 2); - assert(system.output_dim() == 3); - assert(system.is_controllable()); - assert(system.is_observable()); - assert(system.is_stable()); - return system; -} - -std::tuple, std::vector> gather_offline_data(DiscreteLTI &system) -{ - const int samples = 1'000; - std::vector u_d; - for (int i = 0; i < samples; ++i) - u_d.push_back(VectorXd::Random(system.input_dim())); - std::vector y_d = system.apply_multiple(u_d); - return {u_d, y_d}; -} - -void expect_near(const std::vector& value, const std::vector& expected, double abs_error) -{ - // Equal sizes - EXPECT_EQ(value.size(), expected.size()); - const int size = value.size(); - - // Equal dimensions - for (int i = 0; i < size; ++i) - EXPECT_EQ(value[i].size(), expected[i].size()); - const int dim = value[0].size(); - - // Near values - for (int i = 0; i < size; ++i) - for (int j = 0; j < dim; ++j) - EXPECT_NEAR(value[i](j), expected[i](j), abs_error); -} - -class Test_deePC_1D_input_1D_output : public ::testing::Test -{ -protected: - DiscreteLTI system; - std::vector u_d, y_d; - std::vector u_ini, y_ini; - std::vector target; - - void SetUp() override - { - system = lti_1D_input_1D_output(); - - // Offline data - std::tie(u_d, y_d) = gather_offline_data(system); - - // Initial conditions - u_ini = std::vector(20, Vector(1)); - y_ini = system.apply_multiple(u_ini); - - // Reference trajectory - target = std::vector(2, Vector(3)); - } -}; - -TEST_F(Test_deePC_1D_input_1D_output, Unconstrained) -{ - std::vector u_star = deePC(u_d, y_d, u_ini, y_ini, target); - - std::vector y_star = system.apply_multiple(u_star); - expect_near(y_star, target, 1e-5); -} - -TEST_F(Test_deePC_1D_input_1D_output, Constrained) -{ - std::vector u_star = deePC( - u_d, y_d, u_ini, y_ini, target, - [](const VectorXd &u) - { - return clamp(u, -15, 15); - }); - - std::vector y_star = system.apply_multiple(u_star); - expect_near(y_star, target, 1e-5); -} - -class Test_deePC_2D_input_3D_output : public ::testing::Test -{ -protected: - DiscreteLTI system; - std::vector u_d, y_d; - std::vector u_ini, y_ini; - std::vector target; - - void SetUp() override - { - system = lti_2D_input_3D_output(); - - // Offline data - std::tie(u_d, y_d) = gather_offline_data(system); - - // Initial conditions - u_ini = std::vector(20, Vector(1, 1)); - y_ini = system.apply_multiple(u_ini); - - // Reference trajectory - target = Vectors({0.35, 1.1, 0.35}); - } -}; - -TEST_F(Test_deePC_2D_input_3D_output, Unconstrained) -{ - std::vector u_star = deePC(u_d, y_d, u_ini, y_ini, target); - - std::vector y_star = system.apply_multiple(u_star); - expect_near(y_star, target, 0.05); -} - -TEST_F(Test_deePC_2D_input_3D_output, Constrained) -{ - std::vector u_star = deePC( - u_d, y_d, u_ini, y_ini, target, - [](const VectorXd &u) - { - return clamp(u, -15, 15); - }); - - std::vector y_star = system.apply_multiple(u_star); - expect_near(y_star, target, 0.05); -} - -TEST(Test_deePC_simple_system, 1D_input_1D_output) -{ - std::vector u_star = deePC( - Vectors(1, 2, 3, 4, -5, 6, 7, 8, 9, 10), - Vectors(1, 2, 3, 4, -5, 6, 7, 8, 9, 10), - Vectors(2), - Vectors(2), - Vectors(4)); - - expect_near(u_star, Vectors(4), 1e-5); -} - -TEST(Test_deePC_simple_system, 1D_input_1D_output_3_targets) -{ - std::vector u_star = deePC( - Vectors(1, 2, 3, 4, -5, 6, 7, 8, 9, 10), - Vectors(1, 2, 3, 4, -5, 6, 7, 8, 9, 10), - Vectors(2), - Vectors(2), - Vectors(4, 4, 4)); - - expect_near(u_star, Vectors(4, 4, 4), 1e-5); -} - -TEST(Test_deePC_simple_system, 2D_input_3D_output) -{ - std::vector u_d, y_d, u_ini, y_ini, u, target; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - u_d.push_back(Vector(i, j)); - for (const VectorXd &u : u_d) - y_d.push_back(Vector(u(0), u(1), u(0) + u(1))); - u_ini = Vectors({2, 2}); - for (const VectorXd &u : u_ini) - y_ini.push_back(Vector(u(0), u(1), u(0) + u(1))); - u = Vectors({1, 3}); - for (const VectorXd &u : u) - target.push_back(Vector(u(0), u(1), u(0) + u(1))); - - auto u_star = deePC(u_d, y_d, u_ini, y_ini, target); - - expect_near(u_star, u, 1e-5); -} - -// Warm up the controller until it is initialized -void warm_up_controller(Controller& controller, DiscreteLTI& system, const VectorXd& u) -{ - while (!controller.is_initialized()) - { - VectorXd y = system.apply(u); - controller.update(u, y); - } -} - -// Control the system for a given number of time steps. -// Returns the output of the system after the last time step. -VectorXd control_system(Controller& controller, DiscreteLTI& system, const std::vector& target, int time_steps) -{ - VectorXd u, y; - for (int i = 0; i < time_steps; ++i) - { - u = controller.apply(target).front(); - y = system.apply(u); - controller.update(u, y); - } - return y; -} - -TEST(Test_Controller, unconstrained_2D_LTI) -{ - DiscreteLTI system = lti_1D_input_1D_output(); - auto [u_d, y_d] = gather_offline_data(system); - int T_ini = 20; - std::vector target = Vectors({10}, {10}, {10}); - - Controller controller(u_d, y_d, T_ini, target.size()); - warm_up_controller(controller, system, Vector(1)); - VectorXd y = control_system(controller, system, target, 2 * T_ini); -} - -TEST(Test_Controller, constrained_2D_LTI) -{ - DiscreteLTI system = lti_1D_input_1D_output(); - auto [u_d, y_d] = gather_offline_data(system); - int T_ini = 20; - std::vector target = Vectors({10}, {10}, {10}); - - auto input_constrain_fkt = [](const VectorXd &u) { return clamp(u, 0, 25); }; - Controller controller(u_d, y_d, T_ini, target.size(), input_constrain_fkt); - warm_up_controller(controller, system, Vector(1)); - VectorXd y = control_system(controller, system, target, 2 * T_ini); -} \ No newline at end of file