tlbmc/thermal/controller/algorithm/second_order_adrc.h - gbmcweb - Git at Google

 #ifndef THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_ADRC_H_
 #define THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_ADRC_H_

 #include <cmath>
 #include <optional>
 #include <vector>

 #include "absl/log/log.h"
 #include "absl/strings/str_format.h"
 #include "absl/types/span.h"
 #include "thermal_config.pb.h"
 #include "tlbmc/thermal/controller/algorithm/utils/nonlinear_tracking_differentiator.h"

 namespace milotic_tlbmc {
 namespace thermal {

 struct DisturbanceFactors {
   double d;  // Control gain for this disturbance.
   double reference_setpoint;
 };

 /*
  * `SecondOrderAdrcThermalLoop` is an adaptive controller based on 2nd-order
  * Active Disturbance Rejection Control (ADRC), capable of adjusting its
  * coefficients during runtime to adapt to transient system dynamics. It serves
  * as a powerful alternative to both PID control and modern model-based control,
  * as it requires little tuning, and is robust to external disturbances and
  * system parameter variations (e.g., hardware manufactural differences, aging).
  *
  * The strategy of implemented ADRC here is linear state error feedback (LESF)
  * that primarily counteracts disturbances and eliminates the output
  * error of the tracking differentiator (TD) and extended state observer (ESO).
  *
  * 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 SecondOrderAdrcThermalLoop {
  public:
   explicit SecondOrderAdrcThermalLoop(
       const SecondOrderAdrcLoopConfig& second_order_adrc_params)
       : second_order_adrc_params_(second_order_adrc_params) {
     Initialize(second_order_adrc_params);
   }

   explicit SecondOrderAdrcThermalLoop(
       const SecondOrderAdrcLoopConfig& second_order_adrc_params,
       const std::vector<DisturbanceFactors>& disturbance_factors)
       : second_order_adrc_params_(second_order_adrc_params),
         disturbance_factors_(disturbance_factors) {
     Initialize(second_order_adrc_params);
   }

   void Initialize(const SecondOrderAdrcLoopConfig& second_order_adrc_params) {
     if (second_order_adrc_params.sample_time_sec() <= 0) {
       second_order_adrc_params_.set_sample_time_sec(1.0);
     }
     if (second_order_adrc_params.settle_time_sec() <= 0) {
       second_order_adrc_params_.set_settle_time_sec(5.0);
     }

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

     double controller_bandwidth =
         6.0 / second_order_adrc_params_.settle_time_sec();
     double observer_bandwidth =
         second_order_adrc_params_.observer_bandwidth_factor() *
         controller_bandwidth;

     double kp = std::pow(controller_bandwidth, 2);
     double kd = 2.0 * controller_bandwidth;
     // Precompute `K_{p} / b_{0}` for the update step.
     kp_over_b0_ = kp / second_order_adrc_params_.b0();

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

     double l1 = 1.0 - std::pow(z_pole, 3);
     double l2 = (3.0 / (2.0 * second_order_adrc_params_.sample_time_sec())) *
                 std::pow(1.0 - z_pole, 2) * (1.0 + z_pole);
     double l3 =
         (1.0 / std::pow(second_order_adrc_params_.sample_time_sec(), 2)) *
         std::pow(1.0 - z_pole, 3);

     // Coordinate transformation scaling
     l_hat_[0] = (kp / second_order_adrc_params_.b0()) * l1;
     l_hat_[1] = (kd / second_order_adrc_params_.b0()) * l2;
     l_hat_[2] = (1.0 / second_order_adrc_params_.b0()) * l3;
     gain_sum_ = l_hat_[0] + l_hat_[1] + l_hat_[2];

     // `\hat{A} = T^{-1} * A_{d} * T`
     a_hat_[0][0] = 1.0;
     a_hat_[0][1] = second_order_adrc_params_.sample_time_sec() * (kp / kd);
     a_hat_[0][2] = second_order_adrc_params_.sample_time_sec() *
                    second_order_adrc_params_.sample_time_sec() / 2.0 * kp;
     a_hat_[1][0] = 0.0;
     a_hat_[1][1] = 1.0;
     a_hat_[1][2] = second_order_adrc_params_.sample_time_sec() * kd;
     a_hat_[2][0] = 0.0;
     a_hat_[2][1] = 0.0;
     a_hat_[2][2] = 1.0;

     // `\hat{B} = T^{-1} * B_{d}`
     b_hat_[0] = (kp / second_order_adrc_params_.b0()) *
                 (second_order_adrc_params_.b0() *
                  second_order_adrc_params_.sample_time_sec() *
                  second_order_adrc_params_.sample_time_sec() / 2.0);
     b_hat_[1] = (kd / second_order_adrc_params_.b0()) *
                 (second_order_adrc_params_.b0() *
                  second_order_adrc_params_.sample_time_sec());
     b_hat_[2] = 0.0;

     LOG(WARNING) << absl::StrFormat(
         "Initialized 2nd-order ADRC coefficients: sample_time_sec = %lf; "
         "settle_time_sec = %lf; b0 = %lf; observer_bandwidth_factor = %lf; "
         "l_hat_ = [%lf, %lf, %lf]; gain_sum_ = %lf; kp_over_b0_ = %lf; "
         "u_precompute_ = %lf; a_hat_ = [[%lf, %lf, %lf], [%lf, %lf, %lf], "
         "[%lf, %lf, %lf]]; b_hat_ = [%lf, %lf, %lf]",
         second_order_adrc_params_.sample_time_sec(),
         second_order_adrc_params_.settle_time_sec(),
         second_order_adrc_params_.b0(),
         second_order_adrc_params_.observer_bandwidth_factor(), l_hat_[0],
         l_hat_[1], l_hat_[2], gain_sum_, kp_over_b0_, u_precompute_,
         a_hat_[0][0], a_hat_[0][1], a_hat_[0][2], a_hat_[1][0], a_hat_[1][1],
         a_hat_[1][2], a_hat_[2][0], a_hat_[2][1], a_hat_[2][2], b_hat_[0],
         b_hat_[1], b_hat_[2]);
   }

   // `ExecuteSecondOrderAdrcLoop` executes one-step of the 2nd-order ADRC
   // controller, and returns the computed control output `u(t)`.
   // By providing `disturbances`, the ADRC controller can explicitly compensate
   // for the effects of external disturbances with specified tuned factors.
   //
   // The formula is:
   // `u(t+1)
   //  = \hat{A} * \hat{x}(t) +
   //    \hat{B} * (u(t) + d(t)) +
   //    \hat{L} * (y(t) - \hat{y}(t))`
   //
   // Note that if `disturbances` is emtpy, we have `d(t) = 0`.
   double ExecuteSecondOrderAdrcLoop(double setpoint, double input,
                                     absl::Span<const double> disturbances,
                                     bool enable_calculation_log = false);

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

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

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

   // Precomputed Transformed Gains and Matrices
   double gain_sum_;  // `\sum_{i=1}^{3} \hat{L}_{i}`
   double kp_over_b0_;
   double a_hat_[3][3];
   double b_hat_[3];
   double l_hat_[3];

   // State Variables
   double x_hat_[3] = {0.0, 0.0, 0.0};
   double u_precompute_ = 0.0;  // `u(k|k - 1)`

   std::vector<DisturbanceFactors> disturbance_factors_;
 };

 }  // namespace thermal
 }  // namespace milotic_tlbmc

 #endif  // THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_ADRC_H_
	#ifndef THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_ADRC_H_
	#define THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_ADRC_H_

	#include <cmath>
	#include <optional>
	#include <vector>

	#include "absl/log/log.h"
	#include "absl/strings/str_format.h"
	#include "absl/types/span.h"
	#include "thermal_config.pb.h"
	#include "tlbmc/thermal/controller/algorithm/utils/nonlinear_tracking_differentiator.h"

	namespace milotic_tlbmc {
	namespace thermal {

	struct DisturbanceFactors {
	double d; // Control gain for this disturbance.
	double reference_setpoint;
	};

	/*
	* `SecondOrderAdrcThermalLoop` is an adaptive controller based on 2nd-order
	* Active Disturbance Rejection Control (ADRC), capable of adjusting its
	* coefficients during runtime to adapt to transient system dynamics. It serves
	* as a powerful alternative to both PID control and modern model-based control,
	* as it requires little tuning, and is robust to external disturbances and
	* system parameter variations (e.g., hardware manufactural differences, aging).
	*
	* The strategy of implemented ADRC here is linear state error feedback (LESF)
	* that primarily counteracts disturbances and eliminates the output
	* error of the tracking differentiator (TD) and extended state observer (ESO).
	*
	* 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 SecondOrderAdrcThermalLoop {
	public:
	explicit SecondOrderAdrcThermalLoop(
	const SecondOrderAdrcLoopConfig& second_order_adrc_params)
	: second_order_adrc_params_(second_order_adrc_params) {
	Initialize(second_order_adrc_params);
	}

	explicit SecondOrderAdrcThermalLoop(
	const SecondOrderAdrcLoopConfig& second_order_adrc_params,
	const std::vector<DisturbanceFactors>& disturbance_factors)
	: second_order_adrc_params_(second_order_adrc_params),
	disturbance_factors_(disturbance_factors) {
	Initialize(second_order_adrc_params);
	}

	void Initialize(const SecondOrderAdrcLoopConfig& second_order_adrc_params) {
	if (second_order_adrc_params.sample_time_sec() <= 0) {
	second_order_adrc_params_.set_sample_time_sec(1.0);
	}
	if (second_order_adrc_params.settle_time_sec() <= 0) {
	second_order_adrc_params_.set_settle_time_sec(5.0);
	}

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

	double controller_bandwidth =
	6.0 / second_order_adrc_params_.settle_time_sec();
	double observer_bandwidth =
	second_order_adrc_params_.observer_bandwidth_factor() *
	controller_bandwidth;

	double kp = std::pow(controller_bandwidth, 2);
	double kd = 2.0 * controller_bandwidth;
	// Precompute `K_{p} / b_{0}` for the update step.
	kp_over_b0_ = kp / second_order_adrc_params_.b0();

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

	double l1 = 1.0 - std::pow(z_pole, 3);
	double l2 = (3.0 / (2.0 * second_order_adrc_params_.sample_time_sec())) *
	std::pow(1.0 - z_pole, 2) * (1.0 + z_pole);
	double l3 =
	(1.0 / std::pow(second_order_adrc_params_.sample_time_sec(), 2)) *
	std::pow(1.0 - z_pole, 3);

	// Coordinate transformation scaling
	l_hat_[0] = (kp / second_order_adrc_params_.b0()) * l1;
	l_hat_[1] = (kd / second_order_adrc_params_.b0()) * l2;
	l_hat_[2] = (1.0 / second_order_adrc_params_.b0()) * l3;
	gain_sum_ = l_hat_[0] + l_hat_[1] + l_hat_[2];

	// `\hat{A} = T^{-1} * A_{d} * T`
	a_hat_[0][0] = 1.0;
	a_hat_[0][1] = second_order_adrc_params_.sample_time_sec() * (kp / kd);
	a_hat_[0][2] = second_order_adrc_params_.sample_time_sec() *
	second_order_adrc_params_.sample_time_sec() / 2.0 * kp;
	a_hat_[1][0] = 0.0;
	a_hat_[1][1] = 1.0;
	a_hat_[1][2] = second_order_adrc_params_.sample_time_sec() * kd;
	a_hat_[2][0] = 0.0;
	a_hat_[2][1] = 0.0;
	a_hat_[2][2] = 1.0;

	// `\hat{B} = T^{-1} * B_{d}`
	b_hat_[0] = (kp / second_order_adrc_params_.b0()) *
	(second_order_adrc_params_.b0() *
	second_order_adrc_params_.sample_time_sec() *
	second_order_adrc_params_.sample_time_sec() / 2.0);
	b_hat_[1] = (kd / second_order_adrc_params_.b0()) *
	(second_order_adrc_params_.b0() *
	second_order_adrc_params_.sample_time_sec());
	b_hat_[2] = 0.0;

	LOG(WARNING) << absl::StrFormat(
	"Initialized 2nd-order ADRC coefficients: sample_time_sec = %lf; "
	"settle_time_sec = %lf; b0 = %lf; observer_bandwidth_factor = %lf; "
	"l_hat_ = [%lf, %lf, %lf]; gain_sum_ = %lf; kp_over_b0_ = %lf; "
	"u_precompute_ = %lf; a_hat_ = [[%lf, %lf, %lf], [%lf, %lf, %lf], "
	"[%lf, %lf, %lf]]; b_hat_ = [%lf, %lf, %lf]",
	second_order_adrc_params_.sample_time_sec(),
	second_order_adrc_params_.settle_time_sec(),
	second_order_adrc_params_.b0(),
	second_order_adrc_params_.observer_bandwidth_factor(), l_hat_[0],
	l_hat_[1], l_hat_[2], gain_sum_, kp_over_b0_, u_precompute_,
	a_hat_[0][0], a_hat_[0][1], a_hat_[0][2], a_hat_[1][0], a_hat_[1][1],
	a_hat_[1][2], a_hat_[2][0], a_hat_[2][1], a_hat_[2][2], b_hat_[0],
	b_hat_[1], b_hat_[2]);
	}

	// `ExecuteSecondOrderAdrcLoop` executes one-step of the 2nd-order ADRC
	// controller, and returns the computed control output `u(t)`.
	// By providing `disturbances`, the ADRC controller can explicitly compensate
	// for the effects of external disturbances with specified tuned factors.
	//
	// The formula is:
	// `u(t+1)
	// = \hat{A} * \hat{x}(t) +
	// \hat{B} * (u(t) + d(t)) +
	// \hat{L} * (y(t) - \hat{y}(t))`
	//
	// Note that if `disturbances` is emtpy, we have `d(t) = 0`.
	double ExecuteSecondOrderAdrcLoop(double setpoint, double input,
	absl::Span<const double> disturbances,
	bool enable_calculation_log = false);

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

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

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

	// Precomputed Transformed Gains and Matrices
	double gain_sum_; // `\sum_{i=1}^{3} \hat{L}_{i}`
	double kp_over_b0_;
	double a_hat_[3][3];
	double b_hat_[3];
	double l_hat_[3];

	// State Variables
	double x_hat_[3] = {0.0, 0.0, 0.0};
	double u_precompute_ = 0.0; // `u(k\|k - 1)`

	std::vector<DisturbanceFactors> disturbance_factors_;
	};

	} // namespace thermal
	} // namespace milotic_tlbmc

	#endif // THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_ADRC_H_