tlbmc/thermal/controller/algorithm/second_order_adrc.cc

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

#include <algorithm>
#include <vector>

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

namespace milotic_tlbmc {
namespace thermal {

double SecondOrderAdrcThermalLoop::ExecuteSecondOrderAdrcLoop(
    double setpoint, double input, absl::Span<const double> disturbances,
    bool enable_calculation_log) {
  // Attempt to filter `input` if NTD is configured.
  if (tracking_differentiator_.has_value()) {
    NtdOutput ntd_out = tracking_differentiator_->FilterInput(input);

    // Note that the filtered derivative is not used explicitly in 2nd order
    // ADRC, as `x_hat_[1]` already provides the derivative estimation.
    input = ntd_out.filtered_output;
  }

  // Derive the error
  double y_hat = x_hat_[0] / kp_over_b0_;
  double error = input - y_hat;

  // Calculate and clamp `u(t)`
  double u_t = u_precompute_ - (gain_sum_ * error);
  u_t = std::clamp(u_t, second_order_adrc_params_.u_limit_min(),
                   second_order_adrc_params_.u_limit_max());

  // Calculate disturbance sum for observer update and precomputation.
  double disturbance_sum = 0.0;
  if (!disturbances.empty()) {
    for (int i = 0; i < disturbance_factors_.size(); ++i) {
      disturbance_sum +=
          disturbance_factors_[i].d *
          (disturbance_factors_[i].reference_setpoint - disturbances[i]);
    }
  }

  // Update observer states
  double corr[3];
  for (int i = 0; i < 3; ++i) {
    corr[i] = x_hat_[i] + l_hat_[i] * error;
  }

  double next_x[3];
  for (int i = 0; i < 3; ++i) {
    next_x[i] =
        b_hat_[i] * (u_t + disturbance_sum / second_order_adrc_params_.b0());
    for (int j = 0; j < 3; ++j) {
      next_x[i] += a_hat_[i][j] * corr[j];
    }
  }

  // Shift states
  x_hat_[0] = next_x[0];
  x_hat_[1] = next_x[1];
  if (!IsSaturated(u_t)) {
    x_hat_[2] = next_x[2];
  }

  // Precompute for the next sampling period (i.e., `u(t + 1)`)
  u_precompute_ = (kp_over_b0_ * setpoint) -
                  (x_hat_[0] + x_hat_[1] + x_hat_[2]) -
                  (disturbance_sum / second_order_adrc_params_.b0());

  if (enable_calculation_log) {
    LOG(WARNING)
        << "[Thermal Debug]: "
        << absl::StrFormat(
               "Executed 2nd-order ADRC loop: setpoint = %lf; input = %lf; "
               "disturbance_sum = %lf; y_hat = %lf; error = %lf; u_t = %lf; "
               "x_hat_ = [%lf, %lf, %lf]; u_precompute_ = %lf",
               setpoint, input, disturbance_sum, y_hat, error, u_t, x_hat_[0],
               x_hat_[1], x_hat_[2], u_precompute_);
  }

  return u_t;
}

}  // namespace thermal
}  // namespace milotic_tlbmc