SecularAndHarmonic.java

  1. /* Copyright 2002-2025 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.utils;

  19. import java.util.ArrayList;
  20. import java.util.Collection;
  21. import java.util.List;

  23. import org.hipparchus.analysis.ParametricUnivariateFunction;
  24. import org.hipparchus.fitting.AbstractCurveFitter;
  25. import org.hipparchus.fitting.PolynomialCurveFitter;
  26. import org.hipparchus.fitting.WeightedObservedPoint;
  27. import org.hipparchus.linear.DiagonalMatrix;
  28. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
  29. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
  30. import org.hipparchus.util.FastMath;
  31. import org.hipparchus.util.SinCos;
  32. import org.orekit.time.AbsoluteDate;

  34. /** Class for fitting evolution of osculating orbital parameters.
  35.  * <p>
  36.  * This class allows conversion from osculating parameters to mean parameters.
  37.  * </p>
  38.  *
  39.  * @author Luc Maisonobe
  40.  */
  41. public class SecularAndHarmonic {

  43.     /** Degree of polynomial secular part. */
  44.     private final int secularDegree;

  46.     /** Pulsations of harmonic part. */
  47.     private final double[] pulsations;

  49.     /** Reference date for the model. */
  50.     private AbsoluteDate reference;

  52.     /** Fitted parameters. */
  53.     private double[] fitted;

  55.     /** Observed points. */
  56.     private List<WeightedObservedPoint> observedPoints;

  58.     /** RMS for convergence.
  59.      * @since 10.3
  60.      */
  61.     private double convergenceRMS;

  63.     /** Maximum number of iterations.
  64.      * @since 10.3
  65.      */
  66.     private int maxIter;

  68.     /** Simple constructor.
  69.      * @param secularDegree degree of polynomial secular part
  70.      * @param pulsations pulsations of harmonic part
  71.      */
  72.     public SecularAndHarmonic(final int secularDegree, final double... pulsations) {
  73.         this.secularDegree  = secularDegree;
  74.         this.pulsations     = pulsations.clone();
  75.         this.observedPoints = new ArrayList<>();
  76.         this.convergenceRMS = 0.0;
  77.         this.maxIter        = Integer.MAX_VALUE;
  78.     }

  80.     /** Reset fitting.
  81.      * @param date reference date
  82.      * @param initialGuess initial guess for the parameters
  83.      * @see #getReferenceDate()
  84.      */
  85.     public void resetFitting(final AbsoluteDate date, final double... initialGuess) {
  86.         reference = date;
  87.         fitted    = initialGuess.clone();
  88.         observedPoints.clear();
  89.     }

  91.     /** Set RMS for convergence.
  92.      * <p>
  93.      * The RMS is the square-root of the sum of squared of
  94.      * the residuals, divided by the number of measurements.
  95.      * </p>
  96.      * @param convergenceRMS RMS below which convergence is considered to have been reached
  97.      * @since 10.3
  98.      */
  99.     public void setConvergenceRMS(final double convergenceRMS) {
  100.         this.convergenceRMS = convergenceRMS;
  101.     }

  103.     /** Set maximum number of iterations.
  104.      * @param maxIter maximum number of iterations
  105.      * @since 10.3
  106.      */
  107.     public void setMaxIter(final int maxIter) {
  108.         this.maxIter = maxIter;
  109.     }

  111.     /** Add a fitting point.
  112.      * <p>
  113.      * The point weight is set to 1.0
  114.      * </p>
  115.      * @param date date of the point
  116.      * @param osculatingValue osculating value
  117.      * @see #addWeightedPoint(AbsoluteDate, double, double)
  118.      */
  119.     public void addPoint(final AbsoluteDate date, final double osculatingValue) {
  120.         addWeightedPoint(date, osculatingValue, 1.0);
  121.     }

  123.     /** Add a weighted fitting point.
  124.      * @param date date of the point
  125.      * @param osculatingValue osculating value
  126.      * @param weight weight of the points
  127.      * @since 12.0
  128.      */
  129.     public void addWeightedPoint(final AbsoluteDate date, final double osculatingValue, final double weight) {
  130.         observedPoints.add(new WeightedObservedPoint(weight, date.durationFrom(reference), osculatingValue));
  131.     }

  133.     /** Get the reference date.
  134.      * @return reference date
  135.      * @see #resetFitting(AbsoluteDate, double...)
  136.      */
  137.     public AbsoluteDate getReferenceDate() {
  138.         return reference;
  139.     }

  141.     /** Get degree of polynomial secular part.
  142.      * @return degree of polynomial secular part
  143.      * @since 12.0
  144.      */
  145.     public int getSecularDegree() {
  146.         return secularDegree;
  147.     }

  149.     /** Get the pulsations of harmonic part.
  150.      * @return pulsations of harmonic part
  151.      * @since 12.0
  152.      */
  153.     public double[] getPulsations() {
  154.         return pulsations.clone();
  155.     }

  157.     /** Get an upper bound of the fitted harmonic amplitude.
  158.      * @return upper bound of the fitted harmonic amplitude
  159.      */
  160.     public double getHarmonicAmplitude() {
  161.         double amplitude = 0;
  162.         for (int i = 0; i < pulsations.length; ++i) {
  163.             amplitude += FastMath.hypot(fitted[secularDegree + 2 * i + 1],
  164.                                         fitted[secularDegree + 2 * i + 2]);
  165.         }
  166.         return amplitude;
  167.     }

  169.     /** Fit parameters.
  170.      * @see #getFittedParameters()
  171.      */
  172.     public void fit() {

  174.         final AbstractCurveFitter fitter = new AbstractCurveFitter() {
  175.             /** {@inheritDoc} */
  176.             @Override
  177.             protected LeastSquaresProblem getProblem(final Collection<WeightedObservedPoint> observations) {
  178.                 // Prepare least-squares problem.
  179.                 final int len = observations.size();
  180.                 final double[] target  = new double[len];
  181.                 final double[] weights = new double[len];

  183.                 int i = 0;
  184.                 for (final WeightedObservedPoint obs : observations) {
  185.                     target[i]  = obs.getY();
  186.                     weights[i] = obs.getWeight();
  187.                     ++i;
  188.                 }

  190.                 final AbstractCurveFitter.TheoreticalValuesFunction model =
  191.                         new AbstractCurveFitter.TheoreticalValuesFunction(new LocalParametricFunction(), observations);

  193.                 // build a new least squares problem set up to fit a secular and harmonic curve to the observed points
  194.                 return new LeastSquaresBuilder().
  195.                         maxEvaluations(Integer.MAX_VALUE).
  196.                         maxIterations(maxIter).
  197.                         checker((iteration, previous, current) -> current.getRMS() <= convergenceRMS).
  198.                         start(fitted).
  199.                         target(target).
  200.                         weight(new DiagonalMatrix(weights)).
  201.                         model(model.getModelFunction(), model.getModelFunctionJacobian()).
  202.                         build();

  204.             }
  205.         };

  207.         fitted = fitter.fit(observedPoints);

  209.     }

  211.     /** Local parametric function used for fitting. */
  212.     private class LocalParametricFunction implements ParametricUnivariateFunction {

  214.         /** {@inheritDoc} */
  215.         public double value(final double x, final double... parameters) {
  216.             return truncatedValue(secularDegree, pulsations.length, x, parameters);
  217.         }

  219.         /** {@inheritDoc} */
  220.         public double[] gradient(final double x, final double... parameters) {
  221.             final double[] gradient = new double[secularDegree + 1 + 2 * pulsations.length];

  223.             // secular part
  224.             double xN = 1.0;
  225.             for (int i = 0; i <= secularDegree; ++i) {
  226.                 gradient[i] = xN;
  227.                 xN *= x;
  228.             }

  230.             // harmonic part
  231.             for (int i = 0; i < pulsations.length; ++i) {
  232.                 final SinCos sc = FastMath.sinCos(pulsations[i] * x);
  233.                 gradient[secularDegree + 2 * i + 1] = sc.cos();
  234.                 gradient[secularDegree + 2 * i + 2] = sc.sin();
  235.             }

  237.             return gradient;
  238.         }

  240.     }

  242.     /** Get a copy of the last fitted parameters.
  243.      * @return copy of the last fitted parameters.
  244.      * @see #fit()
  245.      */
  246.     public double[] getFittedParameters() {
  247.         return fitted.clone();
  248.     }

  250.     /** Get fitted osculating value.
  251.      * @param date current date
  252.      * @return osculating value at current date
  253.      */
  254.     public double osculatingValue(final AbsoluteDate date) {
  255.         return truncatedValue(secularDegree, pulsations.length,
  256.                               date.durationFrom(reference), fitted);
  257.     }

  259.     /** Get fitted osculating derivative.
  260.      * @param date current date
  261.      * @return osculating derivative at current date
  262.      */
  263.     public double osculatingDerivative(final AbsoluteDate date) {
  264.         return truncatedDerivative(secularDegree, pulsations.length,
  265.                                    date.durationFrom(reference), fitted);
  266.     }

  268.     /** Get fitted osculating second derivative.
  269.      * @param date current date
  270.      * @return osculating second derivative at current date
  271.      */
  272.     public double osculatingSecondDerivative(final AbsoluteDate date) {
  273.         return truncatedSecondDerivative(secularDegree, pulsations.length,
  274.                                          date.durationFrom(reference), fitted);
  275.     }

  277.     /** Get mean value, truncated to first components.
  278.      * @param date current date
  279.      * @param degree degree of polynomial secular part to consider
  280.      * @param harmonics number of harmonics terms to consider
  281.      * @return mean value at current date
  282.      */
  283.     public double meanValue(final AbsoluteDate date, final int degree, final int harmonics) {
  284.         return truncatedValue(degree, harmonics, date.durationFrom(reference), fitted);
  285.     }

  287.     /** Get mean derivative, truncated to first components.
  288.      * @param date current date
  289.      * @param degree degree of polynomial secular part to consider
  290.      * @param harmonics number of harmonics terms to consider
  291.      * @return mean derivative at current date
  292.      */
  293.     public double meanDerivative(final AbsoluteDate date, final int degree, final int harmonics) {
  294.         return truncatedDerivative(degree, harmonics, date.durationFrom(reference), fitted);
  295.     }

  297.     /** Approximate an already fitted model to polynomial only terms.
  298.      * <p>
  299.      * This method is mainly used in order to combine the large amplitude long
  300.      * periods with the secular part as a new approximate polynomial model over
  301.      * some time range. This should be used rather than simply extracting the
  302.      * polynomial coefficients from {@link #getFittedParameters()} when some
  303.      * periodic terms amplitudes are large (for example Sun resonance effects
  304.      * on local solar time in sun synchronous orbits). In theses cases, the pure
  305.      * polynomial secular part in the coefficients may be far from the mean model.
  306.      * </p>
  307.      * @param combinedDegree desired degree for the combined polynomial
  308.      * @param combinedReference desired reference date for the combined polynomial
  309.      * @param meanDegree degree of polynomial secular part to consider
  310.      * @param meanHarmonics number of harmonics terms to consider
  311.      * @param start start date of the approximation time range
  312.      * @param end end date of the approximation time range
  313.      * @param step sampling step
  314.      * @return coefficients of the approximate polynomial (in increasing degree order),
  315.      * using the user provided reference date
  316.      */
  317.     public double[] approximateAsPolynomialOnly(final int combinedDegree, final AbsoluteDate combinedReference,
  318.                                                 final int meanDegree, final int meanHarmonics,
  319.                                                 final AbsoluteDate start, final AbsoluteDate end,
  320.                                                 final double step) {
  321.         final List<WeightedObservedPoint> points = new ArrayList<>();
  322.         for (AbsoluteDate date = start; date.compareTo(end) < 0; date = date.shiftedBy(step)) {
  323.             points.add(new WeightedObservedPoint(1.0,
  324.                                                  date.durationFrom(combinedReference),
  325.                                                  meanValue(date, meanDegree, meanHarmonics)));
  326.         }
  327.         return PolynomialCurveFitter.create(combinedDegree).fit(points);
  328.     }

  330.     /** Get mean second derivative, truncated to first components.
  331.      * @param date current date
  332.      * @param degree degree of polynomial secular part
  333.      * @param harmonics number of harmonics terms to consider
  334.      * @return mean second derivative at current date
  335.      */
  336.     public double meanSecondDerivative(final AbsoluteDate date, final int degree, final int harmonics) {
  337.         return truncatedSecondDerivative(degree, harmonics, date.durationFrom(reference), fitted);
  338.     }

  340.     /** Get value truncated to first components.
  341.      * @param degree degree of polynomial secular part
  342.      * @param harmonics number of harmonics terms to consider
  343.      * @param time time parameter
  344.      * @param parameters models parameters (must include all parameters,
  345.      * including the ones ignored due to model truncation)
  346.      * @return truncated value
  347.      */
  348.     private double truncatedValue(final int degree, final int harmonics,
  349.                                   final double time, final double... parameters) {

  351.         double value = 0;

  353.         // secular part
  354.         double tN = 1.0;
  355.         for (int i = 0; i <= degree; ++i) {
  356.             value += parameters[i] * tN;
  357.             tN    *= time;
  358.         }

  360.         // harmonic part
  361.         for (int i = 0; i < harmonics; ++i) {
  362.             final SinCos sc = FastMath.sinCos(pulsations[i] * time);
  363.             value += parameters[secularDegree + 2 * i + 1] * sc.cos() +
  364.                      parameters[secularDegree + 2 * i + 2] * sc.sin();
  365.         }

  367.         return value;

  369.     }

  371.     /** Get derivative truncated to first components.
  372.      * @param degree degree of polynomial secular part
  373.      * @param harmonics number of harmonics terms to consider
  374.      * @param time time parameter
  375.      * @param parameters models parameters (must include all parameters,
  376.      * including the ones ignored due to model truncation)
  377.      * @return truncated derivative
  378.      */
  379.     private double truncatedDerivative(final int degree, final int harmonics,
  380.                                        final double time, final double... parameters) {

  382.         double derivative = 0;

  384.         // secular part
  385.         double tN = 1.0;
  386.         for (int i = 1; i <= degree; ++i) {
  387.             derivative += i * parameters[i] * tN;
  388.             tN         *= time;
  389.         }

  391.         // harmonic part
  392.         for (int i = 0; i < harmonics; ++i) {
  393.             final SinCos sc = FastMath.sinCos(pulsations[i] * time);
  394.             derivative += pulsations[i] * (-parameters[secularDegree + 2 * i + 1] * sc.sin() +
  395.                                             parameters[secularDegree + 2 * i + 2] * sc.cos());
  396.         }

  398.         return derivative;

  400.     }

  402.     /** Get second derivative truncated to first components.
  403.      * @param degree degree of polynomial secular part
  404.      * @param harmonics number of harmonics terms to consider
  405.      * @param time time parameter
  406.      * @param parameters models parameters (must include all parameters,
  407.      * including the ones ignored due to model truncation)
  408.      * @return truncated second derivative
  409.      */
  410.     private double truncatedSecondDerivative(final int degree, final int harmonics,
  411.                                              final double time, final double... parameters) {

  413.         double d2 = 0;

  415.         // secular part
  416.         double tN = 1.0;
  417.         for (int i = 2; i <= degree; ++i) {
  418.             d2 += (i - 1) * i * parameters[i] * tN;
  419.             tN *= time;
  420.         }

  422.         // harmonic part
  423.         for (int i = 0; i < harmonics; ++i) {
  424.             final SinCos sc = FastMath.sinCos(pulsations[i] * time);
  425.             d2 += -pulsations[i] * pulsations[i] *
  426.                   (parameters[secularDegree + 2 * i + 1] * sc.cos() +
  427.                    parameters[secularDegree + 2 * i + 2] * sc.sin());
  428.         }

  430.         return d2;

  432.     }

  434. }
Created with JaCoCo 0.8.12.202403310830