Dev ebci hybrid (#12)

Author: mattia-gramuglia

CMakeLists.txt

@@ -57,6 +57,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/config)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control/funnel_two_layer_mrac)
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control/hybrid_two_layer_mrac)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control/mrac)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control/pid)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control/two_layer_mrac)
@@ -75,6 +76,8 @@ set(SOURCES
   src/control/outer_loop_safety_mechanism.cpp
   src/control/funnel_two_layer_mrac/funnel_two_layer_mrac.cpp
   src/control/funnel_two_layer_mrac/logging_funnel_two_layer_mrac.cpp
+  src/control/hybrid_two_layer_mrac/hybrid_two_layer_mrac.cpp
+  src/control/hybrid_two_layer_mrac/logging_hybrid_two_layer_mrac.cpp
   src/control/mrac/logging_mrac.cpp
   src/control/mrac/mrac.cpp
   src/control/pid/logging_pid.cpp

include/flightstack/config/config.hpp

@@ -53,9 +53,10 @@ class MultiThreadedNode;
 #define __MRAC__ 2
 #define __TWO_LAYER_MRAC__ 3
 #define __FUNNEL_TWO_LAYER_MRAC__ 4
+#define __HYBRID_TWO_LAYER_MRAC__ 5
 
 // SELECT here the CONTROLLER you want to run -----------------------------------------------------------------------
-#define SELECTED_CONTROLLER __FUNNEL_TWO_LAYER_MRAC__
+#define SELECTED_CONTROLLER __HYBRID_TWO_LAYER_MRAC__
 // ------------------------------------------------------------------------------------------------------------------
 
 // Define ControlType based on SELECTED_CONTROLLER using type aliasing
@@ -83,6 +84,12 @@ using ControlType = TwoLayerMRAC;
 #include "logging_funnel_two_layer_mrac.hpp"
 using ControlType = FunnelTwoLayerMRAC;
 
+#elif SELECTED_CONTROLLER == __HYBRID_TWO_LAYER_MRAC__
+
+#include "hybrid_two_layer_mrac.hpp"
+#include "logging_hybrid_two_layer_mrac.hpp"
+using ControlType = HybridTwoLayerMRAC;
+
 #else
 #error "ERROR: Unsupported controller type selected."
 #endif

include/flightstack/control/base_mrac.hpp

@@ -255,7 +255,280 @@ inline std::pair<Eigen::Matrix<double, 1, 1>, int> computeEtaDotFunnel(
   return std::make_pair(Eigen::Matrix<double, 1, 1>(eta_dot), case_eta_dot);
 }
 
-
 } // namespace funnel
 
