tlbmc/thermal/controller/algorithm/first_order_adrc.cc

#include "tlbmc/thermal/controller/algorithm/first_order_adrc.h"

#include <algorithm>

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

namespace milotic_tlbmc {
namespace thermal {

double FirstOrderAdrcThermalLoop::ExecuteFirstOrderAdrcLoop(
    double setpoint, double input, bool enable_calculation_log) {
  // Attempt to filter `input` if NTD is configured.
  if (tracking_differentiator_.has_value()) {
    NtdOutput ntd_out = tracking_differentiator_->FilterInput(input);

    // Only use the filtered sensor reading, without the derivative for
    // phase-advancing, as which may amplify the noise in the worst case.
    input = ntd_out.filtered_output;
  }

  double error = input - x1_hat_;

  // Calculate derivatives based on the continuous Luenberger observer equations
  // & forward Euler numerical integration.
  double x1_hat_dot =
      x1_hat_ +
      first_order_adrc_params_.sample_time_sec() *
          (x2_hat_ + first_order_adrc_params_.b0() * u_precompute_) +
      l1_ * error;
  double x2_hat_dot = x2_hat_ + l2_ * error;

  // Proportional control.
  double u = kp_ * (setpoint - x1_hat_);

  // Reject the estimated disturbance.
  u = (u - x2_hat_) / first_order_adrc_params_.b0();

  // Clamp the output to the actuator limits.
  u = std::clamp(u, first_order_adrc_params_.u_limit_min(),
                 first_order_adrc_params_.u_limit_max());

  // Update `u_precompute_` of the next-cycled observer for anti-windup.
  u_precompute_ = u;

  // Update the estimated state variables.
  x1_hat_ = x1_hat_dot;
  if (!IsSaturated(u)) {
    x2_hat_ = x2_hat_dot;
  }

  if (enable_calculation_log) {
    LOG(WARNING) << "[Thermal Debug]: "
                 << absl::StrFormat(
                        "Executed 1st-order ADRC loop: setpoint = %lf; input = "
                        "%lf; error = %lf; u = %lf; x1_hat_ = %lf; x2_hat_ = "
                        "%lf; u_precompute_ = %lf",
                        setpoint, input, error, u, x1_hat_, x2_hat_,
                        u_precompute_);
  }

  return u;
}

}  // namespace thermal
}  // namespace milotic_tlbmc