DSSTBatchLSModel.java

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

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

  25. import org.hipparchus.linear.Array2DRowRealMatrix;
  26. import org.hipparchus.linear.ArrayRealVector;
  27. import org.hipparchus.linear.MatrixUtils;
  28. import org.hipparchus.linear.RealMatrix;
  29. import org.hipparchus.linear.RealVector;
  30. import org.hipparchus.util.FastMath;
  31. import org.hipparchus.util.Incrementor;
  32. import org.hipparchus.util.Pair;
  33. import org.orekit.estimation.measurements.EstimatedMeasurement;
  34. import org.orekit.estimation.measurements.ObservedMeasurement;
  35. import org.orekit.orbits.Orbit;
  36. import org.orekit.propagation.PropagationType;
  37. import org.orekit.propagation.Propagator;
  38. import org.orekit.propagation.PropagatorsParallelizer;
  39. import org.orekit.propagation.SpacecraftState;
  40. import org.orekit.propagation.conversion.IntegratedPropagatorBuilder;
  41. import org.orekit.propagation.sampling.MultiSatStepHandler;
  42. import org.orekit.propagation.semianalytical.dsst.DSSTJacobiansMapper;
  43. import org.orekit.propagation.semianalytical.dsst.DSSTPartialDerivativesEquations;
  44. import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
  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. import org.orekit.utils.ParameterDriversList.DelegatingDriver;

  50. /** Bridge between {@link ObservedMeasurement measurements} and {@link
  51.  * org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem
  52.  * least squares problems}.
  53.  * <p>
  54.  * This class is an adaption of the {@link BatchLSModel} class
  55.  * but for the {@link DSSTPropagator DSST propagator}.
  56.  * </p>
  57.  * @author Luc Maisonobe
  58.  * @author Bryan Cazabonne
  59.  * @since 10.0
  60.  *
  61.  */
  62. public class DSSTBatchLSModel implements BatchLSODModel {

  63.     /** Builders for propagators. */
  64.     private final IntegratedPropagatorBuilder[] builders;

  65.     /** Array of each builder's selected propagation drivers. */
  66.     private final ParameterDriversList[] estimatedPropagationParameters;

  67.     /** Estimated measurements parameters. */
  68.     private final ParameterDriversList estimatedMeasurementsParameters;

  69.     /** Measurements. */
  70.     private final List<ObservedMeasurement<?>> measurements;

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

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

  75.     /** Map for propagation parameters columns. */
  76.     private final Map<String, Integer> propagationParameterColumns;

  77.     /** Map for measurements parameters columns. */
  78.     private final Map<String, Integer> measurementParameterColumns;

  79.     /** Last evaluations. */
  80.     private final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> evaluations;

  81.     /** Observer to be notified at orbit changes. */
  82.     private final ModelObserver observer;

  83.     /** Counter for the evaluations. */
  84.     private Incrementor evaluationsCounter;

  85.     /** Counter for the iterations. */
  86.     private Incrementor iterationsCounter;

  87.     /** Date of the first enabled measurement. */
  88.     private AbsoluteDate firstDate;

  89.     /** Date of the last enabled measurement. */
  90.     private AbsoluteDate lastDate;

  91.     /** Boolean indicating if the propagation will go forward or backward. */
  92.     private final boolean forwardPropagation;

  93.     /** Mappers for Jacobians. */
  94.     private DSSTJacobiansMapper[] mappers;

  95.     /** Model function value. */
  96.     private RealVector value;

  97.     /** Model function Jacobian. */
  98.     private RealMatrix jacobian;

  99.     /** Type of the orbit used for the propagation.*/
  100.     private PropagationType propagationType;

  101.     /** Type of the elements used to define the orbital state.*/
  102.     private PropagationType stateType;

  103.     /** Simple constructor.
  104.      * @param propagatorBuilders builders to use for propagation
  105.      * @param measurements measurements
  106.      * @param estimatedMeasurementsParameters estimated measurements parameters
  107.      * @param observer observer to be notified at model calls
  108.      * @param propagationType type of the orbit used for the propagation (mean or osculating)
  109.      * @param stateType type of the elements used to define the orbital state (mean or osculating)
  110.      */
  111.     public DSSTBatchLSModel(final IntegratedPropagatorBuilder[] propagatorBuilders,
  112.                      final List<ObservedMeasurement<?>> measurements,
  113.                      final ParameterDriversList estimatedMeasurementsParameters,
  114.                      final ModelObserver observer,
  115.                      final PropagationType propagationType,
  116.                      final PropagationType stateType) {

  117.         this.builders                        = propagatorBuilders.clone();
  118.         this.measurements                    = measurements;
  119.         this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
  120.         this.measurementParameterColumns     = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
  121.         this.estimatedPropagationParameters  = new ParameterDriversList[builders.length];
  122.         this.evaluations                     = new IdentityHashMap<>(measurements.size());
  123.         this.observer                        = observer;
  124.         this.mappers                         = new DSSTJacobiansMapper[builders.length];
  125.         this.propagationType                 = propagationType;
  126.         this.stateType                       = stateType;

  127.         // allocate vector and matrix
  128.         int rows = 0;
  129.         for (final ObservedMeasurement<?> measurement : measurements) {
  130.             rows += measurement.getDimension();
  131.         }

  132.         this.orbitsStartColumns = new int[builders.length];
  133.         this.orbitsEndColumns   = new int[builders.length];
  134.         int columns = 0;
  135.         for (int i = 0; i < builders.length; ++i) {
  136.             this.orbitsStartColumns[i] = columns;
  137.             for (final ParameterDriver driver : builders[i].getOrbitalParametersDrivers().getDrivers()) {
  138.                 if (driver.isSelected()) {
  139.                     ++columns;
  140.                 }
  141.             }
  142.             this.orbitsEndColumns[i] = columns;
  143.         }

  144.         // Gather all the propagation drivers names in a list
  145.         final List<String> estimatedPropagationParametersNames = new ArrayList<>();
  146.         for (int i = 0; i < builders.length; ++i) {
  147.             // The index i in array estimatedPropagationParameters (attribute of the class) is populated
  148.             // when the first call to getSelectedPropagationDriversForBuilder(i) is made
  149.             for (final DelegatingDriver delegating : getSelectedPropagationDriversForBuilder(i).getDrivers()) {
  150.                 final String driverName = delegating.getName();
  151.                 // Add the driver name if it has not been added yet
  152.                 if (!estimatedPropagationParametersNames.contains(driverName)) {
  153.                     estimatedPropagationParametersNames.add(driverName);
  154.                 }
  155.             }
  156.         }
  157.         // Populate the map of propagation drivers' columns and update the total number of columns
  158.         propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
  159.         for (final String driverName : estimatedPropagationParametersNames) {
  160.             propagationParameterColumns.put(driverName, columns);
  161.             ++columns;
  162.         }


  163.         // Populate the map of measurement drivers' columns and update the total number of columns
  164.         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
  165.             measurementParameterColumns.put(parameter.getName(), columns);
  166.             ++columns;
  167.         }

  168.         // Initialize point and value
  169.         value    = new ArrayRealVector(rows);
  170.         jacobian = MatrixUtils.createRealMatrix(rows, columns);

  171.         // Decide whether the propagation will be done forward or backward.
  172.         // Minimize the duration between first measurement treated and orbit determination date
  173.         // Propagator builder number 0 holds the reference date for orbit determination
  174.         final AbsoluteDate refDate = builders[0].getInitialOrbitDate();

  175.         // Sort the measurement list chronologically
  176.         measurements.sort(new ChronologicalComparator());
  177.         firstDate = measurements.get(0).getDate();
  178.         lastDate  = measurements.get(measurements.size() - 1).getDate();

  179.         // Decide the direction of propagation
  180.         if (FastMath.abs(refDate.durationFrom(firstDate)) <= FastMath.abs(refDate.durationFrom(lastDate))) {
  181.             // Propagate forward from firstDate
  182.             forwardPropagation = true;
  183.         } else {
  184.             // Propagate backward from lastDate
  185.             forwardPropagation = false;
  186.         }
  187.     }

  188.     /** {@inheritDoc} */
  189.     public void setEvaluationsCounter(final Incrementor evaluationsCounter) {
  190.         this.evaluationsCounter = evaluationsCounter;
  191.     }

  192.     /** {@inheritDoc} */
  193.     public void setIterationsCounter(final Incrementor iterationsCounter) {
  194.         this.iterationsCounter = iterationsCounter;
  195.     }

  196.     /** {@inheritDoc} */
  197.     public boolean isForwardPropagation() {
  198.         return forwardPropagation;
  199.     }

  200.     /** {@inheritDoc} */
  201.     @Override
  202.     public Pair<RealVector, RealMatrix> value(final RealVector point) {

  203.         // Set up the propagators parallelizer
  204.         final DSSTPropagator[] propagators = createPropagators(point);
  205.         final Orbit[] orbits = new Orbit[propagators.length];
  206.         for (int i = 0; i < propagators.length; ++i) {
  207.             mappers[i] = configureDerivatives(propagators[i]);
  208.             orbits[i]  = propagators[i].getInitialState().getOrbit();
  209.         }
  210.         final PropagatorsParallelizer parallelizer =
  211.                         new PropagatorsParallelizer(Arrays.asList(propagators), configureMeasurements(point));

  212.         // Reset value and Jacobian
  213.         evaluations.clear();
  214.         value.set(0.0);
  215.         for (int i = 0; i < jacobian.getRowDimension(); ++i) {
  216.             for (int j = 0; j < jacobian.getColumnDimension(); ++j) {
  217.                 jacobian.setEntry(i, j, 0.0);
  218.             }
  219.         }

  220.         // Run the propagation, gathering residuals on the fly
  221.         if (forwardPropagation) {
  222.             // Propagate forward from firstDate
  223.             parallelizer.propagate(firstDate.shiftedBy(-1.0), lastDate.shiftedBy(+1.0));
  224.         } else {
  225.             // Propagate backward from lastDate
  226.             parallelizer.propagate(lastDate.shiftedBy(+1.0), firstDate.shiftedBy(-1.0));
  227.         }

  228.         observer.modelCalled(orbits, evaluations);

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

  230.     }

  231.     /** {@inheritDoc} */
  232.     public int getIterationsCount() {
  233.         return iterationsCounter.getCount();
  234.     }

  235.     /** {@inheritDoc} */
  236.     public int getEvaluationsCount() {
  237.         return evaluationsCounter.getCount();
  238.     }

  239.     /** {@inheritDoc} */
  240.     public ParameterDriversList getSelectedPropagationDriversForBuilder(final int iBuilder) {

  241.         // Lazy evaluation, create the list only if it hasn't been created yet
  242.         if (estimatedPropagationParameters[iBuilder] == null) {

  243.             // Gather the drivers
  244.             final ParameterDriversList selectedPropagationDrivers = new ParameterDriversList();
  245.             for (final DelegatingDriver delegating : builders[iBuilder].getPropagationParametersDrivers().getDrivers()) {
  246.                 if (delegating.isSelected()) {
  247.                     for (final ParameterDriver driver : delegating.getRawDrivers()) {
  248.                         selectedPropagationDrivers.add(driver);
  249.                     }
  250.                 }
  251.             }

  252.             // List of propagation drivers are sorted in the BatchLSEstimator class.
  253.             // Hence we need to sort this list so the parameters' indexes match
  254.             selectedPropagationDrivers.sort();

  255.             // Add the list of selected propagation drivers to the array
  256.             estimatedPropagationParameters[iBuilder] = selectedPropagationDrivers;
  257.         }
  258.         return estimatedPropagationParameters[iBuilder];
  259.     }

  260.     /** {@inheritDoc} */
  261.     public DSSTPropagator[] createPropagators(final RealVector point) {

  262.         final DSSTPropagator[] propagators = new DSSTPropagator[builders.length];

  263.         // Set up the propagators
  264.         for (int i = 0; i < builders.length; ++i) {

  265.             // Get the number of selected orbital drivers in the builder
  266.             final int nbOrb    = orbitsEndColumns[i] - orbitsStartColumns[i];

  267.             // Get the list of selected propagation drivers in the builder and its size
  268.             final ParameterDriversList selectedPropagationDrivers = getSelectedPropagationDriversForBuilder(i);
  269.             final int nbParams = selectedPropagationDrivers.getNbParams();

  270.             // Init the array of normalized parameters for the builder
  271.             final double[] propagatorArray = new double[nbOrb + nbParams];

  272.             // Add the orbital drivers normalized values
  273.             for (int j = 0; j < nbOrb; ++j) {
  274.                 propagatorArray[j] = point.getEntry(orbitsStartColumns[i] + j);
  275.             }

  276.             // Add the propagation drivers normalized values
  277.             for (int j = 0; j < nbParams; ++j) {
  278.                 propagatorArray[nbOrb + j] =
  279.                                 point.getEntry(propagationParameterColumns.get(selectedPropagationDrivers.getDrivers().get(j).getName()));
  280.             }

  281.             // Build the propagator
  282.             propagators[i] = (DSSTPropagator) builders[i].buildPropagator(propagatorArray);
  283.         }

  284.         return propagators;

  285.     }

  286.     /** Configure the multi-satellites handler to handle measurements.
  287.      * @param point evaluation point
  288.      * @return multi-satellites handler to handle measurements
  289.      */
  290.     private MultiSatStepHandler configureMeasurements(final RealVector point) {

  291.         // Set up the measurement parameters
  292.         int index = orbitsEndColumns[builders.length - 1] + propagationParameterColumns.size();
  293.         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
  294.             parameter.setNormalizedValue(point.getEntry(index++));
  295.         }

  296.         // Set up measurements handler
  297.         final List<PreCompensation> precompensated = new ArrayList<>();
  298.         for (final ObservedMeasurement<?> measurement : measurements) {
  299.             if (measurement.isEnabled()) {
  300.                 precompensated.add(new PreCompensation(measurement, evaluations.get(measurement)));
  301.             }
  302.         }
  303.         precompensated.sort(new ChronologicalComparator());

  304.         // Assign first and last date
  305.         firstDate = precompensated.get(0).getDate();
  306.         lastDate  = precompensated.get(precompensated.size() - 1).getDate();

  307.         // Reverse the list in case of backward propagation
  308.         if (!forwardPropagation) {
  309.             Collections.reverse(precompensated);
  310.         }

  311.         return new MeasurementHandler(this, precompensated);

  312.     }

  313.     /** Configure the propagator to compute derivatives.
  314.      * @param propagators {@link Propagator} to configure
  315.      * @return mapper for this propagator
  316.      */
  317.     private DSSTJacobiansMapper configureDerivatives(final DSSTPropagator propagators) {

  318.         final String equationName = DSSTBatchLSModel.class.getName() + "-derivatives";

  319.         final DSSTPartialDerivativesEquations partials = new DSSTPartialDerivativesEquations(equationName, propagators, propagationType);

  320.         // add the derivatives to the initial state
  321.         final SpacecraftState rawState = propagators.getInitialState();
  322.         final SpacecraftState stateWithDerivatives = partials.setInitialJacobians(rawState);
  323.         propagators.setInitialState(stateWithDerivatives, stateType);

  324.         return partials.getMapper();

  325.     }

  326.     /** {@inheritDoc} */
  327.     public void fetchEvaluatedMeasurement(final int index, final EstimatedMeasurement<?> evaluation) {

  328.         // States and observed measurement
  329.         final SpacecraftState[]      evaluationStates    = evaluation.getStates();
  330.         final ObservedMeasurement<?> observedMeasurement = evaluation.getObservedMeasurement();

  331.         // compute weighted residuals
  332.         evaluations.put(observedMeasurement, evaluation);
  333.         if (evaluation.getStatus() == EstimatedMeasurement.Status.REJECTED) {
  334.             return;
  335.         }

  336.         final double[] evaluated = evaluation.getEstimatedValue();
  337.         final double[] observed  = observedMeasurement.getObservedValue();
  338.         final double[] sigma     = observedMeasurement.getTheoreticalStandardDeviation();
  339.         final double[] weight    = evaluation.getObservedMeasurement().getBaseWeight();
  340.         for (int i = 0; i < evaluated.length; ++i) {
  341.             value.setEntry(index + i, weight[i] * (evaluated[i] - observed[i]) / sigma[i]);
  342.         }

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

  344.             final int p = observedMeasurement.getSatellites().get(k).getPropagatorIndex();

  345.             // partial derivatives of the current Cartesian coordinates with respect to current orbital state
  346.             final double[][] aCY = new double[6][6];
  347.             final Orbit currentOrbit = evaluationStates[k].getOrbit();
  348.             currentOrbit.getJacobianWrtParameters(builders[p].getPositionAngle(), aCY);
  349.             final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);

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

  353.             // short period derivatives
  354.             mappers[p].setShortPeriodJacobians(evaluationStates[k]);

  355.             // Jacobian of the measurement with respect to initial orbital state
  356.             final double[][] aYY0 = new double[6][6];
  357.             mappers[p].getStateJacobian(evaluationStates[k], aYY0);
  358.             final RealMatrix dYdY0 = new Array2DRowRealMatrix(aYY0, false);
  359.             final RealMatrix dMdY0 = dMdY.multiply(dYdY0);
  360.             for (int i = 0; i < dMdY0.getRowDimension(); ++i) {
  361.                 int jOrb = orbitsStartColumns[p];
  362.                 for (int j = 0; j < dMdY0.getColumnDimension(); ++j) {
  363.                     final ParameterDriver driver = builders[p].getOrbitalParametersDrivers().getDrivers().get(j);
  364.                     if (driver.isSelected()) {
  365.                         jacobian.setEntry(index + i, jOrb++,
  366.                                           weight[i] * dMdY0.getEntry(i, j) / sigma[i] * driver.getScale());
  367.                     }
  368.                 }
  369.             }

  370.             // Jacobian of the measurement with respect to propagation parameters
  371.             final ParameterDriversList selectedPropagationDrivers = getSelectedPropagationDriversForBuilder(p);
  372.             final int nbParams = selectedPropagationDrivers.getNbParams();
  373.             if ( nbParams > 0) {
  374.                 final double[][] aYPp  = new double[6][nbParams];
  375.                 mappers[p].getParametersJacobian(evaluationStates[k], aYPp);
  376.                 final RealMatrix dYdPp = new Array2DRowRealMatrix(aYPp, false);
  377.                 final RealMatrix dMdPp = dMdY.multiply(dYdPp);
  378.                 for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
  379.                     for (int j = 0; j < nbParams; ++j) {
  380.                         final ParameterDriver delegating = selectedPropagationDrivers.getDrivers().get(j);
  381.                         jacobian.addToEntry(index + i, propagationParameterColumns.get(delegating.getName()),
  382.                                             weight[i] * dMdPp.getEntry(i, j) / sigma[i] * delegating.getScale());
  383.                     }
  384.                 }
  385.             }

  386.         }

  387.         // Jacobian of the measurement with respect to measurements parameters
  388.         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
  389.             if (driver.isSelected()) {
  390.                 final double[] aMPm = evaluation.getParameterDerivatives(driver);
  391.                 for (int i = 0; i < aMPm.length; ++i) {
  392.                     jacobian.setEntry(index + i, measurementParameterColumns.get(driver.getName()),
  393.                                       weight[i] * aMPm[i] / sigma[i] * driver.getScale());
  394.                 }
  395.             }
  396.         }

  397.     }

  398. }