+
+namespace hybrid {
+
+/*
+  Store the previous iteration trajectory tracking error.
+*/
+template <typename VectorType>
+inline VectorType computePreviousTrajectoryTrackingError(const VectorType& e)
+{
+  static_assert(
+    static_cast<int>(VectorType::ColsAtCompileTime) == 1,
+    "e must be a column vector"
+  );
+  VectorType e_previous = e;
+
+  return e_previous;
+}
+
+/*
+  Evaluate the s-th element of the series used for reference resetting events.
+*/
+inline double evaluateSeriesElement(double s, double alpha)
+{
+  return 1.0 / std::pow(s, alpha);
+}
+
+/*
+  Compute the next index s in the convergent series.
+  weighted_e_squared = eᵀ(t_j) P e(t_j)
+*/
+inline int findSseries(int s_previous, double weighted_e_squared, double alpha)
+{
+  // Compute ceil(weighted_e_squared^(-1/alpha))
+  double exponent = -1.0 / alpha;
+  int candidate = static_cast<int>(
+    std::ceil(std::pow(weighted_e_squared, exponent))
+  );
+  int s = std::max(candidate, s_previous + 1);
+  return s;
+}
+
+/*
+  Compute the quadratic form: vectorᵀ * matrix * vector
+*/
+template <typename VectorType, typename MatrixType>
+inline double computeQuadraticForm(const VectorType& vector, const MatrixType& matrix)
+{
+  static_assert(static_cast<int>(VectorType::ColsAtCompileTime) == 1, "vector must be a column vector");
+
+  static_assert(static_cast<int>(MatrixType::RowsAtCompileTime) == static_cast<int>(MatrixType::ColsAtCompileTime),
+    "matrix must be square"
+  );
+
+  static_assert(static_cast<int>(VectorType::RowsAtCompileTime) == static_cast<int>(MatrixType::RowsAtCompileTime),
+    "Dimension mismatch between vector and matrix"
+  );
+
+  double result = (vector.transpose() * matrix * vector).value();
+
+  return result;
+}
+
+/*
+  Update the hybrid summation term.
+*/
+inline double updateSummationHybridP(double ePe, double ePe_previous, double summation)
+{
+  if (ePe > ePe_previous)
+  {
+    summation += (ePe - ePe_previous);
+  }
+  return summation;
+}
+
+/*
+  Reset the hybrid series index when either:
+  - it grows beyond a prescribed threshold, or
+  - sufficient time has elapsed since the last trajectory reset.
+*/
+inline int resetSeriesIfNeeded(
+  int s_hybrid,
+  double time_now,
+  double time_of_last_trajectory_reset,
+  double tolerance_time_reset_series
+)
+{
+  constexpr int S_MAX_THRESHOLD = 1000000000;
+  const double time_since_last_trajectory_reset = time_now - time_of_last_trajectory_reset;
+
+  if (s_hybrid > S_MAX_THRESHOLD || (s_hybrid > 0 && time_since_last_trajectory_reset > tolerance_time_reset_series))
+  {
+    s_hybrid = 0;
+  }
+  return s_hybrid;
+}
+
+/*
+  Check the hybrid reset condition and, if satisfied, update:
+  - reset time,
+  - series index s,
+  - reference trajectory,
+  - integral term storage.
+*/
+template <typename VectorType, typename MatrixType, typename DerivedRef>
+inline std::pair<int, double> checkAndTriggerTrajectoryReset(
+  double summation_hybrid_P,
+  double time_now,
+  double time_of_last_trajectory_reset,
+  double ePe,
+  double alpha_hybrid_series,
+  int s_hybrid,
+  const VectorType& e,
+  const VectorType& e_previous,
+  const MatrixType& Q,
+  Eigen::MatrixBase<DerivedRef>& reference_model_state_map,
+  Eigen::Ref<Eigen::Matrix<double,1,1>> integral_eQe_map
+)
+{
+  static_assert(VectorType::ColsAtCompileTime == 1, "Vectors must be column vectors");
+  static_assert(MatrixType::RowsAtCompileTime == MatrixType::ColsAtCompileTime, "Q must be square");
+
+  double integral_eQe = integral_eQe_map(0);
+
+  // std::cout << "integral_eQe: " << integral_eQe << std::endl;
+  // std::cout << "summation_hybrid_P: " << summation_hybrid_P << std::endl;
+
+  if (integral_eQe >= summation_hybrid_P)
+  {
+    // Update reset time
+    time_of_last_trajectory_reset = time_now;
+
+    // Update series index
+    s_hybrid = findSseries(s_hybrid, ePe, alpha_hybrid_series);
+
+    // Update reference trajectory (jump)
+    const double series_element = evaluateSeriesElement(static_cast<double>(s_hybrid), alpha_hybrid_series);
+    const double delta_series = ePe - series_element;
+    const double jump_factor = 1.0 - std::sqrt(delta_series / ePe);
+    const VectorType jump_reference_trajectory = jump_factor * e;
+    reference_model_state_map +=  jump_reference_trajectory;
+
+    // Update integral storage term related to (e^T * Q * e)
+    const double eQe = computeQuadraticForm(e, Q);
+    const double eQe_previous = computeQuadraticForm(e_previous, Q);
+    const double delta_integral_eQe = eQe - eQe_previous;
+    integral_eQe_map(0) += delta_integral_eQe;
+
+    std::cout << "checkAndTriggerTrajectoryReset() cond. verified [integral_eQe >= summation_hybrid_P]" << std::endl;
+  }
+
+  return {s_hybrid, time_of_last_trajectory_reset};
+}
+
+/*
+  Execute one hybrid MRAC step:
+  - Update quadratic forms and summation term
+  - Handle series reset logic
+  - Trigger trajectory reset if condition is met
+*/
+template <typename VectorType, typename MatrixType, typename DerivedRef>
+inline std::tuple<Eigen::Matrix<double,1,1>, double, int, double> runHybridStep(
+  const VectorType& e,
+  const VectorType& e_previous,
+  const MatrixType& P,
+  const MatrixType& Q,
+  double summation_hybrid_P,
+  int s_hybrid,
+  double time_of_last_trajectory_reset,
+  double alpha_hybrid_series,
+  double time_now,
+  double tolerance_time_reset_series_hybrid,
+  Eigen::MatrixBase<DerivedRef>& reference_model_state_map,
+  Eigen::Ref<Eigen::Matrix<double,1,1>> integral_eQe_map,
+  bool& first_controller_loop
+)
+{
+  static_assert(VectorType::ColsAtCompileTime == 1, "Vectors must be column vectors");
+  static_assert(MatrixType::RowsAtCompileTime == MatrixType::ColsAtCompileTime, "Matrices must be square");
+
+  // Compute quadratic terms
+  Eigen::Matrix<double,1,1> eQe;
+  eQe(0,0) = computeQuadraticForm(e, Q);
+  const double ePe = computeQuadraticForm(e, P);
+  const double ePe_previous = computeQuadraticForm(e_previous, P);
+  // Update hybrid summation term
+  summation_hybrid_P = updateSummationHybridP(ePe, ePe_previous, summation_hybrid_P);
+
+  if (!first_controller_loop)
+  {
+    // Reset series index if required
+    s_hybrid = resetSeriesIfNeeded(
+      s_hybrid,
+      time_now,
+      time_of_last_trajectory_reset,
+      tolerance_time_reset_series_hybrid
+    );
+
+    // Check reset condition
+    std::tie(s_hybrid, time_of_last_trajectory_reset) = checkAndTriggerTrajectoryReset(
+      summation_hybrid_P,
+      time_now,
+      time_of_last_trajectory_reset,
+      ePe,
+      alpha_hybrid_series,
+      s_hybrid,
+      e,
+      e_previous,
+      Q,
+      reference_model_state_map,
+      integral_eQe_map               
+    );
+  }
+
+  first_controller_loop = false;
+
+  return {eQe, summation_hybrid_P, s_hybrid, time_of_last_trajectory_reset};
+}
+
+
+} // namespace hybrid
+
+
+namespace ebci {
+
+/*
+  Compute the error bounding control input (non-adaptive EBCI).
+*/
+template <typename MatrixBType, typename MatrixPType, typename VectorType>
+inline Eigen::Matrix<typename MatrixBType::Scalar, MatrixBType::ColsAtCompileTime, 1> computeErrorBoundingControlInput(
+  double xi_bar_d,
+  double lambda_bar,
+  double delta_ebci,
+  const MatrixBType& B,
+  const MatrixPType& P,
+  const VectorType& e,
+  bool use_ebci
+)
+{
+  using Scalar = typename MatrixBType::Scalar;
+
+  constexpr int n = MatrixBType::RowsAtCompileTime;
+  constexpr int m = MatrixBType::ColsAtCompileTime;
+
+  static_assert(VectorType::ColsAtCompileTime == 1, "e must be a column vector");
+  static_assert(MatrixPType::RowsAtCompileTime == MatrixPType::ColsAtCompileTime, "P must be square");
+  static_assert(MatrixPType::RowsAtCompileTime == n, "Dimension mismatch: P and B must have same row size");
+  static_assert(VectorType::RowsAtCompileTime == n, "Dimension mismatch: e and B must have same row size");
+
+  using InputVector = Eigen::Matrix<Scalar, m, 1>;
+
+  if (!use_ebci)
+  {
+    return InputVector::Zero();
+  }
+
+  InputVector BPe = B.transpose() * P * e;
+  Scalar BPe_norm = BPe.norm();
+
+  // Dead-zone
+  if (BPe_norm < delta_ebci)
+  {
+    return InputVector::Zero();
+  }
+
+  Eigen::Matrix<Scalar, n, 1> Pe = P * e;
+  Scalar sum_Pe = Pe.cwiseAbs().sum();
+  InputVector control_input = - (xi_bar_d / lambda_bar) * (BPe / BPe_norm) * sum_Pe;
+
+  return control_input;
+}
+
+} // namespace ebci
+
+
 } // namespace base_mrac

