UnscentedKalmanModel.java

  1. /* Copyright 2002-2022 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.sequential;

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

  24. import org.hipparchus.filtering.kalman.ProcessEstimate;
  25. import org.hipparchus.filtering.kalman.unscented.UnscentedEvolution;
  26. import org.hipparchus.filtering.kalman.unscented.UnscentedProcess;
  27. import org.hipparchus.linear.ArrayRealVector;
  28. import org.hipparchus.linear.MatrixUtils;
  29. import org.hipparchus.linear.RealMatrix;
  30. import org.hipparchus.linear.RealVector;
  31. import org.orekit.estimation.measurements.EstimatedMeasurement;
  32. import org.orekit.estimation.measurements.ObservedMeasurement;
  33. import org.orekit.orbits.Orbit;
  34. import org.orekit.orbits.OrbitType;
  35. import org.orekit.orbits.PositionAngle;
  36. import org.orekit.propagation.Propagator;
  37. import org.orekit.propagation.PropagatorsParallelizer;
  38. import org.orekit.propagation.SpacecraftState;
  39. import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
  40. import org.orekit.propagation.numerical.NumericalPropagator;
  41. import org.orekit.time.AbsoluteDate;
  42. import org.orekit.utils.ParameterDriver;
  43. import org.orekit.utils.ParameterDriversList;
  44. import org.orekit.utils.ParameterDriversList.DelegatingDriver;

  45. /** Class defining the process model dynamics to use with a {@link UnscentedKalmanEstimator}.
  46.  * @author GaĆ«tan Pierre
  47.  * @author Bryan Cazabonne
  48.  * @since 11.3
  49.  */
  50. public class UnscentedKalmanModel implements KalmanEstimation, UnscentedProcess<MeasurementDecorator> {

  51.     /** Builders for propagators. */
  52.     private final List<NumericalPropagatorBuilder> builders;

  53.     /** Estimated orbital parameters. */
  54.     private final ParameterDriversList allEstimatedOrbitalParameters;

  55.     /** Estimated propagation drivers. */
  56.     private final ParameterDriversList allEstimatedPropagationParameters;

  57.     /** Per-builder estimated orbital parameters. */
  58.     private final ParameterDriversList[] estimatedOrbitalParameters;

  59.     /** Per-builder estimated propagation parameters. */
  60.     private final ParameterDriversList[] estimatedPropagationParameters;

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

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

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

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

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

  71.     /** Provider for covariance matrice. */
  72.     private final List<CovarianceMatrixProvider> covarianceMatrixProviders;

  73.     /** Process noise matrix provider for measurement parameters. */
  74.     private final CovarianceMatrixProvider measurementProcessNoiseMatrix;

  75.     /** Indirection arrays to extract the noise components for estimated parameters. */
  76.     private final int[][] covarianceIndirection;

  77.     /** Position angle types used during orbit determination. */
  78.     private final PositionAngle[] angleTypes;

  79.     /** Orbit types used during orbit determination. */
  80.     private final OrbitType[] orbitTypes;

  81.     /** Current number of measurement. */
  82.     private int currentMeasurementNumber;

  83.     /** Current corrected estimate. */
  84.     private ProcessEstimate correctedEstimate;

  85.     /** Current date. */
  86.     private AbsoluteDate currentDate;

  87.     /** Previous date. */
  88.     private AbsoluteDate previousDate;

  89.     /** Predicted spacecraft states. */
  90.     private SpacecraftState[] predictedSpacecraftStates;

  91.     /** Corrected spacecraft states. */
  92.     private SpacecraftState[] correctedSpacecraftStates;

  93.     /** Predicted measurement. */
  94.     private EstimatedMeasurement<?> predictedMeasurement;

  95.     /** Corrected measurement. */
  96.     private EstimatedMeasurement<?> correctedMeasurement;

  97.     /** Unscented Kalman process model constructor (package private).
  98.      * @param propagatorBuilders propagators builders used to evaluate the orbits.
  99.      * @param covarianceMatrixProviders provider for covariance matrix
  100.      * @param estimatedMeasurementParameters measurement parameters to estimate
  101.      * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
  102.      */
  103.     protected UnscentedKalmanModel(final List<NumericalPropagatorBuilder> propagatorBuilders,
  104.                                    final List<CovarianceMatrixProvider> covarianceMatrixProviders,
  105.                                    final ParameterDriversList estimatedMeasurementParameters,
  106.                                    final CovarianceMatrixProvider measurementProcessNoiseMatrix) {

  107.         this.builders                        = propagatorBuilders;
  108.         this.estimatedMeasurementsParameters = estimatedMeasurementParameters;
  109.         this.measurementParameterColumns     = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
  110.         this.currentMeasurementNumber        = 0;
  111.         this.currentDate                     = propagatorBuilders.get(0).getInitialOrbitDate();
  112.         this.previousDate                    = currentDate;
  113.         this.covarianceMatrixProviders       = covarianceMatrixProviders;
  114.         this.measurementProcessNoiseMatrix   = measurementProcessNoiseMatrix;

  115.         final Map<String, Integer> orbitalParameterColumns = new HashMap<>(6 * builders.size());
  116.         orbitsStartColumns      = new int[builders.size()];
  117.         orbitsEndColumns        = new int[builders.size()];
  118.         int columns = 0;
  119.         allEstimatedOrbitalParameters = new ParameterDriversList();
  120.         estimatedOrbitalParameters    = new ParameterDriversList[builders.size()];
  121.         // Set estimated orbital parameters
  122.         for (int k = 0; k < builders.size(); ++k) {
  123.             estimatedOrbitalParameters[k] = new ParameterDriversList();
  124.             orbitsStartColumns[k] = columns;
  125.             final String suffix = propagatorBuilders.size() > 1 ? "[" + k + "]" : null;
  126.             for (final ParameterDriver driver : builders.get(k).getOrbitalParametersDrivers().getDrivers()) {
  127.                 if (driver.getReferenceDate() == null) {
  128.                     driver.setReferenceDate(currentDate);
  129.                 }
  130.                 if (suffix != null && !driver.getName().endsWith(suffix)) {
  131.                     // we add suffix only conditionally because the method may already have been called
  132.                     // and suffixes may have already been appended
  133.                     driver.setName(driver.getName() + suffix);
  134.                 }
  135.                 if (driver.isSelected()) {
  136.                     allEstimatedOrbitalParameters.add(driver);
  137.                     estimatedOrbitalParameters[k].add(driver);
  138.                     orbitalParameterColumns.put(driver.getName(), columns++);
  139.                 }
  140.             }
  141.             orbitsEndColumns[k] = columns;
  142.         }

  143.         // Set estimated propagation parameters
  144.         allEstimatedPropagationParameters = new ParameterDriversList();
  145.         estimatedPropagationParameters    = new ParameterDriversList[builders.size()];
  146.         final List<String> estimatedPropagationParametersNames = new ArrayList<>();
  147.         for (int k = 0; k < builders.size(); ++k) {
  148.             estimatedPropagationParameters[k] = new ParameterDriversList();
  149.             for (final ParameterDriver driver : builders.get(k).getPropagationParametersDrivers().getDrivers()) {
  150.                 if (driver.getReferenceDate() == null) {
  151.                     driver.setReferenceDate(currentDate);
  152.                 }
  153.                 if (driver.isSelected()) {
  154.                     allEstimatedPropagationParameters.add(driver);
  155.                     estimatedPropagationParameters[k].add(driver);
  156.                     final String driverName = driver.getName();
  157.                     // Add the driver name if it has not been added yet
  158.                     if (!estimatedPropagationParametersNames.contains(driverName)) {
  159.                         estimatedPropagationParametersNames.add(driverName);
  160.                     }
  161.                 }
  162.             }
  163.         }
  164.         estimatedPropagationParametersNames.sort(Comparator.naturalOrder());

  165.         // Populate the map of propagation drivers' columns and update the total number of columns
  166.         propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
  167.         for (final String driverName : estimatedPropagationParametersNames) {
  168.             propagationParameterColumns.put(driverName, columns++);
  169.         }

  170.         // Populate the map of measurement drivers' columns and update the total number of columns
  171.         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
  172.             // Verify if the driver reference date has been set
  173.             if (parameter.getReferenceDate() == null) {
  174.                 parameter.setReferenceDate(currentDate);
  175.             }
  176.             measurementParameterColumns.put(parameter.getName(), columns);
  177.             columns++;
  178.         }

  179.         // set angle and orbit types
  180.         angleTypes = new PositionAngle[builders.size()];
  181.         orbitTypes = new OrbitType[builders.size()];
  182.         for (int k = 0; k < builders.size(); k++) {
  183.             angleTypes[k] = builders.get(k).getPositionAngle();
  184.             orbitTypes[k] = builders.get(k).getOrbitType();
  185.         }

  186.         // set covariance indirection
  187.         this.covarianceIndirection = new int[covarianceMatrixProviders.size()][columns];

  188.         for (int k = 0; k < covarianceIndirection.length; ++k) {
  189.             final ParameterDriversList orbitDrivers      = builders.get(k).getOrbitalParametersDrivers();
  190.             final ParameterDriversList parametersDrivers = builders.get(k).getPropagationParametersDrivers();
  191.             Arrays.fill(covarianceIndirection[k], -1);
  192.             int i = 0;
  193.             for (final ParameterDriver driver : orbitDrivers.getDrivers()) {
  194.                 final Integer c = orbitalParameterColumns.get(driver.getName());
  195.                 covarianceIndirection[k][i++] = (c == null) ? -1 : c.intValue();
  196.             }
  197.             for (final ParameterDriver driver : parametersDrivers.getDrivers()) {
  198.                 final Integer c = propagationParameterColumns.get(driver.getName());
  199.                 if (c != null) {
  200.                     covarianceIndirection[k][i++] = c.intValue();
  201.                 }
  202.             }
  203.             for (final ParameterDriver driver : estimatedMeasurementParameters.getDrivers()) {
  204.                 final Integer c = measurementParameterColumns.get(driver.getName());
  205.                 if (c != null) {
  206.                     covarianceIndirection[k][i++] = c.intValue();
  207.                 }
  208.             }
  209.         }

  210.         // Initialize the estimated state and fill its values
  211.         final RealVector correctedState = MatrixUtils.createRealVector(columns);

  212.         int p = 0;
  213.         for (final ParameterDriver driver : allEstimatedOrbitalParameters.getDrivers()) {
  214.             correctedState.setEntry(p++, driver.getValue());
  215.         }
  216.         for (final ParameterDriver driver : allEstimatedPropagationParameters.getDrivers()) {
  217.             correctedState.setEntry(p++, driver.getValue());
  218.         }
  219.         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
  220.             correctedState.setEntry(p++, driver.getValue());
  221.         }

  222.         this.predictedSpacecraftStates = new SpacecraftState[propagatorBuilders.size()];
  223.         for (int i = 0; i < propagatorBuilders.size(); i++) {
  224.             predictedSpacecraftStates[i] = propagatorBuilders.get(i).buildPropagator(propagatorBuilders.get(i).getSelectedNormalizedParameters()).getInitialState();
  225.         }
  226.         this.correctedSpacecraftStates = predictedSpacecraftStates.clone();

  227.         // Number of estimated measurement parameters
  228.         final int nbMeas = estimatedMeasurementParameters.getNbParams();

  229.         final RealMatrix correctedCovariance = MatrixUtils.createRealMatrix(columns, columns);
  230.         for (int k = 0; k < covarianceMatrixProviders.size(); k++) {
  231.             // Number of estimated dynamic parameters (orbital + propagation)
  232.             final int nbDyn  = orbitsEndColumns[k] - orbitsStartColumns[k] +
  233.                                estimatedPropagationParameters[k].getNbParams();
  234.             // Covariance matrix
  235.             final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
  236.             final RealMatrix noiseP = covarianceMatrixProviders.get(k).
  237.                                       getInitialCovarianceMatrix(correctedSpacecraftStates[k]);
  238.             noiseK.setSubMatrix(noiseP.getData(), 0, 0);
  239.             if (measurementProcessNoiseMatrix != null) {
  240.                 final RealMatrix noiseM = measurementProcessNoiseMatrix.
  241.                                           getInitialCovarianceMatrix(correctedSpacecraftStates[k]);
  242.                 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
  243.             }

  244.             KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
  245.                                                builders.get(k).getOrbitalParametersDrivers(),
  246.                                                builders.get(k).getPropagationParametersDrivers(),
  247.                                                estimatedMeasurementsParameters);

  248.             final int[] indK = covarianceIndirection[k];
  249.             for (int i = 0; i < indK.length; ++i) {
  250.                 if (indK[i] >= 0) {
  251.                     for (int j = 0; j < indK.length; ++j) {
  252.                         if (indK[j] >= 0) {
  253.                             correctedCovariance.setEntry(indK[i], indK[j], noiseK.getEntry(i, j));
  254.                         }
  255.                     }
  256.                 }
  257.             }
  258.         }

  259.         // Initialize corrected estimate
  260.         this.correctedEstimate = new ProcessEstimate(0.0, correctedState, correctedCovariance);

  261.     }

  262.     /** {@inheritDoc} */
  263.     @Override
  264.     public UnscentedEvolution getEvolution(final double previousTime, final RealVector[] sigmaPoints,
  265.                                            final MeasurementDecorator measurement) {

  266.         // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
  267.         final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
  268.         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
  269.             if (driver.getReferenceDate() == null) {
  270.                 driver.setReferenceDate(builders.get(0).getInitialOrbitDate());
  271.             }
  272.         }

  273.         // Increment measurement number
  274.         ++currentMeasurementNumber;

  275.         // Update the current date
  276.         currentDate = measurement.getObservedMeasurement().getDate();

  277.         // Initialize arrays of predicted states and measurements
  278.         final RealVector[] predictedStates       = new RealVector[sigmaPoints.length];
  279.         final RealVector[] predictedMeasurements = new RealVector[sigmaPoints.length];

  280.         // Initialize the relevant states used for measurement estimation
  281.         final SpacecraftState[][] statesForMeasurementEstimation = new SpacecraftState[sigmaPoints.length][builders.size()];

  282.         // Loop on builders
  283.         for (int k = 0; k < builders.size(); k++ ) {

  284.             // Sigma points for the current builder
  285.             final RealVector[] currentSigmaPoints = new ArrayRealVector[sigmaPoints.length];
  286.             for (int i = 0; i < sigmaPoints.length; i++) {
  287.                 currentSigmaPoints[i] = sigmaPoints[i].getSubVector(orbitsStartColumns[k], orbitsEndColumns[k] - orbitsStartColumns[k]);
  288.             }

  289.             // Predict states
  290.             final List<SpacecraftState> states = predictStates(currentSigmaPoints, k);

  291.             // Loop on states
  292.             for (int i = 0; i < states.size(); ++i) {
  293.                 if (k == 0) {
  294.                     predictedStates[i] = new ArrayRealVector(sigmaPoints[i].getDimension());
  295.                 }
  296.                 // Current predicted state
  297.                 final SpacecraftState predicted = states.get(i);
  298.                 // First, convert the predicted state to an array
  299.                 final double[] predictedArray = new double[currentSigmaPoints[i].getDimension()];
  300.                 orbitTypes[k].mapOrbitToArray(predicted.getOrbit(), angleTypes[k], predictedArray, null);
  301.                 predictedStates[i].setSubVector(orbitsStartColumns[k], new ArrayRealVector(predictedArray));
  302.                 // One has the same information in predictedStates
  303.                 // and statesForMeasurementEstimation but differently arranged
  304.                 statesForMeasurementEstimation[i][k] = predicted;
  305.             }
  306.         }

  307.         // Loop on sigma points to predict measurements
  308.         for (int i = 0; i < sigmaPoints.length; i++) {
  309.             final EstimatedMeasurement<?> estimated = observedMeasurement.estimate(currentMeasurementNumber, currentMeasurementNumber,
  310.                                                                                    KalmanEstimatorUtil.filterRelevant(observedMeasurement,
  311.                                                                                                                       statesForMeasurementEstimation[i]));
  312.             predictedMeasurements[i] = new ArrayRealVector(estimated.getEstimatedValue());
  313.         }

  314.         // compute process noise matrix
  315.         final RealMatrix processNoise = MatrixUtils.createRealMatrix(sigmaPoints[0].getDimension(), sigmaPoints[0].getDimension());

  316.         for (int k = 0; k < covarianceMatrixProviders.size(); ++k) {

  317.             // Number of estimated measurement parameters
  318.             final int nbMeas = estimatedMeasurementsParameters.getNbParams();

  319.             // Number of estimated dynamic parameters (orbital + propagation)
  320.             final int nbDyn  = orbitsEndColumns[k] - orbitsStartColumns[k] +
  321.                                estimatedPropagationParameters[k].getNbParams();

  322.             // Covariance matrix
  323.             final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
  324.             final RealMatrix noiseP = covarianceMatrixProviders.get(k).
  325.                                       getProcessNoiseMatrix(correctedSpacecraftStates[k],
  326.                                                             predictedSpacecraftStates[k]);
  327.             noiseK.setSubMatrix(noiseP.getData(), 0, 0);
  328.             if (measurementProcessNoiseMatrix != null) {
  329.                 final RealMatrix noiseM = measurementProcessNoiseMatrix.
  330.                                           getProcessNoiseMatrix(correctedSpacecraftStates[k],
  331.                                                                 predictedSpacecraftStates[k]);
  332.                 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
  333.             }

  334.             KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
  335.                                                builders.get(k).getOrbitalParametersDrivers(),
  336.                                                builders.get(k).getPropagationParametersDrivers(),
  337.                                                estimatedMeasurementsParameters);

  338.             final int[] indK = covarianceIndirection[k];
  339.             for (int i = 0; i < indK.length; ++i) {
  340.                 if (indK[i] >= 0) {
  341.                     for (int j = 0; j < indK.length; ++j) {
  342.                         if (indK[j] >= 0) {
  343.                             processNoise.setEntry(indK[i], indK[j], noiseK.getEntry(i, j));
  344.                         }
  345.                     }
  346.                 }
  347.             }

  348.         }

  349.         // Update epoch
  350.         previousDate = currentDate;

  351.         // Return
  352.         return new UnscentedEvolution(measurement.getTime(), predictedStates, predictedMeasurements, processNoise);
  353.     }

  354.     /** {@inheritDoc} */
  355.     @Override
  356.     public RealVector getInnovation(final MeasurementDecorator measurement, final RealVector predictedMeas,
  357.                                     final RealVector predictedState, final RealMatrix innovationCovarianceMatrix) {

  358.         // Loop on builders
  359.         for (int k = 0; k < builders.size(); k++) {

  360.             // Update predicted states
  361.             final double[] predictedStateArray = predictedState.getSubVector(orbitsStartColumns[k], orbitsEndColumns[k] - orbitsStartColumns[k]).toArray();
  362.             final Orbit predictedOrbit = orbitTypes[k].mapArrayToOrbit(predictedStateArray, null, angleTypes[k],
  363.                                                                        currentDate, builders.get(k).getMu(), builders.get(k).getFrame());
  364.             predictedSpacecraftStates[k] = new SpacecraftState(predictedOrbit);

  365.             // Update the builder with the predicted orbit
  366.             builders.get(k).resetOrbit(predictedOrbit);

  367.         }

  368.         // Predicted measurement
  369.         predictedMeasurement = measurement.getObservedMeasurement().estimate(currentMeasurementNumber, currentMeasurementNumber,
  370.                                                                              KalmanEstimatorUtil.filterRelevant(measurement.getObservedMeasurement(),
  371.                                                                                                                 predictedSpacecraftStates));
  372.         predictedMeasurement.setEstimatedValue(predictedMeas.toArray());

  373.         // set estimated value to the predicted value by the filter
  374.         KalmanEstimatorUtil.applyDynamicOutlierFilter(predictedMeasurement, innovationCovarianceMatrix);

  375.         // Compute the innovation vector (not normalized for unscented Kalman filter)
  376.         return KalmanEstimatorUtil.computeInnovationVector(predictedMeasurement);

  377.     }

  378.     /**
  379.      * Predict the predicted states for the given sigma points.
  380.      * @param sigmaPoints current sigma points
  381.      * @param index the index corresponding to the satellite one is dealing with
  382.      * @return predicted state for the given sigma point
  383.      */
  384.     private List<SpacecraftState> predictStates(final RealVector[] sigmaPoints, final int index) {

  385.         // Loop on sigma points to create the propagator parallelizer
  386.         final List<Propagator> propagators = new ArrayList<>(sigmaPoints.length);
  387.         for (int k = 0; k < sigmaPoints.length; ++k) {
  388.             // Current sigma point
  389.             final double[] currentPoint = sigmaPoints[k].copy().toArray();
  390.             // Create the corresponding orbit propagator
  391.             final Propagator currentPropagator = createPropagator(currentPoint, index);
  392.             // Add it to the list of propagators
  393.             propagators.add(currentPropagator);
  394.         }

  395.         // Create the propagator parallelizer and predict states
  396.         // (the shift is done to start a little bit before the previous measurement epoch)
  397.         final PropagatorsParallelizer parallelizer = new PropagatorsParallelizer(propagators, interpolators -> { });
  398.         final List<SpacecraftState>   states       = parallelizer.propagate(previousDate.shiftedBy(-1.0e-3), currentDate);

  399.         // Return
  400.         return states;

  401.     }

  402.     /**
  403.      * Create a propagator for the given sigma point.
  404.      * @param point input sigma point
  405.      * @param index the index corresponding to the satellite one is dealing with
  406.      * @return the corresponding orbit propagator
  407.      */
  408.     private Propagator createPropagator(final double[] point, final int index) {
  409.         // Create a new instance of the current propagator builder
  410.         final NumericalPropagatorBuilder copy = builders.get(index).copy();
  411.         // Convert the given sigma point to an orbit
  412.         final Orbit orbit = orbitTypes[index].mapArrayToOrbit(point, null, angleTypes[index], copy.getInitialOrbitDate(),
  413.                                                       copy.getMu(), copy.getFrame());
  414.         copy.resetOrbit(orbit);
  415.         // Create the propagator
  416.         final NumericalPropagator propagator = copy.buildPropagator(copy.getSelectedNormalizedParameters());
  417.         return propagator;
  418.     }

  419.     /** Finalize estimation.
  420.      * @param observedMeasurement measurement that has just been processed
  421.      * @param estimate corrected estimate
  422.      */
  423.     public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
  424.                                    final ProcessEstimate estimate) {

  425.         correctedEstimate = estimate;

  426.         // Loop on builders
  427.         for (int k = 0; k < builders.size(); k++ ) {

  428.             // Update corrected state
  429.             final double[] correctedStateArray = estimate.getState().getSubVector(orbitsStartColumns[k], orbitsEndColumns[k] - orbitsStartColumns[k]).toArray();
  430.             final Orbit correctedOrbit = orbitTypes[k].mapArrayToOrbit(correctedStateArray, null, angleTypes[k],
  431.                                                                    currentDate, builders.get(k).getMu(), builders.get(k).getFrame());
  432.             correctedSpacecraftStates[k] = new SpacecraftState(correctedOrbit);

  433.             // Update the builder
  434.             builders.get(k).resetOrbit(correctedOrbit);

  435.         }

  436.         // Corrected measurement
  437.         correctedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
  438.                                                             currentMeasurementNumber,
  439.                                                             KalmanEstimatorUtil.filterRelevant(observedMeasurement,
  440.                                                                                                getCorrectedSpacecraftStates()));
  441.     }

  442.     /** Get the propagators estimated with the values set in the propagators builders.
  443.      * @return propagators based on the current values in the builder
  444.      */
  445.     public Propagator[] getEstimatedPropagators() {
  446.         // Return propagators built with current instantiation of the propagator builders
  447.         final Propagator[] propagators = new Propagator[builders.size()];
  448.         for (int k = 0; k < builders.size(); ++k) {
  449.             propagators[k] = builders.get(k).buildPropagator(builders.get(k).getSelectedNormalizedParameters());
  450.         }
  451.         return propagators;
  452.     }

  453.     /** Get the current corrected estimate.
  454.      * @return current corrected estimate
  455.      */
  456.     public ProcessEstimate getEstimate() {
  457.         return correctedEstimate;
  458.     }

  459.     /** {@inheritDoc} */
  460.     @Override
  461.     public ParameterDriversList getEstimatedOrbitalParameters() {
  462.         return allEstimatedOrbitalParameters;
  463.     }

  464.     /** {@inheritDoc} */
  465.     @Override
  466.     public ParameterDriversList getEstimatedPropagationParameters() {
  467.         return allEstimatedPropagationParameters;
  468.     }

  469.     /** {@inheritDoc} */
  470.     @Override
  471.     public ParameterDriversList getEstimatedMeasurementsParameters() {
  472.         return estimatedMeasurementsParameters;
  473.     }

  474.     /** {@inheritDoc} */
  475.     @Override
  476.     public SpacecraftState[] getPredictedSpacecraftStates() {
  477.         return predictedSpacecraftStates.clone();
  478.     }

  479.     /** {@inheritDoc} */
  480.     @Override
  481.     public SpacecraftState[] getCorrectedSpacecraftStates() {
  482.         return correctedSpacecraftStates.clone();
  483.     }

  484.     /** {@inheritDoc} */
  485.     @Override
  486.     public RealVector getPhysicalEstimatedState() {
  487.         // Method {@link ParameterDriver#getValue()} is used to get
  488.         // the physical values of the state.
  489.         // The scales'array is used to get the size of the state vector
  490.         final RealVector physicalEstimatedState = new ArrayRealVector(getEstimate().getState().getDimension());
  491.         int i = 0;
  492.         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
  493.             physicalEstimatedState.setEntry(i++, driver.getValue());
  494.         }
  495.         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
  496.             physicalEstimatedState.setEntry(i++, driver.getValue());
  497.         }
  498.         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
  499.             physicalEstimatedState.setEntry(i++, driver.getValue());
  500.         }

  501.         return physicalEstimatedState;
  502.     }

  503.     /** {@inheritDoc} */
  504.     @Override
  505.     public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
  506.         return correctedEstimate.getCovariance();
  507.     }


  508.     /** {@inheritDoc} */
  509.     @Override
  510.     public RealMatrix getPhysicalStateTransitionMatrix() {
  511.         return null;
  512.     }

  513.     /** {@inheritDoc} */
  514.     @Override
  515.     public RealMatrix getPhysicalMeasurementJacobian() {
  516.         return null;
  517.     }

  518.     /** {@inheritDoc} */
  519.     @Override
  520.     public RealMatrix getPhysicalInnovationCovarianceMatrix() {
  521.         return correctedEstimate.getInnovationCovariance();
  522.     }

  523.     /** {@inheritDoc} */
  524.     @Override
  525.     public RealMatrix getPhysicalKalmanGain() {
  526.         return correctedEstimate.getKalmanGain();
  527.     }

  528.     /** {@inheritDoc} */
  529.     @Override
  530.     public int getCurrentMeasurementNumber() {
  531.         return currentMeasurementNumber;
  532.     }

  533.     /** {@inheritDoc} */
  534.     @Override
  535.     public AbsoluteDate getCurrentDate() {
  536.         return currentDate;
  537.     }

  538.     /** {@inheritDoc} */
  539.     @Override
  540.     public EstimatedMeasurement<?> getPredictedMeasurement() {
  541.         return predictedMeasurement;
  542.     }

  543.     /** {@inheritDoc} */
  544.     @Override
  545.     public EstimatedMeasurement<?> getCorrectedMeasurement() {
  546.         return correctedMeasurement;
  547.     }

  548. }