AbstractPropagatorConverter.java

  1. /* Copyright 2002-2016 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.propagation.conversion;

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

  23. import org.apache.commons.math3.analysis.MultivariateMatrixFunction;
  24. import org.apache.commons.math3.analysis.MultivariateVectorFunction;
  25. import org.apache.commons.math3.exception.MaxCountExceededException;
  26. import org.apache.commons.math3.fitting.leastsquares.LeastSquaresBuilder;
  27. import org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory;
  28. import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer;
  29. import org.apache.commons.math3.fitting.leastsquares.LeastSquaresProblem;
  30. import org.apache.commons.math3.fitting.leastsquares.LeastSquaresProblem.Evaluation;
  31. import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer;
  32. import org.apache.commons.math3.fitting.leastsquares.MultivariateJacobianFunction;
  33. import org.apache.commons.math3.linear.DiagonalMatrix;
  34. import org.apache.commons.math3.linear.MatrixUtils;
  35. import org.apache.commons.math3.linear.RealMatrix;
  36. import org.apache.commons.math3.linear.RealVector;
  37. import org.apache.commons.math3.optim.ConvergenceChecker;
  38. import org.apache.commons.math3.optim.SimpleVectorValueChecker;
  39. import org.apache.commons.math3.util.FastMath;
  40. import org.apache.commons.math3.util.Pair;
  41. import org.orekit.errors.OrekitException;
  42. import org.orekit.errors.OrekitExceptionWrapper;
  43. import org.orekit.errors.OrekitMessages;
  44. import org.orekit.frames.Frame;
  45. import org.orekit.propagation.Propagator;
  46. import org.orekit.propagation.SpacecraftState;
  47. import org.orekit.time.AbsoluteDate;
  48. import org.orekit.utils.PVCoordinates;

  50. /** Common handling of {@link PropagatorConverter} methods for propagators conversions.
  51.  * <p>
  52.  * This abstract class factorizes the common code for propagators conversion.
  53.  * Only one method must be implemented by derived classes: {@link #getObjectiveFunction()}.
  54.  * </p>
  55.  * <p>
  56.  * The converter uses the LevenbergMarquardtOptimizer from the <a
  57.  * href="http://commons.apache.org/math/">commons math</a> library.
  58.  * Different implementations correspond to different methods for computing the jacobian.
  59.  * </p>
  60.  * @author Pascal Parraud
  61.  * @since 6.0
  62.  */
  63. public abstract class AbstractPropagatorConverter implements PropagatorConverter {

  65.     /** Spacecraft states sample. */
  66.     private List<SpacecraftState> sample;

  68.     /** Reference date for conversion (1st date of the states sample). */
  69.     private AbsoluteDate date;

  71.     /** Target position and velocities at sample points. */
  72.     private double[] target;

  74.     /** Weight for residuals. */
  75.     private double[] weight;

  77.     /** Auxiliary outputData: RMS of solution. */
  78.     private double rms;

  80.     /** Position use indicator. */
  81.     private boolean onlyPosition;

  83.     /** Adapted propagator. */
  84.     private Propagator adapted;

  86.     /** List of the desired free parameters names. */
  87.     private List<String> parameters;

  89.     /** List of the available free parameters names. */
  90.     private final Collection<String> availableParameters;

  92.     /** Propagator builder. */
  93.     private final PropagatorBuilder builder;

  95.     /** Frame. */
  96.     private final Frame frame;

  98.     /** Optimizer for fitting. */
  99.     private final LevenbergMarquardtOptimizer optimizer;

  101.     /** Optimum found. */
  102.     private LeastSquaresOptimizer.Optimum optimum;

  104.     /** Convergence checker for optimization algorithm. */
  105.     private final ConvergenceChecker<Evaluation> checker;

  107.     /** Maximum number of iterations for optimization. */
  108.     private final int maxIterations;

  110.     /** Build a new instance.
  111.      * @param builder propagator builder
  112.      * @param threshold absolute convergence threshold for optimization algorithm
  113.      * @param maxIterations maximum number of iterations for fitting
  114.      */
  115.     protected AbstractPropagatorConverter(final PropagatorBuilder builder,
  116.                                           final double threshold,
  117.                                           final int maxIterations) {
  118.         this.builder             = builder;
  119.         this.frame               = builder.getFrame();
  120.         this.availableParameters = builder.getSupportedParameters();
  121.         this.optimizer           = new LevenbergMarquardtOptimizer();
  122.         this.maxIterations       = maxIterations;
  123.         this.sample              = new ArrayList<SpacecraftState>();

  125.         final SimpleVectorValueChecker svvc = new SimpleVectorValueChecker(-1.0, threshold);
  126.         this.checker = LeastSquaresFactory.evaluationChecker(svvc);

  128.     }

  130.     /** Convert a propagator to another.
  131.      * @param source initial propagator
  132.      * @param timeSpan time span for fitting
  133.      * @param nbPoints number of fitting points over time span
  134.      * @param freeParameters names of the free parameters
  135.      * @return adapted propagator
  136.      * @exception OrekitException if propagator cannot be adapted
  137.      * @exception IllegalArgumentException if one of the parameters cannot be free
  138.      */
  139.     public Propagator convert(final Propagator source,
  140.                               final double timeSpan,
  141.                               final int nbPoints,
  142.                               final List<String> freeParameters)
  143.         throws OrekitException, IllegalArgumentException {

  145.         checkParameters(freeParameters);
  146.         final List<SpacecraftState> states = createSample(source, timeSpan, nbPoints);
  147.         return convert(states, false, freeParameters);
  148.     }

  150.     /** Convert a propagator to another.
  151.      * @param source initial propagator
  152.      * @param timeSpan time span for fitting
  153.      * @param nbPoints number of fitting points over time span
  154.      * @param freeParameters names of the free parameters
  155.      * @return adapted propagator
  156.      * @exception OrekitException if propagator cannot be adapted
  157.      * @exception IllegalArgumentException if one of the parameters cannot be free
  158.      */
  159.     public Propagator convert(final Propagator source,
  160.                               final double timeSpan,
  161.                               final int nbPoints,
  162.                               final String ... freeParameters)
  163.         throws OrekitException, IllegalArgumentException {

  165.         checkParameters(freeParameters);
  166.         final List<SpacecraftState> states = createSample(source, timeSpan, nbPoints);
  167.         return convert(states, false, freeParameters);
  168.     }

  170.     /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
  171.      * @param states spacecraft states sample to fit
  172.      * @param positionOnly if true, consider only position data otherwise both position and velocity are used
  173.      * @param freeParameters names of the free parameters
  174.      * @return adapted propagator
  175.      * @exception OrekitException if propagator cannot be adapted
  176.      * @exception IllegalArgumentException if one of the parameters cannot be free
  177.      */
  178.     public Propagator convert(final List<SpacecraftState> states,
  179.                               final boolean positionOnly,
  180.                               final List<String> freeParameters)
  181.         throws OrekitException, IllegalArgumentException {

  183.         checkParameters(freeParameters);

  185.         parameters = new ArrayList<String>();
  186.         parameters.addAll(freeParameters);

  188.         builder.setFreeParameters(parameters);

  190.         return adapt(states, positionOnly);
  191.     }

  193.     /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
  194.      * @param states spacecraft states sample to fit
  195.      * @param positionOnly if true, consider only position data otherwise both position and velocity are used
  196.      * @param freeParameters names of the free parameters
  197.      * @return adapted propagator
  198.      * @exception OrekitException if propagator cannot be adapted
  199.      * @exception IllegalArgumentException if one of the parameters cannot be free
  200.      */
  201.     public Propagator convert(final List<SpacecraftState> states,
  202.                               final boolean positionOnly,
  203.                               final String ... freeParameters)
  204.         throws OrekitException, IllegalArgumentException {

  206.         checkParameters(freeParameters);

  208.         parameters = new ArrayList<String>();
  209.         for (final String name : freeParameters) {
  210.             parameters.add(name);
  211.         }

  213.         builder.setFreeParameters(parameters);

  215.         return adapt(states, positionOnly);
  216.     }

  218.     /** Get the available free parameters.
  219.      * @return available free parameters
  220.      */
  221.     public Collection<String> getAvailableParameters() {
  222.         return availableParameters;
  223.     }

  225.     /** Check if a parameter can be free.
  226.      * @param name parameter name to check
  227.      * @return true if the parameter can be free
  228.      */
  229.     public boolean isAvailable(final String name) {
  230.         for (final String supportedName : availableParameters) {
  231.             if (supportedName.equals(name)) {
  232.                 return true;
  233.             }
  234.         }
  235.         return false;
  236.     }

  238.     /** Get the adapted propagator.
  239.      * @return adapted propagator
  240.      */
  241.     public Propagator getAdaptedPropagator() {
  242.         return adapted;
  243.     }

  245.     /** Get the Root Mean Square Deviation of the fitting.
  246.      * @return RMSD
  247.      */
  248.     public double getRMS() {
  249.         return rms;
  250.     }

  252.     /** Get the number of objective function evaluations.
  253.      *  @return the number of objective function evaluations.
  254.      */
  255.     public int getEvaluations() {
  256.         return optimum.getEvaluations();
  257.     }

  259.     /** Get the function computing position/velocity at sample points.
  260.      * @return function computing position/velocity at sample points
  261.      */
  262.     protected abstract MultivariateVectorFunction getObjectiveFunction();

  264.     /** Get the Jacobian of the function computing position/velocity at sample points.
  265.      * @return Jacobian of the function computing position/velocity at sample points
  266.      */
  267.     protected abstract MultivariateMatrixFunction getObjectiveFunctionJacobian();

  269.     /** Check if fitting uses only sample positions.
  270.      * @return true if fitting uses only sample positions
  271.      */
  272.     protected boolean isOnlyPosition() {
  273.         return onlyPosition;
  274.     }

  276.     /** Get the size of the target.
  277.      * @return target size
  278.      */
  279.     protected int getTargetSize() {
  280.         return target.length;
  281.     }

  283.     /** Get the date of the initial state.
  284.      * @return the date
  285.      */
  286.     protected AbsoluteDate getDate() {
  287.         return date;
  288.     }

  290.     /** Get the frame of the initial state.
  291.      * @return the orbit frame
  292.      */
  293.     protected Frame getFrame() {
  294.         return frame;
  295.     }

  297.     /** Get the states sample.
  298.      * @return the states sample
  299.      */
  300.     protected List<SpacecraftState> getSample() {
  301.         return sample;
  302.     }

  304.     /** Get the free parameters.
  305.      * @return the free parameters
  306.      */
  307.     protected Collection<String>  getFreeParameters() {
  308.         return parameters;
  309.     }

  311.     /** Create a sample of {@link SpacecraftState}.
  312.      * @param source initial propagator
  313.      * @param timeSpan time span for the sample
  314.      * @param nbPoints number of points for the sample over the time span
  315.      * @return a sample of {@link SpacecraftState}
  316.      * @exception OrekitException if one of the sample point cannot be computed
  317.      */
  318.     private List<SpacecraftState> createSample(final Propagator source,
  319.                                                final double timeSpan,
  320.                                                final int nbPoints) throws OrekitException {

  322.         final SpacecraftState initialState = source.getInitialState();

  324.         final List<SpacecraftState> states = new ArrayList<SpacecraftState>();

  326.         final double stepSize = timeSpan / (nbPoints - 1);
  327.         final AbsoluteDate iniDate = source.getInitialState().getDate();
  328.         for (double dt = 0; dt < timeSpan; dt += stepSize) {
  329.             states.add(source.propagate(iniDate.shiftedBy(dt)));
  330.         }

  332.         source.resetInitialState(initialState);

  334.         return states;
  335.     }

  337.     /** Check if parameters can be free.
  338.      * @param freeParameters names of the free parameters
  339.      * @exception OrekitException if one of the parameters cannot be free
  340.      */
  341.     private void checkParameters(final Collection<String> freeParameters) throws OrekitException {
  342.         for (String parameter : freeParameters) {
  343.             checkParameter(parameter);
  344.         }
  345.     }

  347.     /** Check if parameters can be free.
  348.      * @param freeParameters names of the free parameters
  349.      * @exception OrekitException if one of the parameters cannot be free
  350.      */
  351.     private void checkParameters(final String ... freeParameters) throws OrekitException {
  352.         for (String parameter : freeParameters) {
  353.             checkParameter(parameter);
  354.         }
  355.     }

  357.     /** Check if parameter can be free.
  358.      * @param parameter name of the free parameter
  359.      * @exception OrekitException if the parameter cannot be free
  360.      */
  361.     private void checkParameter(final String parameter) throws OrekitException {

  363.         if ( !availableParameters.contains(parameter) ) {

  365.             // build the list of supported parameters
  366.             final StringBuilder sBuilder = new StringBuilder();
  367.             for (final String available : availableParameters) {
  368.                 if (sBuilder.length() > 0) {
  369.                     sBuilder.append(", ");
  370.                 }
  371.                 sBuilder.append(available);
  372.             }

  374.             throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
  375.                                       parameter, sBuilder.toString());

  377.         }

  379.     }

  381.     /** Adapt a propagator to minimize the mean square error for a set of {@link SpacecraftState states}.
  382.      * @param states set of spacecraft states to fit
  383.      * @param positionOnly if true, consider only position data otherwise both position and velocity are used
  384.      * @return adapted propagator
  385.      * @exception OrekitException if propagator cannot be adapted
  386.      */
  387.     private Propagator adapt(final List<SpacecraftState> states,
  388.                              final boolean positionOnly) throws OrekitException {

  390.         this.date = states.get(0).getDate();
  391.         this.onlyPosition = positionOnly;

  393.         // very rough first guess using osculating parameters of first sample point
  394.         final double[] initial = new double[6 + parameters.size()];
  395.         builder.getOrbitType().mapOrbitToArray(states.get(0).getOrbit(),
  396.                                                builder.getPositionAngle(),
  397.                                                initial);
  398.         int i = 6;
  399.         for (String name : parameters) {
  400.             initial[i++] = builder.getParameter(name);
  401.         }

  403.         // warm-up iterations, using only a few points
  404.         setSample(states.subList(0, onlyPosition ? 2 : 1));
  405.         final double[] intermediate = fit(initial);

  407.         // final search using all points
  408.         setSample(states);
  409.         final double[] result = fit(intermediate);

  411.         rms = getRMS(result);
  412.         adapted = buildAdaptedPropagator(result);

  414.         return adapted;
  415.     }

  417.     /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
  418.      * @param initial initial estimation parameters (position, velocity, free parameters)
  419.      * @return fitted parameters
  420.      * @exception OrekitException if propagator cannot be adapted
  421.      * @exception MaxCountExceededException if maximal number of iterations is exceeded
  422.      */
  423.     private double[] fit(final double[] initial)
  424.         throws OrekitException, MaxCountExceededException {

  426.         final MultivariateVectorFunction f = getObjectiveFunction();
  427.         final MultivariateMatrixFunction jac = getObjectiveFunctionJacobian();
  428.         final MultivariateJacobianFunction fJac = new MultivariateJacobianFunction() {
  429.             /** {@inheritDoc} */
  430.             @Override
  431.             public Pair<RealVector, RealMatrix> value(final RealVector point) {
  432.                 final double[] p = point.toArray();
  433.                 return new Pair<RealVector, RealMatrix>(MatrixUtils.createRealVector(f.value(p)),
  434.                         MatrixUtils.createRealMatrix(jac.value(p)));
  435.             }
  436.         };
  437.         final LeastSquaresProblem problem = new LeastSquaresBuilder().
  438.                                             maxIterations(maxIterations).
  439.                                             maxEvaluations(Integer.MAX_VALUE).
  440.                                             model(fJac).
  441.                                             target(target).
  442.                                             weight(new DiagonalMatrix(weight)).
  443.                                             start(initial).
  444.                                             checker(checker).
  445.                                             build();
  446.         optimum = optimizer.optimize(problem);
  447.         return optimum.getPoint().toArray();
  448.     }

  450.     /** Get the Root Mean Square Deviation for a given parameters set.
  451.      * @param parameterSet position/velocity parameters set
  452.      * @return RMSD
  453.      * @exception OrekitException if position/velocity cannot be computed at some date
  454.      */
  455.     private double getRMS(final double[] parameterSet) throws OrekitException {

  457.         final double[] residuals = getResiduals(parameterSet);
  458.         double sum2 = 0;
  459.         for (final double residual : residuals) {
  460.             sum2 += residual * residual;
  461.         }
  462.         return FastMath.sqrt(sum2 / residuals.length);

  464.     }

  466.     /** Get the residuals for a given position/velocity parameters set.
  467.      * @param parameterSet position/velocity parameters set
  468.      * @return residuals
  469.      * @exception OrekitException if position/velocity cannot be computed at some date
  470.      */
  471.     private double[] getResiduals(final double[] parameterSet) throws OrekitException {
  472.         try {
  473.             final double[] residuals = getObjectiveFunction().value(parameterSet);
  474.             for (int i = 0; i < residuals.length; ++i) {
  475.                 residuals[i] = target[i] - residuals[i];
  476.             }
  477.             return residuals;
  478.         } catch (OrekitExceptionWrapper oew) {
  479.             throw oew.getException();
  480.         }
  481.     }

  483.     /** Build the adpated propagator for a given position/velocity(/free) parameters set.
  484.      * @param parameterSet position/velocity(/free) parameters set
  485.      * @return adapted propagator
  486.      * @exception OrekitException if propagator cannot be build
  487.      */
  488.     private Propagator buildAdaptedPropagator(final double[] parameterSet)
  489.         throws OrekitException {
  490.         return builder.buildPropagator(date, parameterSet);
  491.     }

  493.     /** Set the states sample.
  494.      * @param states spacecraft states sample
  495.      * @exception OrekitException if position/velocity cannot be extracted from sample
  496.      */
  497.     private void setSample(final List<SpacecraftState> states) throws OrekitException {

  499.         this.sample = states;

  501.         if (onlyPosition) {
  502.             target = new double[states.size() * 3];
  503.             weight = new double[states.size() * 3];
  504.         } else {
  505.             target = new double[states.size() * 6];
  506.             weight = new double[states.size() * 6];
  507.         }

  509.         int k = 0;
  510.         for (int i = 0; i < states.size(); i++) {

  512.             final PVCoordinates pv = states.get(i).getPVCoordinates(frame);

  514.             // position
  515.             target[k]   = pv.getPosition().getX();
  516.             weight[k++] = 1;
  517.             target[k]   = pv.getPosition().getY();
  518.             weight[k++] = 1;
  519.             target[k]   = pv.getPosition().getZ();
  520.             weight[k++] = 1;

  522.             // velocity
  523.             if (!onlyPosition) {
  524.                 // velocity weight relative to position
  525.                 final double r2 = pv.getPosition().getNormSq();
  526.                 final double v  = pv.getVelocity().getNorm();
  527.                 final double vWeight = v * r2 / states.get(i).getMu();

  529.                 target[k]   = pv.getVelocity().getX();
  530.                 weight[k++] = vWeight;
  531.                 target[k]   = pv.getVelocity().getY();
  532.                 weight[k++] = vWeight;
  533.                 target[k]   = pv.getVelocity().getZ();
  534.                 weight[k++] = vWeight;
  535.             }

  537.         }

  539.     }

  541. }
Created with JaCoCo 0.7.4.201502262128