Added Hybrid EBCI MRAC controller

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

README.md

@@ -1,6 +1,5 @@
 # ACSL Flight Stack
 [![BSD License](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](LICENSE.txt)
-[![Website](https://img.shields.io/badge/Website-acslstack.com-green)](https://www.acslstack.com/)
 
 To clone this repo with all the needed submodules you can run the command:
 
@@ -37,7 +36,11 @@ This software currently offers two control solutions for the inner and outer loo
 1. **Continuous-Time Feedback-Linearizing Control Law** combined with a **PID (Proportional-Integral-Derivative) Control Law**.
 2. The above control law is augmented by a **Robust Model Reference Adaptive Control (MRAC) System**, incorporating a simplified quadratic-in-the-velocity aerodynamic model.
 
-For further details on these control architectures, refer to the publications found [here](https://www.acslstack.com/Journals).
+For further details on these control architectures, refer to the following publications:
+
+- M. Gramuglia, G. M. Kumar, and A. L'Afflitto, ["Two-Layer Adaptive Funnel MRAC with Applications to the Control of Multi-Rotor UAVs,"](https://doi.org/10.1109/RoMoCo60539.2024.10604361) *2024 13th International Workshop on Robot Motion and Control (RoMoCo),* Poznań, Poland, 2024, pp. 31-36.
+- M. Gramuglia, G. M. Kumar, and A. L’Afflitto, ["A Hybrid Model Reference Adaptive Control System for Multi-Rotor Unmanned Aerial Vehicles,"](https://doi.org/10.2514/6.2024-0755) *AIAA SCITECH 2024 Forum, 2024.*
+- E. Lavretsky and K. Wise, *"Robust and Adaptive Control: With Aerospace Applications,"*, London, UK: Springer, 2012.
 
 Future versions of the software will include additional control systems.
 
@@ -49,10 +52,6 @@ This flight stack is compatible with **[ROS2 Foxy](https://docs.ros.org/en/foxy/
 
 - Provided by a motion capture system over UDP (or, alternatively, GNSS).
 - IMU (Inertial Measurement Unit) from the Pixhawk flight controller.
-
-## How to Plot Log Data
-
-The GitHub repository **[ACSL-flightstack-accessories](https://github.com/andrealaffly/ACSL-flightstack-accessories)** provides MATLAB scripts to plot flight log data and examples of flight tests data.
 
 ## Demo
 A demo of this flight stack is available on **[YouTube](https://youtu.be/Ykjjg21iAm0)**
@@ -62,7 +61,7 @@ A demo of this flight stack is available on **[YouTube](https://youtu.be/Ykjjg21
 - [**Andrea L'Afflitto**](https://github.com/andrealaffly)
 - [**Mattia Gramuglia**](https://github.com/mattia-gramuglia)
 
-For more information, visit [acslstack.com](https://www.acslstack.com/).
+For more information, visit [https://lafflitto.com](https://lafflitto.com).
 
 [![ACSL Flight Stack Logo](https://lafflitto.com/images/ACSL_Logo.jpg)](https://lafflitto.com/ACSL.html)
 

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;