include/flightstack/control/control.hpp

@@ -242,6 +242,8 @@ class Control
     return K_hat_state_dot;
   }
 
+  inline static constexpr bool has_post_integration_algorithm_ = false;
+
 protected:
 
   VehicleInfo vehicle_info;

include/flightstack/control/funnel_two_layer_mrac/logging_funnel_two_layer_mrac.hpp

@@ -86,35 +86,6 @@ class MultiThreadedNode;
 // Forward declaration of FunnelTwoLayerMRAC class
 class FunnelTwoLayerMRAC;
 
-// Struct that contains info related to the logger that can be accessed via a getter from the logger class.
-struct LoggingInfo
-{
-  std::string timestamp;             // YYYYMMDD_HHMMSS
-  std::string date;                  // YYYYMMDD
-
-  std::string base_dir;              // ./src/flightstack/log/<date>/<controller>
-
-  std::string logs_dir;
-  std::string log_filename;
-
-  std::string gains_dir;
-  std::string gains_target_filename;
-
-  std::string info_dir;
-  std::string safe_mech_dir;
-  std::string safe_mech_target_filename;
-  std::string git_info_dir;
-  std::string git_info_filename;
-  std::string low_pass_filter_dir;
-  std::string low_pass_filter_filename;
-
-  std::string der_gains_dir;
-  std::string der_gains_filename;
-
-  std::string logs_debug_dir;
-  std::string log_debug_filename;
-};
-
 class LogData_FunnelTwoLayerMRAC
 {
 public: