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 "absl/log/log.h"
#include "absl/strings/str_format.h"
#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) {
    Initialize(first_order_adrc_params);
  }

  void Initialize(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(5.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;

    LOG(WARNING) << absl::StrFormat(
        "Initialized 1st-order ADRC coefficients: sample_time_sec = %lf; "
        "settle_time_sec = %lf; b0 = %lf; observer_bandwidth_factor = %lf; l1_ "
        "= %lf; l2_ = %lf; x1_hat_ = %lf; x2_hat_ = %lf; u_precompute_ = %lf",
        first_order_adrc_params_.sample_time_sec(),
        first_order_adrc_params_.settle_time_sec(),
        first_order_adrc_params_.b0(),
        first_order_adrc_params_.observer_bandwidth_factor(), l1_, l2_, x1_hat_,
        x2_hat_, u_precompute_);
  }

  double ExecuteFirstOrderAdrcLoop(double setpoint, double input,
                                   bool enable_calculation_log = false);

  double GetSampleTimeSec() const {
    return first_order_adrc_params_.sample_time_sec();
  }
  double GetSettleTimeSec() const {
    return first_order_adrc_params_.settle_time_sec();
  }
  double GetB0() const { return first_order_adrc_params_.b0(); }
  double GetObserverBandwidthFactor() const {
    return first_order_adrc_params_.observer_bandwidth_factor();
  }
  double GetOutputLimitMax() const {
    return first_order_adrc_params_.u_limit_max();
  }
  double GetOutputLimitMin() const {
    return first_order_adrc_params_.u_limit_min();
  }

 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_