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;

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

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

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

  55.     /** Builders for propagators. */
  56.     private final List<NumericalPropagatorBuilder> builders;

  58.     /** Estimated orbital parameters. */
  59.     private final ParameterDriversList allEstimatedOrbitalParameters;

  61.     /** Estimated propagation drivers. */
  62.     private final ParameterDriversList allEstimatedPropagationParameters;

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

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

  70.     /** Estimated measurements parameters. */
  71.     private final ParameterDriversList estimatedMeasurementsParameters;

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

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

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

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

  85.     /** Provider for covariance matrice. */
  86.     private final List<CovarianceMatrixProvider> covarianceMatrixProviders;

  88.     /** Process noise matrix provider for measurement parameters. */
  89.     private final CovarianceMatrixProvider measurementProcessNoiseMatrix;

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

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

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

  100.     /** Current number of measurement. */
  101.     private int currentMeasurementNumber;

  103.     /** Current corrected estimate. */
  104.     private ProcessEstimate correctedEstimate;

  106.     /** Current date. */
  107.     private AbsoluteDate currentDate;

  109.     /** Previous date. */
  110.     private AbsoluteDate previousDate;

  112.     /** Predicted spacecraft states. */
  113.     private SpacecraftState[] predictedSpacecraftStates;

  115.     /** Corrected spacecraft states. */
  116.     private SpacecraftState[] correctedSpacecraftStates;

  118.     /** Predicted measurement. */
  119.     private EstimatedMeasurement<?> predictedMeasurement;

  121.     /** Corrected measurement. */
  122.     private EstimatedMeasurement<?> correctedMeasurement;

  124.     /** Unscented Kalman process model constructor (package private).
  125.      * @param propagatorBuilders propagators builders used to evaluate the orbits.
  126.      * @param covarianceMatrixProviders provider for covariance matrix
  127.      * @param estimatedMeasurementParameters measurement parameters to estimate
  128.      * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
  129.      */
  130.     protected UnscentedKalmanModel(final List<NumericalPropagatorBuilder> propagatorBuilders,
  131.                                    final List<CovarianceMatrixProvider> covarianceMatrixProviders,
  132.                                    final ParameterDriversList estimatedMeasurementParameters,
  133.                                    final CovarianceMatrixProvider measurementProcessNoiseMatrix) {

  135.         this.builders                        = propagatorBuilders;
  136.         this.estimatedMeasurementsParameters = estimatedMeasurementParameters;
  137.         this.measurementParameterColumns     = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
  138.         this.currentMeasurementNumber        = 0;
  139.         this.currentDate                     = propagatorBuilders.get(0).getInitialOrbitDate();
  140.         this.previousDate                    = currentDate;
  141.         this.covarianceMatrixProviders       = covarianceMatrixProviders;
  142.         this.measurementProcessNoiseMatrix   = measurementProcessNoiseMatrix;

  144.         final Map<String, Integer> orbitalParameterColumns = new HashMap<>(6 * builders.size());
  145.         orbitsStartColumns      = new int[builders.size()];
  146.         orbitsEndColumns        = new int[builders.size()];
  147.         int columns = 0;
  148.         allEstimatedOrbitalParameters = new ParameterDriversList();
  149.         estimatedOrbitalParameters    = new ParameterDriversList[builders.size()];
  150.         // Set estimated orbital parameters
  151.         for (int k = 0; k < builders.size(); ++k) {
  152.             estimatedOrbitalParameters[k] = new ParameterDriversList();
  153.             orbitsStartColumns[k] = columns;
  154.             final String suffix = propagatorBuilders.size() > 1 ? "[" + k + "]" : null;
  155.             for (final ParameterDriver driver : builders.get(k).getOrbitalParametersDrivers().getDrivers()) {
  156.                 if (driver.getReferenceDate() == null) {
  157.                     driver.setReferenceDate(currentDate);
  158.                 }
  159.                 if (suffix != null && !driver.getName().endsWith(suffix)) {
  160.                     // we add suffix only conditionally because the method may already have been called
  161.                     // and suffixes may have already been appended
  162.                     driver.setName(driver.getName() + suffix);
  163.                 }
  164.                 if (driver.isSelected()) {
  165.                     allEstimatedOrbitalParameters.add(driver);
  166.                     estimatedOrbitalParameters[k].add(driver);
  167.                     orbitalParameterColumns.put(driver.getName(), columns++);
  168.                 }
  169.             }
  170.             orbitsEndColumns[k] = columns;
  171.         }

  173.         // Set estimated propagation parameters
  174.         allEstimatedPropagationParameters = new ParameterDriversList();
  175.         estimatedPropagationParameters    = new ParameterDriversList[builders.size()];
  176.         final List<String> estimatedPropagationParametersNames = new ArrayList<>();
  177.         for (int k = 0; k < builders.size(); ++k) {
  178.             estimatedPropagationParameters[k] = new ParameterDriversList();
  179.             for (final ParameterDriver driver : builders.get(k).getPropagationParametersDrivers().getDrivers()) {
  180.                 if (driver.getReferenceDate() == null) {
  181.                     driver.setReferenceDate(currentDate);
  182.                 }
  183.                 if (driver.isSelected()) {
  184.                     allEstimatedPropagationParameters.add(driver);
  185.                     estimatedPropagationParameters[k].add(driver);
  186.                     final String driverName = driver.getName();
  187.                     // Add the driver name if it has not been added yet
  188.                     if (!estimatedPropagationParametersNames.contains(driverName)) {
  189.                         estimatedPropagationParametersNames.add(driverName);
  190.                     }
  191.                 }
  192.             }
  193.         }
  194.         estimatedPropagationParametersNames.sort(Comparator.naturalOrder());

  196.         // Populate the map of propagation drivers' columns and update the total number of columns
  197.         propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
  198.         for (final String driverName : estimatedPropagationParametersNames) {
  199.             propagationParameterColumns.put(driverName, columns++);
  200.         }

  202.         // Populate the map of measurement drivers' columns and update the total number of columns
  203.         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
  204.             // Verify if the driver reference date has been set
  205.             if (parameter.getReferenceDate() == null) {
  206.                 parameter.setReferenceDate(currentDate);
  207.             }
  208.             measurementParameterColumns.put(parameter.getName(), columns);
  209.             columns++;
  210.         }

  212.         // set angle and orbit types
  213.         angleTypes = new PositionAngle[builders.size()];
  214.         orbitTypes = new OrbitType[builders.size()];
  215.         for (int k = 0; k < builders.size(); k++) {
  216.             angleTypes[k] = builders.get(k).getPositionAngle();
  217.             orbitTypes[k] = builders.get(k).getOrbitType();
  218.         }

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

  223.         for (int k = 0; k < covarianceIndirection.length; ++k) {
  224.             final ParameterDriversList orbitDrivers      = builders.get(k).getOrbitalParametersDrivers();
  225.             final ParameterDriversList parametersDrivers = builders.get(k).getPropagationParametersDrivers();
  226.             Arrays.fill(covarianceIndirection[k], -1);
  227.             int i = 0;
  228.             for (final ParameterDriver driver : orbitDrivers.getDrivers()) {
  229.                 final Integer c = orbitalParameterColumns.get(driver.getName());
  230.                 covarianceIndirection[k][i++] = (c == null) ? -1 : c.intValue();
  231.             }
  232.             for (final ParameterDriver driver : parametersDrivers.getDrivers()) {
  233.                 final Integer c = propagationParameterColumns.get(driver.getName());
  234.                 if (c != null) {
  235.                     covarianceIndirection[k][i++] = c.intValue();
  236.                 }
  237.             }
  238.             for (final ParameterDriver driver : estimatedMeasurementParameters.getDrivers()) {
  239.                 final Integer c = measurementParameterColumns.get(driver.getName());
  240.                 if (c != null) {
  241.                     covarianceIndirection[k][i++] = c.intValue();
  242.                 }
  243.             }
  244.         }

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

  249.         int p = 0;
  250.         for (final ParameterDriver driver : allEstimatedOrbitalParameters.getDrivers()) {
  251.             correctedState.setEntry(p++, driver.getValue());
  252.         }
  253.         for (final ParameterDriver driver : allEstimatedPropagationParameters.getDrivers()) {
  254.             correctedState.setEntry(p++, driver.getValue());
  255.         }
  256.         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
  257.             correctedState.setEntry(p++, driver.getValue());
  258.         }

  260.         this.predictedSpacecraftStates = new SpacecraftState[propagatorBuilders.size()];
  261.         for (int i = 0; i < propagatorBuilders.size(); i++) {
  262.             predictedSpacecraftStates[i] = propagatorBuilders.get(i).buildPropagator(propagatorBuilders.get(i).getSelectedNormalizedParameters()).getInitialState();
  263.         }
  264.         this.correctedSpacecraftStates = predictedSpacecraftStates.clone();

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

  269.         final RealMatrix correctedCovariance = MatrixUtils.createRealMatrix(columns, columns);
  270.         for (int k = 0; k < covarianceMatrixProviders.size(); k++) {
  271.             // Number of estimated dynamic parameters (orbital + propagation)
  272.             final int nbDyn  = orbitsEndColumns[k] - orbitsStartColumns[k] +
  273.                                estimatedPropagationParameters[k].getNbParams();
  274.             // Covariance matrix
  275.             final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
  276.             final RealMatrix noiseP = covarianceMatrixProviders.get(k).
  277.                                       getInitialCovarianceMatrix(correctedSpacecraftStates[k]);
  278.             noiseK.setSubMatrix(noiseP.getData(), 0, 0);
  279.             if (measurementProcessNoiseMatrix != null) {
  280.                 final RealMatrix noiseM = measurementProcessNoiseMatrix.
  281.                                           getInitialCovarianceMatrix(correctedSpacecraftStates[k]);
  282.                 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
  283.             }

  285.             KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
  286.                                                builders.get(k).getOrbitalParametersDrivers(),
  287.                                                builders.get(k).getPropagationParametersDrivers(),
  288.                                                estimatedMeasurementsParameters);

  290.             final int[] indK = covarianceIndirection[k];
  291.             for (int i = 0; i < indK.length; ++i) {
  292.                 if (indK[i] >= 0) {
  293.                     for (int j = 0; j < indK.length; ++j) {
  294.                         if (indK[j] >= 0) {
  295.                             correctedCovariance.setEntry(indK[i], indK[j], noiseK.getEntry(i, j));
  296.                         }
  297.                     }
  298.                 }
  299.             }
  300.         }

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

  305.     }

  307.     /** {@inheritDoc} */
  308.     @Override
  309.     public UnscentedEvolution getEvolution(final double previousTime, final RealVector[] sigmaPoints,
  310.                                            final MeasurementDecorator measurement) {

  312.         // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
  313.         final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
  314.         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
  315.             if (driver.getReferenceDate() == null) {
  316.                 driver.setReferenceDate(builders.get(0).getInitialOrbitDate());
  317.             }
  318.         }

  320.         // Increment measurement number
  321.         ++currentMeasurementNumber;

  323.         // Update the current date
  324.         currentDate = measurement.getObservedMeasurement().getDate();

  326.         // Initialize arrays of predicted states and measurements
  327.         final RealVector[] predictedStates       = new RealVector[sigmaPoints.length];
  328.         final RealVector[] predictedMeasurements = new RealVector[sigmaPoints.length];

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

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

  336.             // Sigma points for the current builder
  337.             final RealVector[] currentSigmaPoints = new ArrayRealVector[sigmaPoints.length];
  338.             for (int i = 0; i < sigmaPoints.length; i++) {
  339.                 currentSigmaPoints[i] = sigmaPoints[i].getSubVector(orbitsStartColumns[k], orbitsEndColumns[k] - orbitsStartColumns[k]);
  340.             }

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

  345.             // Loop on states
  346.             for (int i = 0; i < states.size(); ++i) {
  347.                 if (k == 0) {
  348.                     predictedStates[i] = new ArrayRealVector(sigmaPoints[i].getDimension());
  349.                 }
  350.                 // Current predicted state
  351.                 final SpacecraftState predicted = states.get(i);
  352.                 // First, convert the predicted state to an array
  353.                 final double[] predictedArray = new double[currentSigmaPoints[i].getDimension()];
  354.                 orbitTypes[k].mapOrbitToArray(predicted.getOrbit(), angleTypes[k], predictedArray, null);
  355.                 predictedStates[i].setSubVector(orbitsStartColumns[k], new ArrayRealVector(predictedArray));
  356.                 // One has the same information in predictedStates
  357.                 // and statesForMeasurementEstimation but differently arranged
  358.                 statesForMeasurementEstimation[i][k] = predicted;
  359.             }
  360.         }

  362.         // Loop on sigma points to predict measurements
  363.         for (int i = 0; i < sigmaPoints.length; i++) {
  364.             final EstimatedMeasurement<?> estimated = observedMeasurement.estimate(currentMeasurementNumber, currentMeasurementNumber,
  365.                                                                                    KalmanEstimatorUtil.filterRelevant(observedMeasurement,
  366.                                                                                                                       statesForMeasurementEstimation[i]));
  367.             predictedMeasurements[i] = new ArrayRealVector(estimated.getEstimatedValue());
  368.         }

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

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

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

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

  382.             // Covariance matrix
  383.             final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
  384.             final RealMatrix noiseP = covarianceMatrixProviders.get(k).
  385.                                       getProcessNoiseMatrix(correctedSpacecraftStates[k],
  386.                                                             predictedSpacecraftStates[k]);
  387.             noiseK.setSubMatrix(noiseP.getData(), 0, 0);
  388.             if (measurementProcessNoiseMatrix != null) {
  389.                 final RealMatrix noiseM = measurementProcessNoiseMatrix.
  390.                                           getProcessNoiseMatrix(correctedSpacecraftStates[k],
  391.                                                                 predictedSpacecraftStates[k]);
  392.                 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
  393.             }

  395.             KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
  396.                                                builders.get(k).getOrbitalParametersDrivers(),
  397.                                                builders.get(k).getPropagationParametersDrivers(),
  398.                                                estimatedMeasurementsParameters);

  400.             final int[] indK = covarianceIndirection[k];
  401.             for (int i = 0; i < indK.length; ++i) {
  402.                 if (indK[i] >= 0) {
  403.                     for (int j = 0; j < indK.length; ++j) {
  404.                         if (indK[j] >= 0) {
  405.                             processNoise.setEntry(indK[i], indK[j], noiseK.getEntry(i, j));
  406.                         }
  407.                     }
  408.                 }
  409.             }

  411.         }

  413.         // Update epoch
  414.         previousDate = currentDate;

  416.         // Return
  417.         return new UnscentedEvolution(measurement.getTime(), predictedStates, predictedMeasurements, processNoise);
  418.     }

  420.     /** {@inheritDoc} */
  421.     @Override
  422.     public RealVector getInnovation(final MeasurementDecorator measurement, final RealVector predictedMeas,
  423.                                     final RealVector predictedState, final RealMatrix innovationCovarianceMatrix) {

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

  428.             // Update predicted states
  429.             final double[] predictedStateArray = predictedState.getSubVector(orbitsStartColumns[k], orbitsEndColumns[k] - orbitsStartColumns[k]).toArray();
  430.             final Orbit predictedOrbit = orbitTypes[k].mapArrayToOrbit(predictedStateArray, null, angleTypes[k],
  431.                                                                        currentDate, builders.get(k).getMu(), builders.get(k).getFrame());
  432.             predictedSpacecraftStates[k] = new SpacecraftState(predictedOrbit);

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

  437.         }

  439.         // Predicted measurement
  440.         predictedMeasurement = measurement.getObservedMeasurement().estimate(currentMeasurementNumber, currentMeasurementNumber,
  441.                                                                              KalmanEstimatorUtil.filterRelevant(measurement.getObservedMeasurement(),
  442.                                                                                                                 predictedSpacecraftStates));
  443.         predictedMeasurement.setEstimatedValue(predictedMeas.toArray());

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

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

  451.     }

  453.     /**
  454.      * Predict the predicted states for the given sigma points.
  455.      * @param sigmaPoints current sigma points
  456.      * @param index the index corresponding to the satellite one is dealing with
  457.      * @return predicted state for the given sigma point
  458.      */
  459.     private List<SpacecraftState> predictStates(final RealVector[] sigmaPoints, final int index) {

  461.         // Loop on sigma points to create the propagator parallelizer
  462.         final List<Propagator> propagators = new ArrayList<>(sigmaPoints.length);
  463.         for (int k = 0; k < sigmaPoints.length; ++k) {
  464.             // Current sigma point
  465.             final double[] currentPoint = sigmaPoints[k].copy().toArray();
  466.             // Create the corresponding orbit propagator
  467.             final Propagator currentPropagator = createPropagator(currentPoint, index);
  468.             // Add it to the list of propagators
  469.             propagators.add(currentPropagator);
  470.         }

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

  477.         // Return
  478.         return states;

  480.     }

  482.     /**
  483.      * Create a propagator for the given sigma point.
  484.      * @param point input sigma point
  485.      * @param index the index corresponding to the satellite one is dealing with
  486.      * @return the corresponding orbit propagator
  487.      */
  488.     private Propagator createPropagator(final double[] point, final int index) {
  489.         // Create a new instance of the current propagator builder
  490.         final NumericalPropagatorBuilder copy = builders.get(index).copy();
  491.         // Convert the given sigma point to an orbit
  492.         final Orbit orbit = orbitTypes[index].mapArrayToOrbit(point, null, angleTypes[index], copy.getInitialOrbitDate(),
  493.                                                       copy.getMu(), copy.getFrame());
  494.         copy.resetOrbit(orbit);
  495.         // Create the propagator
  496.         final NumericalPropagator propagator = copy.buildPropagator(copy.getSelectedNormalizedParameters());
  497.         return propagator;
  498.     }

  500.     /** Finalize estimation.
  501.      * @param observedMeasurement measurement that has just been processed
  502.      * @param estimate corrected estimate
  503.      */
  504.     public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
  505.                                    final ProcessEstimate estimate) {

  507.         correctedEstimate = estimate;

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

  512.             // Update corrected state
  513.             final double[] correctedStateArray = estimate.getState().getSubVector(orbitsStartColumns[k], orbitsEndColumns[k] - orbitsStartColumns[k]).toArray();
  514.             final Orbit correctedOrbit = orbitTypes[k].mapArrayToOrbit(correctedStateArray, null, angleTypes[k],
  515.                                                                    currentDate, builders.get(k).getMu(), builders.get(k).getFrame());
  516.             correctedSpacecraftStates[k] = new SpacecraftState(correctedOrbit);

  518.             // Update the builder
  519.             builders.get(k).resetOrbit(correctedOrbit);

  521.         }

  523.         // Corrected measurement
  524.         correctedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
  525.                                                             currentMeasurementNumber,
  526.                                                             KalmanEstimatorUtil.filterRelevant(observedMeasurement,
  527.                                                                                                getCorrectedSpacecraftStates()));
  528.     }

  530.     /** Get the propagators estimated with the values set in the propagators builders.
  531.      * @return propagators based on the current values in the builder
  532.      */
  533.     public Propagator[] getEstimatedPropagators() {
  534.         // Return propagators built with current instantiation of the propagator builders
  535.         final Propagator[] propagators = new Propagator[builders.size()];
  536.         for (int k = 0; k < builders.size(); ++k) {
  537.             propagators[k] = builders.get(k).buildPropagator(builders.get(k).getSelectedNormalizedParameters());
  538.         }
  539.         return propagators;
  540.     }

  542.     /** Get the current corrected estimate.
  543.      * @return current corrected estimate
  544.      */
  545.     public ProcessEstimate getEstimate() {
  546.         return correctedEstimate;
  547.     }

  549.     /** {@inheritDoc} */
  550.     @Override
  551.     public ParameterDriversList getEstimatedOrbitalParameters() {
  552.         return allEstimatedOrbitalParameters;
  553.     }

  555.     /** {@inheritDoc} */
  556.     @Override
  557.     public ParameterDriversList getEstimatedPropagationParameters() {
  558.         return allEstimatedPropagationParameters;
  559.     }

  561.     /** {@inheritDoc} */
  562.     @Override
  563.     public ParameterDriversList getEstimatedMeasurementsParameters() {
  564.         return estimatedMeasurementsParameters;
  565.     }

  567.     /** {@inheritDoc} */
  568.     @Override
  569.     public SpacecraftState[] getPredictedSpacecraftStates() {
  570.         return predictedSpacecraftStates.clone();
  571.     }

  573.     /** {@inheritDoc} */
  574.     @Override
  575.     public SpacecraftState[] getCorrectedSpacecraftStates() {
  576.         return correctedSpacecraftStates.clone();
  577.     }

  579.     /** {@inheritDoc} */
  580.     @Override
  581.     public RealVector getPhysicalEstimatedState() {
  582.         // Method {@link ParameterDriver#getValue()} is used to get
  583.         // the physical values of the state.
  584.         // The scales'array is used to get the size of the state vector
  585.         final RealVector physicalEstimatedState = new ArrayRealVector(getEstimate().getState().getDimension());
  586.         int i = 0;
  587.         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
  588.             physicalEstimatedState.setEntry(i++, driver.getValue());
  589.         }
  590.         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
  591.             physicalEstimatedState.setEntry(i++, driver.getValue());
  592.         }
  593.         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
  594.             physicalEstimatedState.setEntry(i++, driver.getValue());
  595.         }

  597.         return physicalEstimatedState;
  598.     }

  600.     /** {@inheritDoc} */
  601.     @Override
  602.     public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
  603.         return correctedEstimate.getCovariance();
  604.     }


  607.     /** {@inheritDoc} */
  608.     @Override
  609.     public RealMatrix getPhysicalStateTransitionMatrix() {
  610.         return null;
  611.     }

  613.     /** {@inheritDoc} */
  614.     @Override
  615.     public RealMatrix getPhysicalMeasurementJacobian() {
  616.         return null;
  617.     }

  619.     /** {@inheritDoc} */
  620.     @Override
  621.     public RealMatrix getPhysicalInnovationCovarianceMatrix() {
  622.         return correctedEstimate.getInnovationCovariance();
  623.     }

  625.     /** {@inheritDoc} */
  626.     @Override
  627.     public RealMatrix getPhysicalKalmanGain() {
  628.         return correctedEstimate.getKalmanGain();
  629.     }

  631.     /** {@inheritDoc} */
  632.     @Override
  633.     public int getCurrentMeasurementNumber() {
  634.         return currentMeasurementNumber;
  635.     }

  637.     /** {@inheritDoc} */
  638.     @Override
  639.     public AbsoluteDate getCurrentDate() {
  640.         return currentDate;
  641.     }

  643.     /** {@inheritDoc} */
  644.     @Override
  645.     public EstimatedMeasurement<?> getPredictedMeasurement() {
  646.         return predictedMeasurement;
  647.     }

  649.     /** {@inheritDoc} */
  650.     @Override
  651.     public EstimatedMeasurement<?> getCorrectedMeasurement() {
  652.         return correctedMeasurement;
  653.     }

  655. }
Created with JaCoCo 0.8.8.202204050719