Model.java

  1. /* Copyright 2002-2017 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.estimation.leastsquares;

  18. import java.util.ArrayList;
  19. import java.util.Arrays;
  20. import java.util.HashMap;
  21. import java.util.IdentityHashMap;
  22. import java.util.List;
  23. import java.util.Map;

  24. import org.hipparchus.linear.Array2DRowRealMatrix;
  25. import org.hipparchus.linear.ArrayRealVector;
  26. import org.hipparchus.linear.MatrixUtils;
  27. import org.hipparchus.linear.RealMatrix;
  28. import org.hipparchus.linear.RealVector;
  29. import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
  30. import org.hipparchus.util.Incrementor;
  31. import org.hipparchus.util.Pair;
  32. import org.orekit.errors.OrekitException;
  33. import org.orekit.errors.OrekitExceptionWrapper;
  34. import org.orekit.estimation.measurements.EstimatedMeasurement;
  35. import org.orekit.estimation.measurements.ObservedMeasurement;
  36. import org.orekit.orbits.Orbit;
  37. import org.orekit.propagation.Propagator;
  38. import org.orekit.propagation.PropagatorsParallelizer;
  39. import org.orekit.propagation.SpacecraftState;
  40. import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
  41. import org.orekit.propagation.numerical.JacobiansMapper;
  42. import org.orekit.propagation.numerical.NumericalPropagator;
  43. import org.orekit.propagation.numerical.PartialDerivativesEquations;
  44. import org.orekit.propagation.sampling.MultiSatStepHandler;
  45. import org.orekit.time.AbsoluteDate;
  46. import org.orekit.time.ChronologicalComparator;
  47. import org.orekit.utils.ParameterDriver;
  48. import org.orekit.utils.ParameterDriversList;

  49. /** Bridge between {@link ObservedMeasurement measurements} and {@link
  50.  * org.hipparchus.fitting.leastsquares.LeastSquaresProblem
  51.  * least squares problems}.
  52.  * @author Luc Maisonobe
  53.  * @since 8.0
  54.  */
  55. class Model implements MultivariateJacobianFunction {

  56.     /** Estimated propagator parameters. */
  57.     private final ParameterDriversList estimatedPropagatorParameters;

  58.     /** Builder for propagator. */
  59.     private final NumericalPropagatorBuilder[] builders;

  60.     /** Measurements. */
  61.     private final List<ObservedMeasurement<?>> measurements;

  62.     /** Estimated measurements parameters. */
  63.     private final ParameterDriversList estimatedMeasurementsParameters;

  64.     /** Start columns for each estimated orbit. */
  65.     private final int[] orbitsStartColumns;

  66.     /** End columns for each estimated orbit. */
  67.     private final int[] orbitsEndColumns;

  68.     /** Map for measurements parameters columns. */
  69.     private final Map<String, Integer> parameterColumns;

  70.     /** Last evaluations. */
  71.     private final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> evaluations;

  72.     /** Observer to be notified at orbit changes. */
  73.     private final ModelObserver observer;

  74.     /** Counter for the evaluations. */
  75.     private Incrementor evaluationsCounter;

  76.     /** Counter for the iterations. */
  77.     private Incrementor iterationsCounter;

  78.     /** Date of the first enabled measurement. */
  79.     private AbsoluteDate firstDate;

  80.     /** Date of the last enabled measurement. */
  81.     private AbsoluteDate lastDate;

  82.     /** Mappers for Jacobians. */
  83.     private JacobiansMapper[] mappers;

  84.     /** Model function value. */
  85.     private RealVector value;

  86.     /** Model function Jacobian. */
  87.     private RealMatrix jacobian;

  88.     /** Simple constructor.
  89.      * @param builders builders to use for propagation
  90.      * @param measurements measurements
  91.      * @param estimatedMeasurementsParameters estimated measurements parameters
  92.      * @param observer observer to be notified at model calls
  93.      * @exception OrekitException if some propagator parameter cannot be set properly
  94.      */
  95.     Model(final NumericalPropagatorBuilder[] builders,
  96.           final List<ObservedMeasurement<?>> measurements, final ParameterDriversList estimatedMeasurementsParameters,
  97.           final ModelObserver observer)
  98.         throws OrekitException {

  99.         this.builders                        = builders;
  100.         this.measurements                    = measurements;
  101.         this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
  102.         this.parameterColumns                = new HashMap<String, Integer>(estimatedMeasurementsParameters.getDrivers().size());
  103.         this.evaluations                     = new IdentityHashMap<ObservedMeasurement<?>, EstimatedMeasurement<?>>(measurements.size());
  104.         this.observer                        = observer;
  105.         this.mappers                         = new JacobiansMapper[builders.length];

  106.         // allocate vector and matrix
  107.         int rows = 0;
  108.         for (final ObservedMeasurement<?> measurement : measurements) {
  109.             rows += measurement.getDimension();
  110.         }

  111.         this.orbitsStartColumns = new int[builders.length];
  112.         this.orbitsEndColumns   = new int[builders.length];
  113.         int columns = 0;
  114.         for (int i = 0; i < builders.length; ++i) {
  115.             this.orbitsStartColumns[i] = columns;
  116.             for (final ParameterDriver driver : builders[i].getOrbitalParametersDrivers().getDrivers()) {
  117.                 if (driver.isSelected()) {
  118.                     ++columns;
  119.                 }
  120.             }
  121.             this.orbitsEndColumns[i] = columns;
  122.         }

  123.         this.estimatedPropagatorParameters = new ParameterDriversList();
  124.         for (int i = 0; i < builders.length; ++i) {
  125.             for (final ParameterDriver driver : builders[i].getPropagationParametersDrivers().getDrivers()) {
  126.                 if (driver.isSelected()) {
  127.                     estimatedPropagatorParameters.add(driver);
  128.                     ++columns;
  129.                 }
  130.             }
  131.         }

  132.         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
  133.             parameterColumns.put(parameter.getName(), columns);
  134.             ++columns;
  135.         }

  136.         value    = new ArrayRealVector(rows);
  137.         jacobian = MatrixUtils.createRealMatrix(rows, columns);

  138.     }

  139.     /** Set the counter for evaluations.
  140.      * @param evaluationsCounter counter for evaluations
  141.      */
  142.     void setEvaluationsCounter(final Incrementor evaluationsCounter) {
  143.         this.evaluationsCounter = evaluationsCounter;
  144.     }

  145.     /** Set the counter for iterations.
  146.      * @param iterationsCounter counter for iterations
  147.      */
  148.     void setIterationsCounter(final Incrementor iterationsCounter) {
  149.         this.iterationsCounter = iterationsCounter;
  150.     }

  151.     /** {@inheritDoc} */
  152.     @Override
  153.     public Pair<RealVector, RealMatrix> value(final RealVector point)
  154.         throws OrekitExceptionWrapper {
  155.         try {

  156.             // set up the propagators parallelizer
  157.             final NumericalPropagator[] propagators = createPropagators(point);
  158.             final Orbit[] orbits = new Orbit[propagators.length];
  159.             for (int i = 0; i < propagators.length; ++i) {
  160.                 mappers[i] = configureDerivatives(propagators[i]);
  161.                 orbits[i]  = propagators[i].getInitialState().getOrbit();
  162.             }
  163.             final PropagatorsParallelizer parallelizer =
  164.                             new PropagatorsParallelizer(Arrays.asList(propagators), configureMeasurements(point));

  165.             // reset value and Jacobian
  166.             evaluations.clear();
  167.             value.set(0.0);
  168.             for (int i = 0; i < jacobian.getRowDimension(); ++i) {
  169.                 for (int j = 0; j < jacobian.getColumnDimension(); ++j) {
  170.                     jacobian.setEntry(i, j, 0.0);
  171.                 }
  172.             }

  173.             // run the propagation, gathering residuals on the fly
  174.             parallelizer.propagate(firstDate.shiftedBy(-1.0), lastDate.shiftedBy(+1.0));

  175.             observer.modelCalled(orbits, evaluations);

  176.             return new Pair<RealVector, RealMatrix>(value, jacobian);

  177.         } catch (OrekitException oe) {
  178.             throw new OrekitExceptionWrapper(oe);
  179.         }
  180.     }

  181.     /** Get the iterations count.
  182.      * @return iterations count
  183.      */
  184.     public int getIterationsCount() {
  185.         return iterationsCounter.getCount();
  186.     }

  187.     /** Get the evaluations count.
  188.      * @return evaluations count
  189.      */
  190.     public int getEvaluationsCount() {
  191.         return evaluationsCounter.getCount();
  192.     }

  193.     /** Create the propagators and parameters corresponding to an evaluation point.
  194.      * @param point evaluation point
  195.      * @return an array of new propagators
  196.      * @exception OrekitException if orbit cannot be created with the current point
  197.      */
  198.     public NumericalPropagator[] createPropagators(final RealVector point)
  199.         throws OrekitException {

  200.         final NumericalPropagator[] propagators = new NumericalPropagator[builders.length];

  201.         for (int i = 0; i < builders.length; ++i) {
  202.             // set up the propagator
  203.             final int nbOrb    = orbitsEndColumns[i] - orbitsStartColumns[i];
  204.             final int nbParams = estimatedPropagatorParameters.getNbParams();
  205.             final double[] propagatorArray = new double[nbOrb + nbParams];
  206.             for (int j = 0; j < nbOrb; ++j) {
  207.                 propagatorArray[j] = point.getEntry(orbitsStartColumns[i] + j);
  208.             }
  209.             for (int j = 0; j < nbParams; ++j) {
  210.                 propagatorArray[nbOrb + j] = point.getEntry(orbitsEndColumns[builders.length - 1] + j);
  211.             }
  212.             propagators[i] = builders[i].buildPropagator(propagatorArray);
  213.         }

  214.         return propagators;

  215.     }

  216.     /** Configure the multi-satellites handler to handle measurements.
  217.      * @param point evaluation point
  218.      * @return multi-satellites handler to handle measurements
  219.      * @exception OrekitException if measurements parameters cannot be set with the current point
  220.      */
  221.     private MultiSatStepHandler configureMeasurements(final RealVector point)
  222.         throws OrekitException {

  223.         // set up the measurement parameters
  224.         int index = orbitsEndColumns[builders.length - 1] + estimatedPropagatorParameters.getNbParams();
  225.         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
  226.             parameter.setNormalizedValue(point.getEntry(index++));
  227.         }

  228.         // set up measurements handler
  229.         final List<PreCompensation> precompensated = new ArrayList<>();
  230.         for (final ObservedMeasurement<?> measurement : measurements) {
  231.             if (measurement.isEnabled()) {
  232.                 precompensated.add(new PreCompensation(measurement, evaluations.get(measurement)));
  233.             }
  234.         }
  235.         precompensated.sort(new ChronologicalComparator());

  236.         firstDate = precompensated.get(0).getDate();
  237.         lastDate  = precompensated.get(precompensated.size() - 1).getDate();

  238.         return new MeasurementHandler(this, precompensated);

  239.     }

  240.     /** Configure the propagator to compute derivatives.
  241.      * @param propagator {@link Propagator} to configure
  242.      * @return mapper for this propagator
  243.      * @exception OrekitException if orbit cannot be created with the current point
  244.      */
  245.     private JacobiansMapper configureDerivatives(final NumericalPropagator propagator)
  246.         throws OrekitException {

  247.         final String equationName = Model.class.getName() + "-derivatives";
  248.         final PartialDerivativesEquations partials = new PartialDerivativesEquations(equationName, propagator);

  249.         // add the derivatives to the initial state
  250.         final SpacecraftState rawState = propagator.getInitialState();
  251.         final SpacecraftState stateWithDerivatives = partials.setInitialJacobians(rawState);
  252.         propagator.resetInitialState(stateWithDerivatives);

  253.         return partials.getMapper();

  254.     }

  255.     /** Fetch a measurement that was evaluated during propagation.
  256.      * @param index index of the measurement first component
  257.      * @param evaluation measurement evaluation
  258.      * @exception OrekitException if Jacobians cannot be computed
  259.      */
  260.     void fetchEvaluatedMeasurement(final int index, final EstimatedMeasurement<?> evaluation)
  261.         throws OrekitException {

  262.         // States and observed measurement
  263.         final SpacecraftState[]      evaluationStates    = evaluation.getStates();
  264.         final ObservedMeasurement<?> observedMeasurement = evaluation.getObservedMeasurement();

  265.         // compute weighted residuals
  266.         evaluations.put(observedMeasurement, evaluation);
  267.         final double[] evaluated = evaluation.getEstimatedValue();
  268.         final double[] observed  = observedMeasurement.getObservedValue();
  269.         final double[] sigma     = observedMeasurement.getTheoreticalStandardDeviation();
  270.         final double[] weight    = evaluation.getCurrentWeight();
  271.         for (int i = 0; i < evaluated.length; ++i) {
  272.             value.setEntry(index + i, weight[i] * (evaluated[i] - observed[i]) / sigma[i]);
  273.         }

  274.         for (int k = 0; k < evaluationStates.length; ++k) {

  275.             final int p = observedMeasurement.getPropagatorsIndices().get(k);

  276.             // partial derivatives of the current Cartesian coordinates with respect to current orbital state
  277.             final double[][] aCY = new double[6][6];
  278.             final Orbit currentOrbit = evaluationStates[k].getOrbit();
  279.             currentOrbit.getJacobianWrtParameters(builders[p].getPositionAngle(), aCY);
  280.             final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);

  281.             // Jacobian of the measurement with respect to current orbital state
  282.             final RealMatrix dMdC = new Array2DRowRealMatrix(evaluation.getStateDerivatives(k), false);
  283.             final RealMatrix dMdY = dMdC.multiply(dCdY);

  284.             // Jacobian of the measurement with respect to initial orbital state
  285.             final double[][] aYY0 = new double[6][6];
  286.             mappers[p].getStateJacobian(evaluationStates[k], aYY0);
  287.             final RealMatrix dYdY0 = new Array2DRowRealMatrix(aYY0, false);
  288.             final RealMatrix dMdY0 = dMdY.multiply(dYdY0);
  289.             for (int i = 0; i < dMdY0.getRowDimension(); ++i) {
  290.                 int jOrb = orbitsStartColumns[p];
  291.                 for (int j = 0; j < dMdY0.getColumnDimension(); ++j) {
  292.                     final ParameterDriver driver = builders[p].getOrbitalParametersDrivers().getDrivers().get(j);
  293.                     if (driver.isSelected()) {
  294.                         jacobian.setEntry(index + i, jOrb++,
  295.                                           weight[i] * dMdY0.getEntry(i, j) / sigma[i] * driver.getScale());
  296.                     }
  297.                 }
  298.             }

  299.             if (estimatedPropagatorParameters.getNbParams() > 0) {
  300.                 // Jacobian of the measurement with respect to propagator parameters
  301.                 final double[][] aYPp  = new double[6][estimatedPropagatorParameters.getNbParams()];
  302.                 mappers[p].getParametersJacobian(evaluationStates[k], aYPp);
  303.                 final RealMatrix dYdPp = new Array2DRowRealMatrix(aYPp, false);
  304.                 final RealMatrix dMdPp = dMdY.multiply(dYdPp);
  305.                 for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
  306.                     int jPar = orbitsEndColumns[builders.length - 1];
  307.                     for (int j = 0; j < estimatedPropagatorParameters.getNbParams(); ++j) {
  308.                         final ParameterDriver driver = estimatedPropagatorParameters.getDrivers().get(j);
  309.                         jacobian.addToEntry(index + i, jPar++,
  310.                                             weight[i] * dMdPp.getEntry(i, j) / sigma[i] * driver.getScale());
  311.                     }
  312.                 }
  313.             }

  314.         }

  315.         // Jacobian of the measurement with respect to measurements parameters
  316.         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
  317.             if (driver.isSelected()) {
  318.                 final double[] aMPm = evaluation.getParameterDerivatives(driver);
  319.                 for (int i = 0; i < aMPm.length; ++i) {
  320.                     jacobian.setEntry(index + i, parameterColumns.get(driver.getName()),
  321.                                       weight[i] * aMPm[i] / sigma[i] * driver.getScale());
  322.                 }
  323.             }
  324.         }

  325.     }

  326. }