Add MPC

.gitignore

@@ -102,6 +102,9 @@ cpp.sublime-workspace
 .vs/
 .vscode/
 .cache/
+.idea/
+**/CMakeCache.txt
+CMakeFiles/**
 
 # whitelist documentation
 !docs/**

CMakeLists.txt

@@ -122,6 +122,7 @@ find_package(Doxygen COMPONENTS dot
 message(STATUS "CMAKE_PREFIX_PATH = ${CMAKE_PREFIX_PATH}")
 include(HDF5)
 include(Armadillo)
+include(OSQP)
 
 if (LDSCTRLEST_BUILD_FIT AND LDSCTRLEST_BUILD_STATIC)
   # for mex compat.

cmake/Modules/OSQP.cmake

@@ -0,0 +1,19 @@
+include(FetchContent)
+message(STATUS "Fetching & installing osqp")
+FetchContent_Declare(
+    osqp
+    PREFIX ${CMAKE_BINARY_DIR}/osqp
+    GIT_REPOSITORY https://github.com/osqp/osqp.git
+    GIT_TAG 4e81a9d0
+)
+FetchContent_MakeAvailable(osqp)
+message(STATUS "Installed osqp to ${osqp_BINARY_DIR}")
+list(APPEND CMAKE_PREFIX_PATH ${osqp_BINARY_DIR})
+
+
+if(TARGET osqp AND NOT TARGET osqp::osqp)
+  add_library(osqp::osqp ALIAS osqp)
+endif()
+
+# find_package(osqp REQUIRED)
+list(APPEND PROJECT_REQUIRED_LIBRARIES_ABSOLUTE_NAME osqp::osqp)

examples/CMakeLists.txt

@@ -18,5 +18,5 @@ foreach(file ${files})
 	# 	FAIL_REGULAR_EXPRESSION "(Exception|Test failed)")
 	set_tests_properties(${file_without_ext}
 		PROPERTIES
-		TIMEOUT 20)
+		TIMEOUT 40)
 endforeach()

examples/eg_lqmpc_ctrl.cpp

@@ -0,0 +1,203 @@
+//===-- eg_lqmpc_ctrl.cpp - Example LQMPC Control
+//---------------------------===//
+//
+// Copyright 2024 Chia-Chien Hung and Kyle Johnsen
+// Copyright 2024 Georgia Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+//===----------------------------------------------------------------------===//
+/// \brief Example LQMPC Control
+///
+/// \example eg_lqmpc_ctrl.cpp
+//===----------------------------------------------------------------------===//
+
+#include <armadillo>
+#include <ldsCtrlEst>
+
+using lds::data_t;
+using lds::Matrix;
+using lds::Sparse;
+using lds::Vector;
+using std::cout;
+
+auto main() -> int {
+  cout << " ********** Example Gaussian MPC Control ********** \n\n";
+
+  // Example as provided by CLOCTools/lqmpc
+
+  auto z_to_y = [](const Matrix& z) -> Matrix {
+    return (arma::log(z) + 5.468) / 61.4;
+  };
+  // Didn't implement y_to_z because it's unused
+
+  const data_t dt = 1e-3;  // Sample period
+  const size_t n_u = 2;    // Input dimensions
+  const size_t n_x = 4;    // State dimensions
+  const size_t n_y = 3;    // Output dimensions
+
+  // Define the system matrices
+  Matrix A = Matrix({{1, -6.66e-13, -2.03e-9, -4.14e-6},
+                     {9.83e-4, 1, -4.09e-8, -8.32e-5},
+                     {4.83e-7, 9.83e-4, 1, -5.34e-4},
+                     {1.58e-10, 4.83e-7, 9.83e-4, .9994}});
+
+  Matrix B = Matrix({{9.83e-4, 4.83e-7, 1.58e-10, 3.89e-14}}).t();
+  Matrix B_ = Matrix({{1e-5, 1e-5, 1e-10, 1e-14}}).t();
+  if (n_u == 2) {
+    B = arma::join_rows(B, -B);
+  } else if (n_u == 3) {
+    B = arma::join_rows(B, -B, B_);
+  }
+
+  Matrix C = Matrix({{-.0096, .0135, .005, -.0095}});
+  if (n_y == 2) {
+    C = arma::join_cols(C, 2 * C);
+  } else if (n_y == 3) {
+    C = arma::join_cols(C, 2 * C, 3 * C);
+  }
+
+  Vector g_true = Vector(n_u).fill(1.0);
+
+  // Initialize the system that is being controlled
+  lds::gaussian::System controlled_system(n_u, n_x, n_y, dt);
+  controlled_system.set_A(A);
+  controlled_system.set_B(B);
+  controlled_system.set_C(C);
+  controlled_system.set_g(g_true);
+  controlled_system.Reset();
+
+  cout << ".....................................\n";
+  cout << "controlled_system:\n";
+  cout << ".....................................\n";
+  controlled_system.Print();
+  cout << ".....................................\n";
+
+  // Initialize the controller
+  lds::gaussian::MpcController controller;
+  const size_t N = 25;  // Prediction horizon
+  const size_t M = 20;  // Control horizon
+  {
+    Matrix Q = C.t() * C * 1e5;
+    Matrix R = Matrix(n_u, n_u, arma::fill::eye) *
+               1e-1;  // using dense instead of sparse matrix
+    Matrix S = Matrix(
+        n_u, n_u, arma::fill::zeros);  // using dense instead of sparse matrix
+
+    Vector umin = {0};
+    Vector umax = {5};
+    if (n_u == 2) {
+      umin = {0, 0};
+      umax = {5, 5};
+    } else if (n_u == 3) {
+      umin = {0, 0, 0};
+      umax = {5, 5, 5};
+    }
+
+    Vector xmin(B.n_rows);
+    xmin.fill(-arma::datum::inf);
+    Vector xmax(B.n_rows);
+    xmax.fill(arma::datum::inf);
+
+    lds::gaussian::System controller_system(controlled_system);
+
+    controller =
+        std::move(lds::gaussian::MpcController(std::move(controller_system)));
+    controller.set_cost(Q, R, S, N, M);
+    controller.set_constraint(xmin, xmax, umin, umax);
+  }
+
+  cout << ".....................................\n";
+  cout << "control system:\n";
+  cout << ".....................................\n";
+  controller.Print();
+  cout << ".....................................\n";
+
+  // Set up variables for simulation
+  Vector u0 = Vector(n_u, arma::fill::zeros);
+  Vector x0 = Vector(n_x, arma::fill::zeros);
+
+  const size_t n_t = 120;     // Number of time steps
+  const data_t t_sim = 0.25;  // Simulation time step
+  Matrix zr = 0.05 * arma::sin(arma::linspace<Matrix>(0, 2 * arma::datum::pi,
+                                                      (n_t + 25) * 250) *
+                               12) +
+              0.1;
+  Matrix yr = z_to_y(zr.t());
+  if (n_y == 2) {
+    yr = arma::join_cols(yr, 2 * yr);
+  } else if (n_y == 3) {
+    yr = arma::join_cols(yr, 2 * yr, 3 * yr);
+  }
+
+  Matrix I = Matrix(n_x, n_x, arma::fill::eye);
+  Matrix ur = arma::pinv(C * arma::inv(I - A) * B) * yr;
+  Matrix xr = arma::inv(I - A) * B * ur;
+
+  // create Matrix to save outputs in...
+  Matrix y_ref(n_y, n_t, arma::fill::zeros);
+  Matrix y_true(n_y, n_t, arma::fill::zeros);
+  Matrix y_hat(n_y, n_t, arma::fill::zeros);
+  Matrix u(n_u, n_t, arma::fill::zeros);
+  Matrix J(1, n_t, arma::fill::zeros);
+
+  // Simulate the system
+  cout << "Starting " << n_t * t_sim << " sec simulation ... \n";
+  auto t1 = std::chrono::high_resolution_clock::now();
+  const size_t n_sim = static_cast<int>(t_sim / dt);
+
+  Vector z(n_y);  // to get initial measurement
+  for (size_t t = 0; t < n_t; ++t) {
+    // Calculate the slice indices
+    size_t start_idx = t * n_sim;
+    size_t end_idx = (t + N) * n_sim - 1;
+
+    auto* j = new data_t;  // cost
+
+    u0 = controller.Control(t_sim, z, xr.cols(start_idx, end_idx), true, j);
+
+    for (size_t i = 0; i < n_sim; i++) {
+      z = controlled_system.Simulate(u0);
+    }
+
+    lds::gaussian::System controller_system_test(controlled_system);
+
+    // save the signals
+    y_ref.col(t) = yr.col(end_idx);
+    y_true.col(t) = controlled_system.y();
+    y_hat.col(t) = controller.sys().y();
+    u.col(t) = u0;
+    J.col(t).fill(*j);
+  }
+
+  auto t2 = std::chrono::high_resolution_clock::now();
+  std::chrono::duration<data_t, std::milli> sim_time_ms = t2 - t1;
+  cout << "Finished simulation in " << sim_time_ms.count() << " ms.\n";
+  cout << "(app. " << (sim_time_ms.count() / n_t) * 1e3 << " µs/time-step)\n";
+
+  cout << "Saving simulation data to disk.\n";
+
+  // saving with hdf5 via armadillo
+  arma::hdf5_opts::opts replace = arma::hdf5_opts::replace;
+
+  auto dt_vec = Vector(1).fill(dt);
+  dt_vec.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "dt"));
+  y_ref.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "y_ref", replace));
+  y_true.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "y_true", replace));
+  y_hat.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "y_hat", replace));
+  u.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "u", replace));
+  J.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "j", replace));
+
+  cout << "fin.\n";
+  return 0;
+}

examples/eg_plds_ctrl.cpp

@@ -159,8 +159,9 @@ auto main() -> int {
   m_hat.col(0) = controller.sys().m();
   m_true.col(0) = controlled_system.m();
 
-  cout << "Starting " << n_t * dt << " sec simulation ... \n";
-  auto start = std::chrono::high_resolution_clock::now();
+  // get the disturbance at each time step ahead of time
+  // to maintain consistent between examples
+  arma::arma_rng::set_seed(100);
   for (size_t t = 1; t < n_t; t++) {
     // simulate a stochastically switched disturbance
     Vector chance = arma::randu<Vector>(1);
@@ -171,12 +172,18 @@ auto main() -> int {
         which_m = 1;
       }
     } else {                       // high disturbance
-      if (chance[0] < pr_hi2lo) {  // swithces high -> low disturbance
+      if (chance[0] < pr_hi2lo) {  // switches high -> low disturbance
         m0_true = std::vector<data_t>(n_x, m_low);
         which_m = 0;
       }
     }
-    controlled_system.set_m(m0_true);
+    m_true.col(t) = m0_true;
+  }
+
+  cout << "Starting " << n_t * dt << " sec simulation ... \n";
+  auto start = std::chrono::high_resolution_clock::now();
+  for (size_t t = 1; t < n_t; t++) {
+    controlled_system.set_m(m_true.col(t));
 
     // e.g., use sinusoidal reference
     data_t f = 0.5;  // freq [=] Hz
@@ -198,7 +205,6 @@ auto main() -> int {
 
     y_true.col(t) = controlled_system.y();
     x_true.col(t) = controlled_system.x();
-    m_true.col(t) = controlled_system.m();
 
     y_hat.col(t) = controller.sys().y();
     x_hat.col(t) = controller.sys().x();