tlbmc/thermal/controller/algorithm/first_order_adrc.h

#ifndef THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_FIRST_ORDER_ADRC_H_
#define THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_FIRST_ORDER_ADRC_H_

#include <cmath>
#include <optional>

#include "thermal_config.pb.h"
#include "tlbmc/thermal/controller/algorithm/utils/nonlinear_tracking_differentiator.h"

namespace milotic_tlbmc {
namespace thermal {

/*
 * `FirstOrderAdrcThermalLoop` is an adaptive controller based on 1st-order
 * Active Disturbance Rejection Control (ADRC). Compared to the more commonly
 * used 2nd-order ADRC, this model is more computationally lightweight, but
 * less-prone to oscillations, due to its less sophisticated quantification of
 * unknown disturbances.
 *
 * This class is not thread-safe.
 *
 * The comment format of numeric expressions follows LaTeX style.
 *
 * More resources can be found at:
 * https://www.mdpi.com/2079-9292/2/3/246
 */
class FirstOrderAdrcThermalLoop {
 public:
  explicit FirstOrderAdrcThermalLoop(
      const FirstOrderAdrcLoopConfig& first_order_adrc_params)
      : first_order_adrc_params_(first_order_adrc_params) {
    if (first_order_adrc_params.sample_time_sec() <= 0) {
      first_order_adrc_params_.set_sample_time_sec(1.0);
    }
    if (first_order_adrc_params.settle_time_sec() <= 0) {
      first_order_adrc_params_.set_settle_time_sec(1.0);
    }

    // Initialize the nonlinear tracking differentiator (NTD) if configured.
    if (first_order_adrc_params
            .has_nonlinear_tracking_differentiator_config()) {
      tracking_differentiator_ = NonlinearTrackingDifferentiator(
          first_order_adrc_params_.nonlinear_tracking_differentiator_config()
              .sample_time_sec(),
          first_order_adrc_params_.nonlinear_tracking_differentiator_config()
              .r(),
          first_order_adrc_params_.nonlinear_tracking_differentiator_config()
              .h0(),
          first_order_adrc_params_.nonlinear_tracking_differentiator_config()
              .initial_filtered_output(),
          first_order_adrc_params_.nonlinear_tracking_differentiator_config()
              .initial_filtered_derivative());
    }

    double controller_bandwidth =
        4.0 / first_order_adrc_params_.settle_time_sec();
    double observer_bandwidth =
        first_order_adrc_params_.observer_bandwidth_factor() *
        controller_bandwidth;

    kp_ = controller_bandwidth;

    // Observer gains with discrete-time Zero-Order-Hold mapping.
    double z_pole = std::exp(-observer_bandwidth *
                             first_order_adrc_params_.sample_time_sec());

    l1_ = 1 - z_pole * z_pole;
    l2_ = (1 / first_order_adrc_params_.sample_time_sec()) * (1 - z_pole) *
          (1 - z_pole);

    x1_hat_ = 0.0;
    x2_hat_ = 0.0;
    u_precompute_ = 0.0;
  }

  double ExecuteFirstOrderAdrcLoop(double setpoint, double input);

 protected:
  bool IsSaturated(double u) {
    return u >= first_order_adrc_params_.u_limit_max() ||
           u <= first_order_adrc_params_.u_limit_min();
  }

 private:
  FirstOrderAdrcLoopConfig
      first_order_adrc_params_;  // The parameters for this ADRC thermal loop.
  std::optional<NonlinearTrackingDifferentiator> tracking_differentiator_ =
      std::nullopt;

  double kp_;       // Proportional gain
  double l1_, l2_;  // Observer gains
  double x1_hat_;   // Estimate of the system output
  double x2_hat_;   // Estimate of the generalized disturbance
  double u_precompute_;
};

}  // namespace thermal
}  // namespace milotic_tlbmc

#endif  // THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_FIRST_ORDER_ADRC_H_