tlbmc/thermal/controller/algorithm/utils/nonlinear_tracking_differentiator.h

#ifndef THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_UTILS_NONLINEAR_TRACKING_DIFFERENTIATOR_H_
#define THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_UTILS_NONLINEAR_TRACKING_DIFFERENTIATOR_H_

namespace milotic_tlbmc {
namespace thermal {

struct NtdOutput {
  double filtered_output;      // Filtered reading
  double filtered_derivative;  // Filtered reading derivative (e.g., `dT/dt`)
};

/*
 * `NonlinearTrackingDifferentiator` is used to timely process noisy sensor
 * data to derive a smoothed signal with its derivative.
 *
 * Traditional filters face a dilemma, where the heavier a signal is filtered to
 * remove noise, the more phase delay is introduced. Additionally, traditional
 * filters may suffer from "chattering" (i.e., rapid, high-frequency vibrations)
 * that occurs when a digital system constantly overshoots its target during the
 * discrete time steps. NTD addresses these issues by eliminating "chattering"
 * via a Time Criterion (DTOC law) and canceling phase delay via Phase-Advancing
 * (PA).
 *
 * The numeric expressions in the comments of nonlinear_tracking_differentiator
 * follows the LaTeX format.
 *
 * Reference:
 *   https://doi.org/10.1016/j.ast.2024.109578
 */
class NonlinearTrackingDifferentiator {
 public:
  explicit NonlinearTrackingDifferentiator(
      double ntd_sample_time_sec, double r, double h0,
      double initial_filtered_output = 0,
      double initial_filtered_derivative = 0)
      : ntd_sample_time_sec_(ntd_sample_time_sec),
        r_(r),
        h0_(h0),
        ntd_output_{initial_filtered_output, initial_filtered_derivative} {}

  NtdOutput FilterInput(double input);

 protected:
  // `Fhan` is Han's Time-Optimal Control Function (i.e., fhan). It solves the
  // discrete-time time-optimal control law, and determines the correct "force"
  // to apply to smooth the signal based on a speed factor (`r`) and a
  // filtering step size (`h_{0}`).
  static double Fhan(double v1, double v2, double r, double h0);

 private:
  double
      ntd_sample_time_sec_;  // NTD sampling time: Usually same as control loop.
  double r_;   // Tracking speed: Larger `r` leads to faster tracking but less
               // filtering.
  double h0_;  // Filtering chronometer: Usually set equal to
               // `ntd_sample_time_sec_`, or slightly larger.

  NtdOutput ntd_output_;
};

}  // namespace thermal
}  // namespace milotic_tlbmc

#endif  // THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_UTILS_NONLINEAR_TRACKING_DIFFERENTIATOR_H_