Add FRE into copybara & meson.build Google-Bug-Id:559754428 PiperOrigin-RevId: 981427428 Change-Id: I080d721dd9b1e75d61e0eb4d09b0841895258eec
diff --git a/copy.bara.sky b/copy.bara.sky index 737cba5..3e66fc0 100644 --- a/copy.bara.sky +++ b/copy.bara.sky
@@ -865,6 +865,10 @@ "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/first_order_adrc_controller.cc", "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/first_order_adrc_controller.h", "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/optimizer/optimizer_logger.h", + "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.cc", + "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.h", + "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.cc", + "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.h", "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/optimizer/pid/pid_optimizer.h", "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/optimizer/pid/ziegler_nichols/differentiated_ziegler_nichols.cc", "google3/third_party/milotic/external/cc/tlbmc/thermal/controller/optimizer/pid/ziegler_nichols/differentiated_ziegler_nichols.h",
diff --git a/tlbmc/meson.build b/tlbmc/meson.build index 5bfad05..42ed7c6 100644 --- a/tlbmc/meson.build +++ b/tlbmc/meson.build
@@ -285,6 +285,8 @@ 'thermal/controller/fan_pid_controller.cc', 'thermal/controller/first_order_adrc_controller.cc', 'thermal/controller/optimizer/optimizer_logger.h', + 'thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.cc', + 'thermal/controller/optimizer/pid/frequency_response_estimation/utils.cc', 'thermal/controller/optimizer/pid/ziegler_nichols/differentiated_ziegler_nichols.cc', 'thermal/controller/optimizer/pid/ziegler_nichols/ziegler_nichols.cc', 'thermal/controller/pid_controller.cc',
diff --git a/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.cc b/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.cc new file mode 100644 index 0000000..abdbe07 --- /dev/null +++ b/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.cc
@@ -0,0 +1,247 @@ +#include "tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.h" + +#include <algorithm> +#include <array> +#include <cmath> +#include <complex> +#include <cstdint> +#include <memory> +#include <vector> + +#include "absl/memory/memory.h" +#include "absl/status/status.h" +#include "absl/status/statusor.h" +#include "absl/strings/str_format.h" +#include "thermal_config.pb.h" +#include "tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.h" +#include "tlbmc/thermal/controller/optimizer/pid/pid_optimizer.h" + +namespace milotic_tlbmc { +namespace thermal { +namespace optimizer { + +absl::StatusOr<std::unique_ptr<PidAutotunerFrequencyResponseEstimation>> +PidAutotunerFrequencyResponseEstimation::Create( + const FrequencyResponseEstimationConstructParameters& params) { + if (params.target_bandwidth <= 0) { + return absl::InvalidArgumentError( + "Target bandwidth must be positive for Frequency Response Estimation."); + } + if (params.target_phase_margin_deg <= 0 || + params.target_phase_margin_deg >= 90) { + return absl::InvalidArgumentError( + "Target phase margin must be between 0 and 90 degrees."); + } + auto tuner = + absl::WrapUnique(new PidAutotunerFrequencyResponseEstimation(params)); + return tuner; +} + +PidAutotunerFrequencyResponseEstimation:: + PidAutotunerFrequencyResponseEstimation( + const FrequencyResponseEstimationConstructParameters& params) + : target_bandwidth_(params.target_bandwidth), + target_phase_margin_deg_(params.target_phase_margin_deg), + sine_amplitude_(params.sine_amplitude), + tuning_duration_ms_(params.tuning_duration_ms), + target_input_value_(params.target_input_value), + loop_interval_(params.loop_interval), + max_output_(params.max_output), + min_output_(params.min_output), + nominal_output_(params.nominal_output), + loop_sign_(params.loop_sign), + rls_estimator_(params.rls_forgetting_factor, + params.rls_initial_covariance), + rls_forgetting_factor_(params.rls_forgetting_factor), + rls_initial_covariance_(params.rls_initial_covariance), + check_convergence_(params.check_convergence), + convergence_tolerance_(params.convergence_tolerance), + min_experiment_duration_ms_(params.min_experiment_duration_ms), + enable_dc_offset_(params.enable_dc_offset), + dc_offset_(params.dc_offset) { + // Test frequencies: `\{0.1, 0.33, 1.0, 3.0, 10.0\} * wc` + test_frequencies_.reserve(kFrequencyMultiplierSize); + for (double m : kFrequencyMultipliers) { + test_frequencies_.push_back(m * target_bandwidth_); + } + estimated_responses_.assign(kFrequencyMultiplierSize, + std::complex<double>(0.0, 0.0)); + prev_responses_.assign(kFrequencyMultiplierSize, + std::complex<double>(0.0, 0.0)); +} + +// Resets tuning lifecycle and clears estimator states for a fresh run. +void PidAutotunerFrequencyResponseEstimation::Initialize( + const PidOptimizerInitializationParameters& params) { + finished_tuning_ = false; + converged_ = false; + start_time_initialized_ = false; + start_time_ms_ = 0; + last_convergence_check_ms_ = 0; + consecutive_stable_checks_ = 0; + prev_responses_initialized_ = false; + coeff_p_ = params.coeff_p; + coeff_i_ = params.coeff_i; + coeff_d_ = params.coeff_d; + estimated_dc_gain_ = 0.0; + rls_estimator_.Reset(rls_initial_covariance_); + std::fill(estimated_responses_.begin(), estimated_responses_.end(), + std::complex<double>(0.0, 0.0)); + std::fill(prev_responses_.begin(), prev_responses_.end(), + std::complex<double>(0.0, 0.0)); +} + +double PidAutotunerFrequencyResponseEstimation::TunePid( + const PidOptimizerTuningParameters& params) { + if (finished_tuning_) { + return nominal_output_; + } + + if (!start_time_initialized_) { + start_time_ms_ = params.current_time_ms; + start_time_initialized_ = true; + last_convergence_check_ms_ = params.current_time_ms; + } + uint32_t elapsed_ms = params.current_time_ms - start_time_ms_; + double elapsed_sec = elapsed_ms / 1000.0; + + // Generate a sinusoid perturbation + double delta_u = 0.0; + double component_amp = sine_amplitude_ / kFrequencyMultiplierSize; + for (int i = 0; i < kFrequencyMultiplierSize; ++i) { + delta_u += component_amp * std::sin(test_frequencies_[i] * elapsed_sec); + } + if (enable_dc_offset_) { + delta_u += dc_offset_; + } + + double current_u = + std::clamp(nominal_output_ + delta_u, min_output_, max_output_); + double current_y = params.input; + + // Deviations from operating baseline: + double delta_u_val = current_u - nominal_output_; + double delta_y_val = current_y - target_input_value_; + + // Construct regressor vector `H(t) \in \mathbb{R}^{11}`. + std::array<double, kRegressorDimension> regressor{}; + for (int i = 0; i < kFrequencyMultiplierSize; ++i) { + double w = test_frequencies_[i]; + regressor[2 * i] = std::cos(w * elapsed_sec); + regressor[2 * i + 1] = std::sin(w * elapsed_sec); + } + regressor[2 * kFrequencyMultiplierSize] = 1.0; + + rls_estimator_.Update(regressor, delta_u_val, delta_y_val); + + // Extract frequency responses and DC gain. + const auto& theta_u = rls_estimator_.GetThetaU(); + const auto& theta_y = rls_estimator_.GetThetaY(); + for (int i = 0; i < kFrequencyMultiplierSize; ++i) { + std::complex<double> u_phasor(theta_u[2 * i], -theta_u[2 * i + 1]); + std::complex<double> y_phasor(theta_y[2 * i], -theta_y[2 * i + 1]); + if (std::abs(u_phasor) > kEps) { + estimated_responses_[i] = y_phasor / u_phasor; + } else { + estimated_responses_[i] = std::complex<double>(1.0, 0.0); + } + } + if (std::abs(theta_u[2 * kFrequencyMultiplierSize]) > kEps) { + estimated_dc_gain_ = theta_y[2 * kFrequencyMultiplierSize] / + theta_u[2 * kFrequencyMultiplierSize]; + } + + // Convergence eval + double min_duration_sec = + std::max(static_cast<double>(min_experiment_duration_ms_) / 1000.0, + 2.0 * (2.0 * kPi / test_frequencies_[0])); + + if (check_convergence_ && elapsed_sec >= min_duration_sec && + (params.current_time_ms - last_convergence_check_ms_ >= 1000 || + last_convergence_check_ms_ == start_time_ms_)) { + last_convergence_check_ms_ = params.current_time_ms; + if (prev_responses_initialized_) { + double max_relative_change = 0.0; + + for (int i = 0; i < kFrequencyMultiplierSize; ++i) { + double diff = std::abs(estimated_responses_[i] - prev_responses_[i]); + double denom = std::abs(estimated_responses_[i]) + kEps; + + max_relative_change = std::max(max_relative_change, diff / denom); + } + + if (max_relative_change < convergence_tolerance_) { + ++consecutive_stable_checks_; + + if (consecutive_stable_checks_ >= 3) { + converged_ = true; + finished_tuning_ = true; + OptimizeGains(); + + LogTuningMessage( + absl::StrFormat("FRE PID autotuner converged at t=%.2fs; final " + "coefficients: Kp=%f, Ki=%f, Kd=%f", + elapsed_sec, coeff_p_, coeff_i_, coeff_d_)); + return nominal_output_; + } + } else { + consecutive_stable_checks_ = 0; + } + } + prev_responses_ = estimated_responses_; + prev_responses_initialized_ = true; + } + + // Timeout + if (elapsed_ms >= tuning_duration_ms_ && !finished_tuning_) { + finished_tuning_ = true; + OptimizeGains(); + + LogTuningMessage(absl::StrFormat( + "Timeout. FRE PID autotuner final coefficients: Kp=%f, Ki=%f, Kd=%f", + coeff_p_, coeff_i_, coeff_d_)); + return nominal_output_; + } + return current_u; +} + +void PidAutotunerFrequencyResponseEstimation::OptimizeGains() { + // Index 2 corresponds to multiplier `1.0 * target_bandwidth_ (\omega_c)`. + std::complex<double> g_crossover = estimated_responses_[2]; + double g_magnitude = std::max(std::abs(g_crossover), kEps); + double g_phase = std::arg(g_crossover); + + // Desired controller magnitude and phase at crossover frequency `\omega_c`. + double target_pm_rad = target_phase_margin_deg_ * kPi / 180.0; + double phi_target = -kPi + target_pm_rad - g_phase; + double c_magnitude = 1.0 / g_magnitude; + double sin_phi = std::sin(phi_target); + + // Proportional gain matches the real part of `C(j * \omega_c)`. + double kp = c_magnitude * std::cos(phi_target); + double ki = 0.0; + double kd = 0.0; + double wc = target_bandwidth_; + + // Resolve imaginary part: + // `\omega_c * K_d - K_i / \omega_c = |C| * \sin(\phi_{\text{target}})` + if (sin_phi < 0.0) { + // Phase lag: Integral term provides dominant phase lag, and derivative + // term provides high-frequency damping. + ki = -wc * c_magnitude * sin_phi; + kd = kp / (4.0 * wc); + } else { + // Phase lead: Fix baseline integrator corner `\omega_i = 0.1 * \omega_c` + // and solve for required derivative gain `K_d`. + ki = 0.1 * kp * wc; + kd = (c_magnitude * sin_phi + ki / wc) / wc; + } + + coeff_p_ = loop_sign_ * std::abs(kp); + coeff_i_ = loop_sign_ * std::abs(ki); + coeff_d_ = loop_sign_ * std::abs(kd); +} + +} // namespace optimizer +} // namespace thermal +} // namespace milotic_tlbmc
diff --git a/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.h b/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.h new file mode 100644 index 0000000..0725c22 --- /dev/null +++ b/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/frequency_response_estimation.h
@@ -0,0 +1,177 @@ +#ifndef THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_OPTIMIZER_PID_FREQUENCY_RESPONSE_ESTIMATION_FREQUENCY_RESPONSE_ESTIMATION_H_ +#define THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_OPTIMIZER_PID_FREQUENCY_RESPONSE_ESTIMATION_FREQUENCY_RESPONSE_ESTIMATION_H_ + +#include <array> +#include <complex> +#include <cstdint> +#include <memory> +#include <vector> + +#include "absl/status/statusor.h" +#include "thermal_config.pb.h" +#include "tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.h" +#include "tlbmc/thermal/controller/optimizer/pid/pid_optimizer.h" + +namespace milotic_tlbmc { +namespace thermal { +namespace optimizer { + +constexpr double kPi = 3.14159265358979323846; +constexpr int kFrequencyMultiplierSize = 5; +constexpr int kRegressorDimension = 2 * kFrequencyMultiplierSize + 1; +constexpr std::array<double, kFrequencyMultiplierSize> kFrequencyMultipliers = { + 0.1, 0.33, 1.0, 3.0, 10.0}; + +struct FrequencyResponseEstimationConstructParameters { + double target_bandwidth = + 0.2; // Target crossover frequency `\omega_c` (rad/s) + double target_phase_margin_deg = 60.0; // Target phase margin (degrees) + double sine_amplitude = 5.0; // Perturbation amplitude + uint32_t tuning_duration_ms = + 100000; // Max duration of perturbation experiment in ms + double target_input_value = 0.0; // Target setpoint + uint32_t loop_interval = 100; // Loop interval in ms + double max_output = 100.0; + double min_output = 0.0; + double nominal_output = 50.0; // Baseline control output `u_0` + double loop_sign = 1.0; // Sign of the coefficient gains + + double rls_forgetting_factor = + 1.0; // RLS forgetting factor `\lambda \in (0, 1]` + double rls_initial_covariance = 1000.0; // Initial covariance diagonal `P_0` + bool check_convergence = true; // Dynamic convergence detection + double convergence_tolerance = 1e-3; // Maximum relative change threshold + uint32_t min_experiment_duration_ms = 0; // Minimum experiment duration in ms + bool enable_dc_offset = false; // Whether to inject small DC perturbation + double dc_offset = 0.0; // DC offset value for `G(0)` estimation +}; + +/* + * `PidAutotunerFrequencyResponseEstimation` implements a PID autotuner based + * on frequency response estimation. + * + * Traditional PID tuning methods (e.g., Ziegler-Nichols relay experiments) + * force the plant into continuous non-linear oscillations at its critical + * frequency, which can induce severe thermal/mechanical stress on physical + * hardware. The Frequency Response Estimation (FRE) autotuner provides a + * non-parametric, frequency-domain identification approach. It identifies the + * plant's complex frequency response `G(j * \omega_k)` across key frequencies + * and analytically solves for PID gains `K_p, K_i, K_d` to achieve a specified + * gain crossover bandwidth `\omega_c` and phase margin `\phi_m`. + * + * The algorithm executes in 2 phases: + * + * 1. Online Frequency Response Estimation: Injects sine-wave perturbations + * and uses an RLS estimator & regressors to identify the frequency + * response `G(j * \omega_k)` at plant DC gain `G(0)` and frequencies + * `\{0.1, \frac{1}{3}, 1.0, 3.0, 10.0\} * \omega_c`. + * More explanations available at: + * `third_party/milotic/external/cc/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.h`. + * + * 2. Online Gain Optimization: Solves for `K_p`, `K_i`, `K_d` coefficients + * that satisfy target crossover bandwidth `\omega_c` and phase margin + * constraints. + * During the optimization, the loop gain + * `L(j * \omega_c) = C(j * \omega_c) * G(j * \omega_c)` + * is enforced to satisfy unity gain + * `|L(j * \omega_c)| = 1` + * and target phase margin + * `\angle L(j * \omega_c) = -\pi + \phi_m` + * at crossover bandwidth `\omega_c`. + * + * This class is not thread-safe. + * + * The comment format of numeric expressions follows LaTeX style. + */ +class PidAutotunerFrequencyResponseEstimation : public PidOptimizer { + public: + static absl::StatusOr< + std::unique_ptr<PidAutotunerFrequencyResponseEstimation>> + Create(const FrequencyResponseEstimationConstructParameters& params); + + void Initialize(const PidOptimizerInitializationParameters& params) final; + double TunePid(const PidOptimizerTuningParameters& params) final; + + bool FinishedTuning() const final { return finished_tuning_; } + bool IsConverged() const { return converged_; } + + double GetCoeffP() const final { return coeff_p_; } + double GetCoeffI() const final { return coeff_i_; } + double GetCoeffD() const final { return coeff_d_; } + + double GetEstimatedDcGain() const { return estimated_dc_gain_; } + + const std::vector<std::complex<double>>& GetEstimatedResponses() const { + return estimated_responses_; + } + + void SetEstimatedResponsesForTesting( + const std::vector<std::complex<double>>& responses) { + estimated_responses_ = responses; + OptimizeGains(); + } + + protected: + explicit PidAutotunerFrequencyResponseEstimation( + const FrequencyResponseEstimationConstructParameters& params); + + private: + void OptimizeGains(); + + // `target_bandwidth` (i.e., `\omega_c`) defines the speed of response and + // disturbance rejection bandwidth, which must be strictly positive. + double target_bandwidth_; + // `target_phase_margin_deg` (i.e., `\phi_m`) determines the closed-loop + // stability and damping ratio (`\zeta \approx \phi_m / 100`). Phase margin + // outside this range (i.e., `(0, 90)`) results in unstable behaviors. + double target_phase_margin_deg_; + // `sine_amplitude` (i.e., `A_{total}`) is the peak amplitude of the + // composite multi-sinusoidal perturbation signal. + double sine_amplitude_; + + uint32_t tuning_duration_ms_; + double target_input_value_; + uint32_t loop_interval_; + double max_output_; + double min_output_; + double nominal_output_; + double loop_sign_; + + // RLS estimator params. + DualRlsEstimator<kRegressorDimension> rls_estimator_; + double rls_forgetting_factor_; + double rls_initial_covariance_; + bool check_convergence_; + double convergence_tolerance_; + uint32_t min_experiment_duration_ms_; + bool enable_dc_offset_; + double dc_offset_; + + bool finished_tuning_ = false; + bool converged_ = false; + uint32_t start_time_ms_ = 0; + bool start_time_initialized_ = false; + + // Multipliers: `\{0.1,0.33, 1.0, 3.0, 10.0\} * \omega_c` + std::vector<double> test_frequencies_; + + // Estimated frequency responses `G(j * \omega_k)` at each test frequency. + std::vector<std::complex<double>> estimated_responses_; + double estimated_dc_gain_ = 0.0; + + // Convergence tracking state + std::vector<std::complex<double>> prev_responses_; + bool prev_responses_initialized_ = false; + uint32_t last_convergence_check_ms_ = 0; + int consecutive_stable_checks_ = 0; + + double coeff_p_ = 0.0; + double coeff_i_ = 0.0; + double coeff_d_ = 0.0; +}; + +} // namespace optimizer +} // namespace thermal +} // namespace milotic_tlbmc + +#endif // THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_OPTIMIZER_PID_FREQUENCY_RESPONSE_ESTIMATION_FREQUENCY_RESPONSE_ESTIMATION_H_
diff --git a/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.cc b/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.cc new file mode 100644 index 0000000..80f701e --- /dev/null +++ b/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.cc
@@ -0,0 +1,79 @@ +#include "tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.h" + +#include <cmath> +#include <cstdlib> +#include <limits> + +namespace milotic_tlbmc { +namespace thermal { +namespace optimizer { + +double SafeHypotenuse(double u0, double u1) { + double a = std::abs(u0); + double b = std::abs(u1); + if (a < b) { + a /= b; + return std::sqrt(a * a + 1.0) * b; + } + if (a > b) { + b /= a; + return std::sqrt(b * b + 1.0) * a; + } + return std::isnan(b) ? std::numeric_limits<double>::quiet_NaN() + : std::sqrt(2.0) * a; +} + +double SafePow(double base, double exp) { + if (std::isnan(base) || std::isnan(exp)) { + return std::numeric_limits<double>::quiet_NaN(); + } + + double abs_base = std::abs(base); + + // Handle `\infty` + if (std::isinf(exp)) { + // `1 ^ {\infty}` is undefined without a very specific mathematical + // context. Examples below: + // 1. `\lim_{n \to \infty} (1 + \frac{1}{n})^n = e` + // 2. `\lim_{n \to \infty} (1 + \frac{1}{\sqrt{n}})^n = \infty` + // 3. `\lim_{n \to \infty} (1 + \frac{k}{n})^n = e^{k}` + if (std::abs(abs_base - 1.0) < kEps) { + return std::numeric_limits<double>::quiet_NaN(); + } + if (abs_base > 1.0) { + return exp > 0.0 ? std::numeric_limits<double>::infinity() : 0.0; + } + return exp > 0.0 ? 0.0 : std::numeric_limits<double>::infinity(); + } + + // `b^0 = 1` + if (std::abs(exp) < kEps) { + return 1.0; + } + + // `b^1 = b`, `b^{-1} = 1/b` + double abs_exp = std::abs(exp); + if (std::abs(abs_exp - 1.0) < kEps) { + return exp > 0.0 ? base : 1.0 / base; + } + + // `b^2 = b * b` + if (std::abs(exp - 2.0) < kEps) { + return base * base; + } + + // `b^{0.5} = \sqrt{b}` + if (std::abs(exp - 0.5) < kEps && base >= 0.0) { + return std::sqrt(base); + } + + // A domain error occurs in `std::pow` if the base is negative and the + // exp is fractional. + return (base < 0.0 && exp != std::floor(exp)) + ? std::numeric_limits<double>::quiet_NaN() + : std::pow(base, exp); +} + +} // namespace optimizer +} // namespace thermal +} // namespace milotic_tlbmc
diff --git a/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.h b/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.h new file mode 100644 index 0000000..4d8c952 --- /dev/null +++ b/tlbmc/thermal/controller/optimizer/pid/frequency_response_estimation/utils.h
@@ -0,0 +1,179 @@ +#ifndef THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_OPTIMIZER_PID_FREQUENCY_RESPONSE_ESTIMATION_UTILS_H_ +#define THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_OPTIMIZER_PID_FREQUENCY_RESPONSE_ESTIMATION_UTILS_H_ + +#include <array> +#include <cmath> +#include <cstddef> + +namespace milotic_tlbmc { +namespace thermal { +namespace optimizer { + +// The comment format of numeric expressions follows LaTeX style. + +constexpr double kEps = 1e-6; + +struct real_number { + double real; + double imag; +}; + +/* + * `SafeHypotenuse` returns `\sqrt{u0^2 + u1^2}` while avoiding + * overflow/underflow. + * + * `std::sqrt(u0 * u0 + u1 * u1)` overflows to `inf` when `u0 >= 10^200`, and + * underflows to `0` when `u0 <= 10^(-200)`. + */ +double SafeHypotenuse(double u0, double u1); + +/* + * `SafePow` returns `base^{exp}` while avoiding overflow with some fast path + * optimization. + */ +double SafePow(double base, double exp); + +/* + * `DualRlsEstimator` implements a dual Recursive Least Squares (RLS) parameter + * estimator with exponential forgetting and regularized covariance tracking. + * + * It simultaneously identifies parameter vectors for input and output: + * `\theta_u, \theta_y \in \mathbb{R}^M` + * that share a common harmonic regressor vector `H(t) \in \mathbb{R}^M`: + * `u(t) = H(t)^T * \theta_u + v_u(t)` + * `y(t) = H(t)^T * \theta_y + v_y(t)` + * + * For frequency response estimation at frequencies `\omega_k`: + * `H(t) = [\cos(\omega_1 t), \sin(\omega_1 t), \dots, \cos(\omega_K t), + * \sin(\omega_K t), 1]^T` + * + * `P(t)` denotes the normalized estimation error covariance matrix: + * `(\sum_{\tau=1}^t \lambda^{t-\tau} H(\tau) H(\tau)^T + \delta I)^{-1}` + * where `\lambda \in (0, 1]` is the forgetting factor and `\delta = 1 / P_0` is + * the initial regularization parameter. + */ +template <size_t M> +class DualRlsEstimator { + public: + explicit DualRlsEstimator(double forgetting_factor = 1.0, + double initial_covariance = 1000.0) + : forgetting_factor_(forgetting_factor) { + Reset(initial_covariance); + } + + // Resets parameter estimates to 0 and initializes `P` to + // `initial_covariance * I_M`. + void Reset(double initial_covariance = 1000.0) { + theta_u_.fill(0.0); + theta_y_.fill(0.0); + covariance_matrix_.fill(0.0); + for (size_t i = 0; i < M; ++i) { + covariance_matrix_[i * M + i] = initial_covariance; + } + } + + // `Update` performs one step of the RLS estimation given regressor vector + // `H \in \mathbb{R}^M`, control input measurement `u(t)`, and plant output + // measurement `y(t)`. It optionally outputs prior prediction innovations + // via `err_u` and `err_y`. + void Update(const std::array<double, M>& regressor, double input_u, + double output_y, double* err_u = nullptr, + double* err_y = nullptr) { + // The intermediate vector `P(t-1) * H(t)` + std::array<double, M> p_times_regressor{}; + for (size_t i = 0; i < M; ++i) { + double sum = 0.0; + for (size_t j = 0; j < M; ++j) { + sum += covariance_matrix_[i * M + j] * regressor[j]; + } + p_times_regressor[i] = sum; + } + + // Scalar normalization: `\alpha = \lambda + H(t)^T * P(t-1) * H(t)` + double h_dot_p_h = 0.0; + for (size_t j = 0; j < M; ++j) { + h_dot_p_h += regressor[j] * p_times_regressor[j]; + } + double alpha = forgetting_factor_ + h_dot_p_h; + alpha = std::fmax(alpha, 1e-12); + + // The gain vector `v / \alpha` + std::array<double, M> kalman_gain{}; + double inv_alpha = 1.0 / alpha; + for (size_t i = 0; i < M; ++i) { + kalman_gain[i] = p_times_regressor[i] * inv_alpha; + } + + // Compute prior prediction innovations: + // `e_u(t) = u(t) - H(t)^T * \theta_u(t-1)` + // `e_y(t) = y(t) - H(t)^T * \theta_y(t-1)` + double predicted_u = 0.0; + double predicted_y = 0.0; + for (size_t j = 0; j < M; ++j) { + predicted_u += regressor[j] * theta_u_[j]; + predicted_y += regressor[j] * theta_y_[j]; + } + double innovation_u = input_u - predicted_u; + double innovation_y = output_y - predicted_y; + if (err_u != nullptr) *err_u = innovation_u; + if (err_y != nullptr) *err_y = innovation_y; + + for (size_t i = 0; i < M; ++i) { + theta_u_[i] += kalman_gain[i] * innovation_u; + theta_y_[i] += kalman_gain[i] * innovation_y; + } + + // Update `P(t) = \frac{1}{\lambda} [ P(t-1) - K(t) * v(t)^T ]` + double inv_lambda = 1.0 / forgetting_factor_; + for (size_t i = 0; i < M; ++i) { + for (size_t j = 0; j < M; ++j) { + covariance_matrix_[i * M + j] = + (covariance_matrix_[i * M + j] - + kalman_gain[i] * p_times_regressor[j]) * + inv_lambda; + } + } + + // Enforce symmetry `P = 0.5 * (P + P^T)` and non-negative covariance for + // tability over long identification runs. + for (size_t i = 0; i < M; ++i) { + for (size_t j = i + 1; j < M; ++j) { + double avg = 0.5 * (covariance_matrix_[i * M + j] + + covariance_matrix_[j * M + i]); + covariance_matrix_[i * M + j] = avg; + covariance_matrix_[j * M + i] = avg; + } + if (covariance_matrix_[i * M + i] < 0.0) { + covariance_matrix_[i * M + i] = 0.0; + } + } + } + + const std::array<double, M>& GetThetaU() const { return theta_u_; } + const std::array<double, M>& GetThetaY() const { return theta_y_; } + void SetThetaU(const std::array<double, M>& theta_u) { theta_u_ = theta_u; } + void SetThetaY(const std::array<double, M>& theta_y) { theta_y_ = theta_y; } + + const std::array<double, M * M>& GetCovarianceMatrix() const { + return covariance_matrix_; + } + + double GetForgettingFactor() const { return forgetting_factor_; } + void SetForgettingFactor(double lambda) { forgetting_factor_ = lambda; } + + private: + // Exponential forgetting factor `\lambda \in (0, 1]` + double forgetting_factor_; + // Estimated Fourier parameter vector for input `u \in \mathbb{R}^M` + std::array<double, M> theta_u_{}; + // Estimated Fourier parameter vector for output `y \in \mathbb{R}^M` + std::array<double, M> theta_y_{}; + // Normalized estimation err covariance matrix `P \in \mathbb{R}^{M \times M}` + std::array<double, M * M> covariance_matrix_{}; +}; + +} // namespace optimizer +} // namespace thermal +} // namespace milotic_tlbmc + +#endif // THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_OPTIMIZER_PID_FREQUENCY_RESPONSE_ESTIMATION_UTILS_H_