UnscentedKalmanModel.java

  1. /* Copyright 2002-2024 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.PositionAngleType;
  38. import org.orekit.propagation.Propagator;
  39. import org.orekit.propagation.PropagatorsParallelizer;
  40. import org.orekit.propagation.SpacecraftState;
  41. import org.orekit.propagation.conversion.PropagatorBuilder;
  42. import org.orekit.time.AbsoluteDate;
  43. import org.orekit.utils.ParameterDriver;
  44. import org.orekit.utils.ParameterDriversList;
  45. import org.orekit.utils.ParameterDriversList.DelegatingDriver;

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

  54.     /** Builders for propagators. */
  55.     private final List<PropagatorBuilder> builders;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  304.     }

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

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

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

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

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

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

  331.             // Sigma points for the current builder
  332.             final RealVector[] currentSigmaPoints = new ArrayRealVector[sigmaPoints.length];
  333.             for (int i = 0; i < sigmaPoints.length; i++) {
  334.                 currentSigmaPoints[i] = sigmaPoints[i].getSubVector(orbitsStartColumns[k], orbitsEndColumns[k] - orbitsStartColumns[k]);
  335.             }

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

  340.             // Loop on states
  341.             for (int i = 0; i < states.size(); ++i) {
  342.                 if (k == 0) {
  343.                     predictedStates[i] = new ArrayRealVector(sigmaPoints[i].getDimension());
  344.                 }
  345.                 // Current predicted state
  346.                 final SpacecraftState predicted = states.get(i);
  347.                 // First, convert the predicted state to an array
  348.                 final double[] predictedArray = new double[currentSigmaPoints[i].getDimension()];
  349.                 orbitTypes[k].mapOrbitToArray(predicted.getOrbit(), angleTypes[k], predictedArray, null);
  350.                 predictedStates[i].setSubVector(orbitsStartColumns[k], new ArrayRealVector(predictedArray));
  351.             }

  353.         }

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

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

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

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

  367.             // Covariance matrix
  368.             final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
  369.             final RealMatrix noiseP = covarianceMatrixProviders.get(k).
  370.                                       getProcessNoiseMatrix(correctedSpacecraftStates[k],
  371.                                                             predictedSpacecraftStates[k]);
  372.             noiseK.setSubMatrix(noiseP.getData(), 0, 0);
  373.             if (measurementProcessNoiseMatrix != null) {
  374.                 final RealMatrix noiseM = measurementProcessNoiseMatrix.
  375.                                           getProcessNoiseMatrix(correctedSpacecraftStates[k],
  376.                                                                 predictedSpacecraftStates[k]);
  377.                 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
  378.             }

  380.             KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
  381.                                                builders.get(k).getOrbitalParametersDrivers(),
  382.                                                builders.get(k).getPropagationParametersDrivers(),
  383.                                                estimatedMeasurementsParameters);

  385.             final int[] indK = covarianceIndirection[k];
  386.             for (int i = 0; i < indK.length; ++i) {
  387.                 if (indK[i] >= 0) {
  388.                     for (int j = 0; j < indK.length; ++j) {
  389.                         if (indK[j] >= 0) {
  390.                             processNoise.setEntry(indK[i], indK[j], noiseK.getEntry(i, j));
  391.                         }
  392.                     }
  393.                 }
  394.             }

  396.         }

  398.         // Update epoch
  399.         previousDate = currentDate;

  401.         // Return
  402.         return new UnscentedEvolution(measurement.getTime(), predictedStates, processNoise);
  403.     }

  405.     /** {@inheritDoc} */
  406.     @Override
  407.     public RealVector[] getPredictedMeasurements(final RealVector[] predictedSigmaPoints, final MeasurementDecorator measurement) {

  409.         // Observed measurement
  410.         final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();

  412.         // Initialize arrays of predicted states and measurements
  413.         final RealVector[] predictedMeasurements = new RealVector[predictedSigmaPoints.length];

  415.         // Initialize the relevant states used for measurement estimation
  416.         final SpacecraftState[][] statesForMeasurementEstimation = new SpacecraftState[predictedSigmaPoints.length][builders.size()];

  418.         // Convert sigma points to spacecraft states
  419.         for (int k = 0; k < builders.size(); k++ ) {

  421.             // Loop on sigma points
  422.             for (int i = 0; i < predictedSigmaPoints.length; i++) {
  423.                 final SpacecraftState state = new SpacecraftState(orbitTypes[k].mapArrayToOrbit(predictedSigmaPoints[i].toArray(), null,
  424.                                                                                                 angleTypes[k], currentDate, builders.get(k).getMu(),
  425.                                                                                                 builders.get(k).getFrame()));
  426.                 statesForMeasurementEstimation[i][k] = state;
  427.             }

  429.         }

  431.         // Loop on sigma points to predict measurements
  432.         for (int i = 0; i < predictedSigmaPoints.length; i++) {
  433.             final EstimatedMeasurement<?> estimated = observedMeasurement.estimate(currentMeasurementNumber, currentMeasurementNumber,
  434.                                                                                    KalmanEstimatorUtil.filterRelevant(observedMeasurement,
  435.                                                                                                                       statesForMeasurementEstimation[i]));
  436.             predictedMeasurements[i] = new ArrayRealVector(estimated.getEstimatedValue());
  437.         }

  439.         // Return the predicted measurements
  440.         return predictedMeasurements;

  442.     }

  444.     /** {@inheritDoc} */
  445.     @Override
  446.     public RealVector getInnovation(final MeasurementDecorator measurement, final RealVector predictedMeas,
  447.                                     final RealVector predictedState, final RealMatrix innovationCovarianceMatrix) {

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

  452.             // Update predicted states
  453.             final double[] predictedStateArray = predictedState.getSubVector(orbitsStartColumns[k], orbitsEndColumns[k] - orbitsStartColumns[k]).toArray();
  454.             final Orbit predictedOrbit = orbitTypes[k].mapArrayToOrbit(predictedStateArray, null, angleTypes[k],
  455.                                                                        currentDate, builders.get(k).getMu(), builders.get(k).getFrame());
  456.             predictedSpacecraftStates[k] = new SpacecraftState(predictedOrbit);

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

  461.         }

  463.         // Predicted measurement
  464.         predictedMeasurement = measurement.getObservedMeasurement().estimate(currentMeasurementNumber, currentMeasurementNumber,
  465.                                                                              KalmanEstimatorUtil.filterRelevant(measurement.getObservedMeasurement(),
  466.                                                                                                                 predictedSpacecraftStates));
  467.         predictedMeasurement.setEstimatedValue(predictedMeas.toArray());

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

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

  475.     }

  477.     /**
  478.      * Predict the predicted states for the given sigma points.
  479.      * @param sigmaPoints current sigma points
  480.      * @param index the index corresponding to the satellite one is dealing with
  481.      * @return predicted state for the given sigma point
  482.      */
  483.     private List<SpacecraftState> predictStates(final RealVector[] sigmaPoints, final int index) {

  485.         // Loop on sigma points to create the propagator parallelizer
  486.         final List<Propagator> propagators = new ArrayList<>(sigmaPoints.length);
  487.         for (int k = 0; k < sigmaPoints.length; ++k) {
  488.             // Current sigma point
  489.             final double[] currentPoint = sigmaPoints[k].copy().toArray();
  490.             // Create the corresponding orbit propagator
  491.             final Propagator currentPropagator = createPropagator(currentPoint, index);
  492.             // Add it to the list of propagators
  493.             propagators.add(currentPropagator);
  494.         }

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

  501.         // Return
  502.         return states;

  504.     }

  506.     /**
  507.      * Create a propagator for the given sigma point.
  508.      * @param point input sigma point
  509.      * @param index the index corresponding to the satellite one is dealing with
  510.      * @return the corresponding orbit propagator
  511.      */
  512.     private Propagator createPropagator(final double[] point, final int index) {
  513.         // Create a new instance of the current propagator builder
  514.         final PropagatorBuilder copy = builders.get(index).copy();
  515.         // Convert the given sigma point to an orbit
  516.         final Orbit orbit = orbitTypes[index].mapArrayToOrbit(point, null, angleTypes[index], copy.getInitialOrbitDate(),
  517.                                                       copy.getMu(), copy.getFrame());
  518.         copy.resetOrbit(orbit);
  519.         // Create the propagator
  520.         final Propagator propagator = copy.buildPropagator(copy.getSelectedNormalizedParameters());
  521.         return propagator;
  522.     }

  524.     /** Finalize estimation.
  525.      * @param observedMeasurement measurement that has just been processed
  526.      * @param estimate corrected estimate
  527.      */
  528.     public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
  529.                                    final ProcessEstimate estimate) {

  531.         correctedEstimate = estimate;

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

  536.             // Update corrected state
  537.             final double[] correctedStateArray = estimate.getState().getSubVector(orbitsStartColumns[k], orbitsEndColumns[k] - orbitsStartColumns[k]).toArray();
  538.             final Orbit correctedOrbit = orbitTypes[k].mapArrayToOrbit(correctedStateArray, null, angleTypes[k],
  539.                                                                    currentDate, builders.get(k).getMu(), builders.get(k).getFrame());
  540.             correctedSpacecraftStates[k] = new SpacecraftState(correctedOrbit);

  542.             // Update the builder
  543.             builders.get(k).resetOrbit(correctedOrbit);

  545.         }

  547.         // Corrected measurement
  548.         correctedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
  549.                                                             currentMeasurementNumber,
  550.                                                             KalmanEstimatorUtil.filterRelevant(observedMeasurement,
  551.                                                                                                getCorrectedSpacecraftStates()));
  552.     }

  554.     /** Get the propagators estimated with the values set in the propagators builders.
  555.      * @return propagators based on the current values in the builder
  556.      */
  557.     public Propagator[] getEstimatedPropagators() {
  558.         // Return propagators built with current instantiation of the propagator builders
  559.         final Propagator[] propagators = new Propagator[builders.size()];
  560.         for (int k = 0; k < builders.size(); ++k) {
  561.             propagators[k] = builders.get(k).buildPropagator(builders.get(k).getSelectedNormalizedParameters());
  562.         }
  563.         return propagators;
  564.     }

  566.     /** Get the current corrected estimate.
  567.      * @return current corrected estimate
  568.      */
  569.     public ProcessEstimate getEstimate() {
  570.         return correctedEstimate;
  571.     }

  573.     /** {@inheritDoc} */
  574.     @Override
  575.     public ParameterDriversList getEstimatedOrbitalParameters() {
  576.         return allEstimatedOrbitalParameters;
  577.     }

  579.     /** {@inheritDoc} */
  580.     @Override
  581.     public ParameterDriversList getEstimatedPropagationParameters() {
  582.         return allEstimatedPropagationParameters;
  583.     }

  585.     /** {@inheritDoc} */
  586.     @Override
  587.     public ParameterDriversList getEstimatedMeasurementsParameters() {
  588.         return estimatedMeasurementsParameters;
  589.     }

  591.     /** {@inheritDoc} */
  592.     @Override
  593.     public SpacecraftState[] getPredictedSpacecraftStates() {
  594.         return predictedSpacecraftStates.clone();
  595.     }

  597.     /** {@inheritDoc} */
  598.     @Override
  599.     public SpacecraftState[] getCorrectedSpacecraftStates() {
  600.         return correctedSpacecraftStates.clone();
  601.     }

  603.     /** {@inheritDoc} */
  604.     @Override
  605.     public RealVector getPhysicalEstimatedState() {
  606.         // Method {@link ParameterDriver#getValue()} is used to get
  607.         // the physical values of the state.
  608.         // The scales'array is used to get the size of the state vector
  609.         final RealVector physicalEstimatedState = new ArrayRealVector(getEstimate().getState().getDimension());
  610.         int i = 0;
  611.         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
  612.             physicalEstimatedState.setEntry(i++, driver.getValue());
  613.         }
  614.         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
  615.             physicalEstimatedState.setEntry(i++, driver.getValue());
  616.         }
  617.         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
  618.             physicalEstimatedState.setEntry(i++, driver.getValue());
  619.         }

  621.         return physicalEstimatedState;
  622.     }

  624.     /** {@inheritDoc} */
  625.     @Override
  626.     public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
  627.         return correctedEstimate.getCovariance();
  628.     }


  631.     /** {@inheritDoc} */
  632.     @Override
  633.     public RealMatrix getPhysicalStateTransitionMatrix() {
  634.         return null;
  635.     }

  637.     /** {@inheritDoc} */
  638.     @Override
  639.     public RealMatrix getPhysicalMeasurementJacobian() {
  640.         return null;
  641.     }

  643.     /** {@inheritDoc} */
  644.     @Override
  645.     public RealMatrix getPhysicalInnovationCovarianceMatrix() {
  646.         return correctedEstimate.getInnovationCovariance();
  647.     }

  649.     /** {@inheritDoc} */
  650.     @Override
  651.     public RealMatrix getPhysicalKalmanGain() {
  652.         return correctedEstimate.getKalmanGain();
  653.     }

  655.     /** {@inheritDoc} */
  656.     @Override
  657.     public int getCurrentMeasurementNumber() {
  658.         return currentMeasurementNumber;
  659.     }

  661.     /** {@inheritDoc} */
  662.     @Override
  663.     public AbsoluteDate getCurrentDate() {
  664.         return currentDate;
  665.     }

  667.     /** {@inheritDoc} */
  668.     @Override
  669.     public EstimatedMeasurement<?> getPredictedMeasurement() {
  670.         return predictedMeasurement;
  671.     }

  673.     /** {@inheritDoc} */
  674.     @Override
  675.     public EstimatedMeasurement<?> getCorrectedMeasurement() {
  676.         return correctedMeasurement;
  677.     }

  679. }
Created with JaCoCo 0.8.11.202310140853