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

#include "tlbmc/thermal/controller/algorithm/utils/nonlinear_tracking_differentiator.h"

#include <cmath>
#include <cstdlib>

namespace milotic_tlbmc {
namespace thermal {

double NonlinearTrackingDifferentiator::Fhan(double v1, double v2, double r,
                                             double h0) {
  // Calculate the linear boundary zone threshold.
  double d = r * h0 * h0;

  // Predict the distance the system will coast over one time step.
  double a0 = h0 * v2;

  // Predict the position error one time-step into the future.
  double y = v1 + a0;

  // Compute an intermediate geometric curve variable.
  // Standard formula: `a_{0std} = sqrt((r * h0)^{2} + 8 * r * |y|)`.
  // By keeping the terms scaled by `h_{0}^{2}` inside the square root, this
  // efficiently computes the equivalent of `(h_{0} * a_{0std})` without needing
  // any division.
  double a1 = std::sqrt(d * (d + 8.0 * std::abs(y)));

  // Evaluate which side of the linear boundary the predicted error is in.
  double a;
  if (std::abs(y) > d) {
    a = a0 + 0.5 * (a1 - d) * (y > 0 ? 1.0 : -1.0);
  } else {
    a = a0 + y;
  }

  // Output the final optimal tracking control action.
  // Apply maximum acceleration if `a` is outside the boundary, otherwise simply
  // apply a linearly scaled fraction of `r`.
  return (std::abs(a) > d ? r * (a > 0 ? 1.0 : -1.0) : r * a / d);
}

NtdOutput NonlinearTrackingDifferentiator::FilterInput(double input) {
  double error = ntd_output_.filtered_output - input;
  double f = Fhan(error, ntd_output_.filtered_derivative, r_, h0_);

  // Discrete integration via Euler-forward to update tracking states.
  ntd_output_.filtered_output +=
      ntd_sample_time_sec_ * ntd_output_.filtered_derivative;
  ntd_output_.filtered_derivative -= ntd_sample_time_sec_ * f;

  return ntd_output_;
}

}  // namespace thermal
}  // namespace milotic_tlbmc