PhaseLineFitter.h

Go to the documentation of this file.

// Copyright (C) 2025 ASTRON (Netherlands Institute for Radio Astronomy)
// SPDX-License-Identifier: GPL-3.0-or-later
 
#ifndef DP3_COMMON_PHASE_LINE_FITTER_H_
#define DP3_COMMON_PHASE_LINE_FITTER_H_
 
#include <cmath>
#include <limits>
#include <vector>
 
#include <aocommon/optionalnumber.h>
 
namespace dp3::common::phase_fitting {
 
struct FitSample {
  double x;
  double y;
  double weight;
};
 
struct SlopeFitRange {
  int max_wraps;
  double wrap_step;
};
 
inline double SlopeModelCost(const std::vector<FitSample>& data, double slope) {
  double cost = 0.0;
  double weight_sum = 0.0;
  for (const FitSample& sample : data) {
    const double y_model = slope * sample.x;
    double sample_cost = std::fmod(std::abs(y_model - sample.y), 2.0 * M_PI);
    if (sample_cost > M_PI) sample_cost = 2.0 * M_PI - sample_cost;
    sample_cost *= sample.weight;
    cost += sample_cost;
    weight_sum += sample.weight;
  }
  if (weight_sum == 0.0)
    return 0.0;
  else
    return cost / weight_sum;
}
 
inline double UnwrapAndFit(const std::vector<FitSample>& data,
                           double unwrapping_slope) {
  // Minimize: sum w_i (slope * x_i - y_i)^2
  // Therefore:
  // sum w_i 2 x_i (slope * x_i - y_i) = 0
  // slope = (sum w_i x_i y_i) / (sum w_i x_i^2)
  double numerator = 0.0;
  double denominator = 0.0;
  for (const FitSample& sample : data) {
    const double y_model = unwrapping_slope * sample.x;
    double difference = std::fmod(sample.y - y_model, 2.0 * M_PI);
    if (difference >= M_PI)
      difference -= 2.0 * M_PI;
    else if (difference < -M_PI)
      difference += 2.0 * M_PI;
    const double unwrapped_y = y_model + difference;
    const double w_times_x = sample.x * sample.weight;
    numerator += w_times_x * unwrapped_y;
    denominator += w_times_x * sample.x;
  }
  if (denominator != 0.0)
    return numerator / denominator;
  else
    return 0.0;
}
 
inline SlopeFitRange GetRange(
    const std::vector<FitSample>& data,
    aocommon::OptionalNumber<int> wrap_count_limit = {}) {
  if (data.empty()) return SlopeFitRange{0, 0.0};
 
  double min_delta = std::numeric_limits<double>::max();
  for (size_t i = 0; i != data.size() - 1; ++i) {
    const double delta = data[i + 1].x - data[i].x;
    assert(delta >= 0.0);  // 'data' should be ordered on 'x'.
    if (delta > 0.0 && delta < min_delta) min_delta = delta;
  }
  const double min_x = data.front().x;
  const double max_x = data.back().x;
  const double max_abs_x = max_x < 0.0 ? -min_x : max_x;
 
  // The search must go from 0 wraps to the maximum number of wraps that can
  // still lead to finding the right wrapping.
  // In theory, it is still possible to find the right slope value when the
  // slope is so steep that there is a wrap in the small grid distance. This can
  // lead to a large nr of wraps; e.g. in an extreme case of fitting a slope
  // through two values around 150 MHz separated by 1 Hz it would cause 150e6
  // values to search through.
  int max_wraps = std::ceil(max_abs_x / min_delta);
 
  // There is another upper bound to the max nr of wraps: with a low nr of
  // values, there is a fixed, low nr of ways that the values can have been
  // wrapped. This leads to an exponential bound (~ 2^n_samples). However, in
  // this case, the search space is not uniform, so we add a bit more space. It
  // is only relevant for a low nr of samples.
  if (data.size() < 20) {
    constexpr int extra_space = 10;
    max_wraps = std::min(max_wraps, extra_space * (2 << data.size()));
  }
 
  if (wrap_count_limit) {
    max_wraps = std::min(max_wraps, *wrap_count_limit);
  }
 
  // To not miss any possible correct unwrapping, we need to step through every
  // slope for which the largest x-value increases by 2pi. Given y = slope * x,
  // we can dy = dslope * x. Therefore, dslope = dy / max_abs_x = 2 pi /
  // max_abs_x. However, with 2pi steps it seems some slopes are missed (unit
  // tests fail), but with 'pi' steps it works.
  const double wrap_step = M_PI / max_abs_x;
 
  return SlopeFitRange{max_wraps, wrap_step};
}
 
inline double FitSlope(const std::vector<FitSample>& data,
                       const SlopeFitRange& range) {
  double best_cost = std::numeric_limits<double>::max();
  double best_estimate = 0.0;
  for (int i = -range.max_wraps; i <= range.max_wraps; ++i) {
    double slope = range.wrap_step * i;
    const double estimate = UnwrapAndFit(data, slope);
    const double cost = SlopeModelCost(data, estimate);
    if (cost < best_cost) {
      best_cost = cost;
      best_estimate = estimate;
    }
  }
  // Perform one more fit iteration on the best estimate. Because a better
  // estimate is known, the wrapping of noisy values might slightly change, so
  // refit.
  return UnwrapAndFit(data, best_estimate);
}
 
inline double FitSlope(const std::vector<FitSample>& data) {
  return FitSlope(data, GetRange(data));
}
 
}  // namespace dp3::common::phase_fitting
 
#endif