SemiAnalyticalKalmanModel.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.Comparator;
  20. import java.util.HashMap;
  21. import java.util.List;
  22. import java.util.Map;

  23. import org.hipparchus.exception.MathRuntimeException;
  24. import org.hipparchus.filtering.kalman.ProcessEstimate;
  25. import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
  26. import org.hipparchus.filtering.kalman.extended.NonLinearEvolution;
  27. import org.hipparchus.filtering.kalman.extended.NonLinearProcess;
  28. import org.hipparchus.linear.Array2DRowRealMatrix;
  29. import org.hipparchus.linear.ArrayRealVector;
  30. import org.hipparchus.linear.MatrixUtils;
  31. import org.hipparchus.linear.QRDecomposition;
  32. import org.hipparchus.linear.RealMatrix;
  33. import org.hipparchus.linear.RealVector;
  34. import org.hipparchus.util.FastMath;
  35. import org.orekit.errors.OrekitException;
  36. import org.orekit.errors.OrekitMessages;
  37. import org.orekit.estimation.measurements.EstimatedMeasurement;
  38. import org.orekit.estimation.measurements.EstimationModifier;
  39. import org.orekit.estimation.measurements.ObservedMeasurement;
  40. import org.orekit.estimation.measurements.modifiers.DynamicOutlierFilter;
  41. import org.orekit.orbits.Orbit;
  42. import org.orekit.orbits.OrbitType;
  43. import org.orekit.propagation.PropagationType;
  44. import org.orekit.propagation.SpacecraftState;
  45. import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
  46. import org.orekit.propagation.semianalytical.dsst.DSSTHarvester;
  47. import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
  48. import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
  49. import org.orekit.propagation.semianalytical.dsst.forces.ShortPeriodTerms;
  50. import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
  51. import org.orekit.time.AbsoluteDate;
  52. import org.orekit.time.ChronologicalComparator;
  53. import org.orekit.utils.ParameterDriver;
  54. import org.orekit.utils.ParameterDriversList;
  55. import org.orekit.utils.ParameterDriversList.DelegatingDriver;

  56. /** Process model to use with a {@link SemiAnalyticalKalmanEstimator}.
  57.  *
  58.  * @see "Folcik Z., Orbit Determination Using Modern Filters/Smoothers and Continuous Thrust Modeling,
  59.  *       Master of Science Thesis, Department of Aeronautics and Astronautics, MIT, June, 2008."
  60.  *
  61.  * @see "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
  62.  *       Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
  63.  *       Specialist Conference, Big Sky, August 2021."
  64.  *
  65.  * @author Julie Bayard
  66.  * @author Bryan Cazabonne
  67.  * @author Maxime Journot
  68.  * @since 11.1
  69.  */
  70. public  class SemiAnalyticalKalmanModel implements KalmanEstimation, NonLinearProcess<MeasurementDecorator> {

  71.     /** Builders for DSST propagator. */
  72.     private final DSSTPropagatorBuilder builder;

  73.     /** Estimated orbital parameters. */
  74.     private final ParameterDriversList estimatedOrbitalParameters;

  75.     /** Per-builder estimated propagation drivers. */
  76.     private final ParameterDriversList estimatedPropagationParameters;

  77.     /** Estimated measurements parameters. */
  78.     private final ParameterDriversList estimatedMeasurementsParameters;

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

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

  83.     /** Scaling factors. */
  84.     private final double[] scale;

  85.     /** Provider for covariance matrix. */
  86.     private final CovarianceMatrixProvider covarianceMatrixProvider;

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

  89.     /** Harvester between two-dimensional Jacobian matrices and one-dimensional additional state arrays. */
  90.     private DSSTHarvester harvester;

  91.     /** Propagators for the reference trajectories, up to current date. */
  92.     private DSSTPropagator dsstPropagator;

  93.     /** Observer to retrieve current estimation info. */
  94.     private KalmanObserver observer;

  95.     /** Current number of measurement. */
  96.     private int currentMeasurementNumber;

  97.     /** Current date. */
  98.     private AbsoluteDate currentDate;

  99.     /** Predicted mean element filter correction. */
  100.     private RealVector predictedFilterCorrection;

  101.     /** Corrected mean element filter correction. */
  102.     private RealVector correctedFilterCorrection;

  103.     /** Predicted measurement. */
  104.     private EstimatedMeasurement<?> predictedMeasurement;

  105.     /** Corrected measurement. */
  106.     private EstimatedMeasurement<?> correctedMeasurement;

  107.     /** Nominal mean spacecraft state. */
  108.     private SpacecraftState nominalMeanSpacecraftState;

  109.     /** Previous nominal mean spacecraft state. */
  110.     private SpacecraftState previousNominalMeanSpacecraftState;

  111.     /** Current corrected estimate. */
  112.     private ProcessEstimate correctedEstimate;

  113.     /** Inverse of the orbital part of the state transition matrix. */
  114.     private RealMatrix phiS;

  115.     /** Propagation parameters part of the state transition matrix. */
  116.     private RealMatrix psiS;

  117.     /** Kalman process model constructor (package private).
  118.      * @param propagatorBuilder propagators builders used to evaluate the orbits.
  119.      * @param covarianceMatrixProvider provider for covariance matrix
  120.      * @param estimatedMeasurementParameters measurement parameters to estimate
  121.      * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
  122.      */
  123.     protected SemiAnalyticalKalmanModel(final DSSTPropagatorBuilder propagatorBuilder,
  124.                                         final CovarianceMatrixProvider covarianceMatrixProvider,
  125.                                         final ParameterDriversList estimatedMeasurementParameters,
  126.                                         final CovarianceMatrixProvider measurementProcessNoiseMatrix) {

  127.         this.builder                         = propagatorBuilder;
  128.         this.estimatedMeasurementsParameters = estimatedMeasurementParameters;
  129.         this.measurementParameterColumns     = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
  130.         this.observer                        = null;
  131.         this.currentMeasurementNumber        = 0;
  132.         this.currentDate                     = propagatorBuilder.getInitialOrbitDate();
  133.         this.covarianceMatrixProvider        = covarianceMatrixProvider;
  134.         this.measurementProcessNoiseMatrix   = measurementProcessNoiseMatrix;

  135.         // Number of estimated parameters
  136.         int columns = 0;

  137.         // Set estimated orbital parameters
  138.         estimatedOrbitalParameters = new ParameterDriversList();
  139.         for (final ParameterDriver driver : builder.getOrbitalParametersDrivers().getDrivers()) {

  140.             // Verify if the driver reference date has been set
  141.             if (driver.getReferenceDate() == null) {
  142.                 driver.setReferenceDate(currentDate);
  143.             }

  144.             // Verify if the driver is selected
  145.             if (driver.isSelected()) {
  146.                 estimatedOrbitalParameters.add(driver);
  147.                 columns++;
  148.             }

  149.         }

  150.         // Set estimated propagation parameters
  151.         estimatedPropagationParameters = new ParameterDriversList();
  152.         final List<String> estimatedPropagationParametersNames = new ArrayList<>();
  153.         for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {

  154.             // Verify if the driver reference date has been set
  155.             if (driver.getReferenceDate() == null) {
  156.                 driver.setReferenceDate(currentDate);
  157.             }

  158.             // Verify if the driver is selected
  159.             if (driver.isSelected()) {
  160.                 estimatedPropagationParameters.add(driver);
  161.                 final String driverName = driver.getName();
  162.                 // Add the driver name if it has not been added yet
  163.                 if (!estimatedPropagationParametersNames.contains(driverName)) {
  164.                     estimatedPropagationParametersNames.add(driverName);
  165.                 }
  166.             }

  167.         }
  168.         estimatedPropagationParametersNames.sort(Comparator.naturalOrder());

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

  175.         // Set the estimated measurement parameters
  176.         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
  177.             if (parameter.getReferenceDate() == null) {
  178.                 parameter.setReferenceDate(currentDate);
  179.             }
  180.             measurementParameterColumns.put(parameter.getName(), columns);
  181.             ++columns;
  182.         }

  183.         // Compute the scale factors
  184.         this.scale = new double[columns];
  185.         int index = 0;
  186.         for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
  187.             scale[index++] = driver.getScale();
  188.         }
  189.         for (final ParameterDriver driver : estimatedPropagationParameters.getDrivers()) {
  190.             scale[index++] = driver.getScale();
  191.         }
  192.         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
  193.             scale[index++] = driver.getScale();
  194.         }

  195.         // Build the reference propagator and add its partial derivatives equations implementation
  196.         updateReferenceTrajectory(getEstimatedPropagator());
  197.         this.nominalMeanSpacecraftState = dsstPropagator.getInitialState();
  198.         this.previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;

  199.         // Initialize "field" short periodic terms
  200.         harvester.initializeFieldShortPeriodTerms(nominalMeanSpacecraftState);

  201.         // Initialize the estimated normalized mean element filter correction (See Ref [1], Eq. 3.2a)
  202.         this.predictedFilterCorrection = MatrixUtils.createRealVector(columns);
  203.         this.correctedFilterCorrection = predictedFilterCorrection;

  204.         // Initialize propagation parameters part of the state transition matrix (See Ref [1], Eq. 3.2c)
  205.         this.psiS = null;
  206.         if (estimatedPropagationParameters.getNbParams() != 0) {
  207.             this.psiS = MatrixUtils.createRealMatrix(getNumberSelectedOrbitalDrivers(),
  208.                                                      getNumberSelectedPropagationDrivers());
  209.         }

  210.         // Initialize inverse of the orbital part of the state transition matrix (See Ref [1], Eq. 3.2d)
  211.         this.phiS = MatrixUtils.createRealIdentityMatrix(getNumberSelectedOrbitalDrivers());

  212.         // Number of estimated measurement parameters
  213.         final int nbMeas = getNumberSelectedMeasurementDrivers();

  214.         // Number of estimated dynamic parameters (orbital + propagation)
  215.         final int nbDyn  = getNumberSelectedOrbitalDrivers() + getNumberSelectedPropagationDrivers();

  216.         // Covariance matrix
  217.         final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
  218.         final RealMatrix noiseP = covarianceMatrixProvider.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
  219.         noiseK.setSubMatrix(noiseP.getData(), 0, 0);
  220.         if (measurementProcessNoiseMatrix != null) {
  221.             final RealMatrix noiseM = measurementProcessNoiseMatrix.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
  222.             noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
  223.         }

  224.         // Verify dimension
  225.         checkDimension(noiseK.getRowDimension(),
  226.                        builder.getOrbitalParametersDrivers(),
  227.                        builder.getPropagationParametersDrivers(),
  228.                        estimatedMeasurementsParameters);

  229.         final RealMatrix correctedCovariance = normalizeCovarianceMatrix(noiseK);

  230.         // Initialize corrected estimate
  231.         this.correctedEstimate = new ProcessEstimate(0.0, correctedFilterCorrection, correctedCovariance);

  232.     }

  233.     /** Get the observer for Kalman Filter estimations.
  234.      * @return the observer for Kalman Filter estimations
  235.      */
  236.     public KalmanObserver getObserver() {
  237.         return observer;
  238.     }

  239.     /** Set the observer.
  240.      * @param observer the observer
  241.      */
  242.     public void setObserver(final KalmanObserver observer) {
  243.         this.observer = observer;
  244.     }

  245.     /** Get the current corrected estimate.
  246.      * @return current corrected estimate
  247.      */
  248.     public ProcessEstimate getEstimate() {
  249.         return correctedEstimate;
  250.     }

  251.     /** Process a single measurement.
  252.      * <p>
  253.      * Update the filter with the new measurements.
  254.      * </p>
  255.      * @param observedMeasurements the list of measurements to process
  256.      * @param filter Extended Kalman Filter
  257.      * @return estimated propagator
  258.      */
  259.     public DSSTPropagator processMeasurements(final List<ObservedMeasurement<?>> observedMeasurements,
  260.                                               final ExtendedKalmanFilter<MeasurementDecorator> filter) {
  261.         try {

  262.             // Sort the measurement
  263.             observedMeasurements.sort(new ChronologicalComparator());

  264.             // Initialize step handler and set it to the propagator
  265.             final EskfMeasurementHandler stepHandler = new EskfMeasurementHandler(this, filter, observedMeasurements, builder.getInitialOrbitDate());
  266.             dsstPropagator.getMultiplexer().add(stepHandler);
  267.             dsstPropagator.propagate(observedMeasurements.get(0).getDate(), observedMeasurements.get(observedMeasurements.size() - 1).getDate());

  268.             // Return the last estimated propagator
  269.             return getEstimatedPropagator();

  270.         } catch (MathRuntimeException mrte) {
  271.             throw new OrekitException(mrte);
  272.         }
  273.     }

  274.     /** Get the propagator estimated with the values set in the propagator builder.
  275.      * @return propagator based on the current values in the builder
  276.      */
  277.     public DSSTPropagator getEstimatedPropagator() {
  278.         // Return propagator built with current instantiation of the propagator builder
  279.         return builder.buildPropagator(builder.getSelectedNormalizedParameters());
  280.     }

  281.     /** {@inheritDoc} */
  282.     @Override
  283.     public NonLinearEvolution getEvolution(final double previousTime, final RealVector previousState,
  284.                                            final MeasurementDecorator measurement) {

  285.         // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
  286.         final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
  287.         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
  288.             if (driver.getReferenceDate() == null) {
  289.                 driver.setReferenceDate(builder.getInitialOrbitDate());
  290.             }
  291.         }

  292.         // Increment measurement number
  293.         ++currentMeasurementNumber;

  294.         // Update the current date
  295.         currentDate = measurement.getObservedMeasurement().getDate();

  296.         // Normalized state transition matrix
  297.         final RealMatrix stm = getErrorStateTransitionMatrix();

  298.         // Predict filter correction
  299.         predictedFilterCorrection = predictFilterCorrection(stm);

  300.         // Short period term derivatives
  301.         analyticalDerivativeComputations(nominalMeanSpacecraftState);

  302.         // Calculate the predicted osculating elements
  303.         final double[] osculating = computeOsculatingElements(predictedFilterCorrection);
  304.         final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, builder.getPositionAngle(),
  305.                                                                             currentDate, nominalMeanSpacecraftState.getMu(),
  306.                                                                             nominalMeanSpacecraftState.getFrame());

  307.         // Compute the predicted measurements  (See Ref [1], Eq. 3.8)
  308.         predictedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
  309.                                                             currentMeasurementNumber,
  310.                                                             new SpacecraftState[] {
  311.                                                                 new SpacecraftState(osculatingOrbit,
  312.                                                                                     nominalMeanSpacecraftState.getAttitude(),
  313.                                                                                     nominalMeanSpacecraftState.getMass(),
  314.                                                                                     nominalMeanSpacecraftState.getAdditionalStatesValues(),
  315.                                                                                     nominalMeanSpacecraftState.getAdditionalStatesDerivatives())
  316.                                                             });

  317.         // Normalized measurement matrix
  318.         final RealMatrix measurementMatrix = getMeasurementMatrix();

  319.         // Number of estimated measurement parameters
  320.         final int nbMeas = getNumberSelectedMeasurementDrivers();

  321.         // Number of estimated dynamic parameters (orbital + propagation)
  322.         final int nbDyn  = getNumberSelectedOrbitalDrivers() + getNumberSelectedPropagationDrivers();

  323.         // Covariance matrix
  324.         final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
  325.         final RealMatrix noiseP = covarianceMatrixProvider.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
  326.         noiseK.setSubMatrix(noiseP.getData(), 0, 0);
  327.         if (measurementProcessNoiseMatrix != null) {
  328.             final RealMatrix noiseM = measurementProcessNoiseMatrix.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
  329.             noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
  330.         }

  331.         // Verify dimension
  332.         checkDimension(noiseK.getRowDimension(),
  333.                        builder.getOrbitalParametersDrivers(),
  334.                        builder.getPropagationParametersDrivers(),
  335.                        estimatedMeasurementsParameters);

  336.         final RealMatrix normalizedProcessNoise = normalizeCovarianceMatrix(noiseK);

  337.         // Return
  338.         return new NonLinearEvolution(measurement.getTime(), predictedFilterCorrection, stm,
  339.                                       normalizedProcessNoise, measurementMatrix);
  340.     }

  341.     /** {@inheritDoc} */
  342.     @Override
  343.     public RealVector getInnovation(final MeasurementDecorator measurement, final NonLinearEvolution evolution,
  344.                                     final RealMatrix innovationCovarianceMatrix) {

  345.         // Apply the dynamic outlier filter, if it exists
  346.         applyDynamicOutlierFilter(predictedMeasurement, innovationCovarianceMatrix);
  347.         if (predictedMeasurement.getStatus() == EstimatedMeasurement.Status.REJECTED)  {
  348.             // set innovation to null to notify filter measurement is rejected
  349.             return null;
  350.         } else {
  351.             // Normalized innovation of the measurement (Nx1)
  352.             final double[] observed  = predictedMeasurement.getObservedMeasurement().getObservedValue();
  353.             final double[] estimated = predictedMeasurement.getEstimatedValue();
  354.             final double[] sigma     = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();
  355.             final double[] residuals = new double[observed.length];

  356.             for (int i = 0; i < observed.length; i++) {
  357.                 residuals[i] = (observed[i] - estimated[i]) / sigma[i];
  358.             }
  359.             return MatrixUtils.createRealVector(residuals);
  360.         }

  361.     }

  362.     /** Finalize estimation.
  363.      * @param observedMeasurement measurement that has just been processed
  364.      * @param estimate corrected estimate
  365.      */
  366.     public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
  367.                                    final ProcessEstimate estimate) {
  368.         // Update the process estimate
  369.         correctedEstimate = estimate;
  370.         // Corrected filter correction
  371.         correctedFilterCorrection = estimate.getState();
  372.         // Update the previous nominal mean spacecraft state
  373.         previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;
  374.         // Calculate the corrected osculating elements
  375.         final double[] osculating = computeOsculatingElements(correctedFilterCorrection);
  376.         final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, builder.getPositionAngle(),
  377.                                                                             currentDate, nominalMeanSpacecraftState.getMu(),
  378.                                                                             nominalMeanSpacecraftState.getFrame());

  379.         // Compute the corrected measurements
  380.         correctedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
  381.                                                             currentMeasurementNumber,
  382.                                                             new SpacecraftState[] {
  383.                                                                 new SpacecraftState(osculatingOrbit,
  384.                                                                                     nominalMeanSpacecraftState.getAttitude(),
  385.                                                                                     nominalMeanSpacecraftState.getMass(),
  386.                                                                                     nominalMeanSpacecraftState.getAdditionalStatesValues(),
  387.                                                                                     nominalMeanSpacecraftState.getAdditionalStatesDerivatives())
  388.                                                             });
  389.     }

  390.     /** Finalize estimation operations on the observation grid. */
  391.     public void finalizeOperationsObservationGrid() {
  392.         // Update parameters
  393.         updateParameters();
  394.     }

  395.     /** {@inheritDoc} */
  396.     @Override
  397.     public ParameterDriversList getEstimatedOrbitalParameters() {
  398.         return estimatedOrbitalParameters;
  399.     }

  400.     /** {@inheritDoc} */
  401.     @Override
  402.     public ParameterDriversList getEstimatedPropagationParameters() {
  403.         return estimatedPropagationParameters;
  404.     }

  405.     /** {@inheritDoc} */
  406.     @Override
  407.     public ParameterDriversList getEstimatedMeasurementsParameters() {
  408.         return estimatedMeasurementsParameters;
  409.     }

  410.     /** {@inheritDoc} */
  411.     @Override
  412.     public SpacecraftState[] getPredictedSpacecraftStates() {
  413.         return new SpacecraftState[] {nominalMeanSpacecraftState};
  414.     }

  415.     /** {@inheritDoc} */
  416.     @Override
  417.     public SpacecraftState[] getCorrectedSpacecraftStates() {
  418.         return new SpacecraftState[] {getEstimatedPropagator().getInitialState()};
  419.     }

  420.     /** {@inheritDoc} */
  421.     @Override
  422.     public RealVector getPhysicalEstimatedState() {
  423.         // Method {@link ParameterDriver#getValue()} is used to get
  424.         // the physical values of the state.
  425.         // The scales'array is used to get the size of the state vector
  426.         final RealVector physicalEstimatedState = new ArrayRealVector(scale.length);
  427.         int i = 0;
  428.         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
  429.             physicalEstimatedState.setEntry(i++, driver.getValue());
  430.         }
  431.         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
  432.             physicalEstimatedState.setEntry(i++, driver.getValue());
  433.         }
  434.         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
  435.             physicalEstimatedState.setEntry(i++, driver.getValue());
  436.         }

  437.         return physicalEstimatedState;
  438.     }

  439.     /** {@inheritDoc} */
  440.     @Override
  441.     public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
  442.         // Un-normalize the estimated covariance matrix (P) from Hipparchus and return it.
  443.         // The covariance P is an mxm matrix where m = nbOrb + nbPropag + nbMeas
  444.         // For each element [i,j] of P the corresponding normalized value is:
  445.         // Pn[i,j] = P[i,j] / (scale[i]*scale[j])
  446.         // Consequently: P[i,j] = Pn[i,j] * scale[i] * scale[j]

  447.         // Normalized covariance matrix
  448.         final RealMatrix normalizedP = correctedEstimate.getCovariance();

  449.         // Initialize physical covariance matrix
  450.         final int nbParams = normalizedP.getRowDimension();
  451.         final RealMatrix physicalP = MatrixUtils.createRealMatrix(nbParams, nbParams);

  452.         // Un-normalize the covairance matrix
  453.         for (int i = 0; i < nbParams; ++i) {
  454.             for (int j = 0; j < nbParams; ++j) {
  455.                 physicalP.setEntry(i, j, normalizedP.getEntry(i, j) * scale[i] * scale[j]);
  456.             }
  457.         }
  458.         return physicalP;
  459.     }

  460.     /** {@inheritDoc} */
  461.     @Override
  462.     public RealMatrix getPhysicalStateTransitionMatrix() {
  463.         //  Un-normalize the state transition matrix (ฯ†) from Hipparchus and return it.
  464.         // ฯ† is an mxm matrix where m = nbOrb + nbPropag + nbMeas
  465.         // For each element [i,j] of normalized ฯ† (ฯ†n), the corresponding physical value is:
  466.         // ฯ†[i,j] = ฯ†n[i,j] * scale[i] / scale[j]

  467.         // Normalized matrix
  468.         final RealMatrix normalizedSTM = correctedEstimate.getStateTransitionMatrix();

  469.         if (normalizedSTM == null) {
  470.             return null;
  471.         } else {
  472.             // Initialize physical matrix
  473.             final int nbParams = normalizedSTM.getRowDimension();
  474.             final RealMatrix physicalSTM = MatrixUtils.createRealMatrix(nbParams, nbParams);

  475.             // Un-normalize the matrix
  476.             for (int i = 0; i < nbParams; ++i) {
  477.                 for (int j = 0; j < nbParams; ++j) {
  478.                     physicalSTM.setEntry(i, j,
  479.                                          normalizedSTM.getEntry(i, j) * scale[i] / scale[j]);
  480.                 }
  481.             }
  482.             return physicalSTM;
  483.         }
  484.     }

  485.     /** {@inheritDoc} */
  486.     @Override
  487.     public RealMatrix getPhysicalMeasurementJacobian() {
  488.         // Un-normalize the measurement matrix (H) from Hipparchus and return it.
  489.         // H is an nxm matrix where:
  490.         //  - m = nbOrb + nbPropag + nbMeas is the number of estimated parameters
  491.         //  - n is the size of the measurement being processed by the filter
  492.         // For each element [i,j] of normalized H (Hn) the corresponding physical value is:
  493.         // H[i,j] = Hn[i,j] * ฯƒ[i] / scale[j]

  494.         // Normalized matrix
  495.         final RealMatrix normalizedH = correctedEstimate.getMeasurementJacobian();

  496.         if (normalizedH == null) {
  497.             return null;
  498.         } else {
  499.             // Get current measurement sigmas
  500.             final double[] sigmas = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();

  501.             // Initialize physical matrix
  502.             final int nbLine = normalizedH.getRowDimension();
  503.             final int nbCol  = normalizedH.getColumnDimension();
  504.             final RealMatrix physicalH = MatrixUtils.createRealMatrix(nbLine, nbCol);

  505.             // Un-normalize the matrix
  506.             for (int i = 0; i < nbLine; ++i) {
  507.                 for (int j = 0; j < nbCol; ++j) {
  508.                     physicalH.setEntry(i, j, normalizedH.getEntry(i, j) * sigmas[i] / scale[j]);
  509.                 }
  510.             }
  511.             return physicalH;
  512.         }
  513.     }

  514.     /** {@inheritDoc} */
  515.     @Override
  516.     public RealMatrix getPhysicalInnovationCovarianceMatrix() {
  517.         // Un-normalize the innovation covariance matrix (S) from Hipparchus and return it.
  518.         // S is an nxn matrix where n is the size of the measurement being processed by the filter
  519.         // For each element [i,j] of normalized S (Sn) the corresponding physical value is:
  520.         // S[i,j] = Sn[i,j] * ฯƒ[i] * ฯƒ[j]

  521.         // Normalized matrix
  522.         final RealMatrix normalizedS = correctedEstimate.getInnovationCovariance();

  523.         if (normalizedS == null) {
  524.             return null;
  525.         } else {
  526.             // Get current measurement sigmas
  527.             final double[] sigmas = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();

  528.             // Initialize physical matrix
  529.             final int nbMeas = sigmas.length;
  530.             final RealMatrix physicalS = MatrixUtils.createRealMatrix(nbMeas, nbMeas);

  531.             // Un-normalize the matrix
  532.             for (int i = 0; i < nbMeas; ++i) {
  533.                 for (int j = 0; j < nbMeas; ++j) {
  534.                     physicalS.setEntry(i, j, normalizedS.getEntry(i, j) * sigmas[i] *   sigmas[j]);
  535.                 }
  536.             }
  537.             return physicalS;
  538.         }
  539.     }

  540.     /** {@inheritDoc} */
  541.     @Override
  542.     public RealMatrix getPhysicalKalmanGain() {
  543.         // Un-normalize the Kalman gain (K) from Hipparchus and return it.
  544.         // K is an mxn matrix where:
  545.         //  - m = nbOrb + nbPropag + nbMeas is the number of estimated parameters
  546.         //  - n is the size of the measurement being processed by the filter
  547.         // For each element [i,j] of normalized K (Kn) the corresponding physical value is:
  548.         // K[i,j] = Kn[i,j] * scale[i] / ฯƒ[j]

  549.         // Normalized matrix
  550.         final RealMatrix normalizedK = correctedEstimate.getKalmanGain();

  551.         if (normalizedK == null) {
  552.             return null;
  553.         } else {
  554.             // Get current measurement sigmas
  555.             final double[] sigmas = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();

  556.             // Initialize physical matrix
  557.             final int nbLine = normalizedK.getRowDimension();
  558.             final int nbCol  = normalizedK.getColumnDimension();
  559.             final RealMatrix physicalK = MatrixUtils.createRealMatrix(nbLine, nbCol);

  560.             // Un-normalize the matrix
  561.             for (int i = 0; i < nbLine; ++i) {
  562.                 for (int j = 0; j < nbCol; ++j) {
  563.                     physicalK.setEntry(i, j, normalizedK.getEntry(i, j) * scale[i] / sigmas[j]);
  564.                 }
  565.             }
  566.             return physicalK;
  567.         }
  568.     }

  569.     /** {@inheritDoc} */
  570.     @Override
  571.     public int getCurrentMeasurementNumber() {
  572.         return currentMeasurementNumber;
  573.     }

  574.     /** {@inheritDoc} */
  575.     @Override
  576.     public AbsoluteDate getCurrentDate() {
  577.         return currentDate;
  578.     }

  579.     /** {@inheritDoc} */
  580.     @Override
  581.     public EstimatedMeasurement<?> getPredictedMeasurement() {
  582.         return predictedMeasurement;
  583.     }

  584.     /** {@inheritDoc} */
  585.     @Override
  586.     public EstimatedMeasurement<?> getCorrectedMeasurement() {
  587.         return correctedMeasurement;
  588.     }

  589.     /** Update the nominal spacecraft state.
  590.      * @param nominal nominal spacecraft state
  591.      */
  592.     public void updateNominalSpacecraftState(final SpacecraftState nominal) {
  593.         this.nominalMeanSpacecraftState = nominal;
  594.         // Update the builder with the nominal mean elements orbit
  595.         builder.resetOrbit(nominal.getOrbit(), PropagationType.MEAN);
  596.     }

  597.     /** Update the reference trajectories using the propagator as input.
  598.      * @param propagator The new propagator to use
  599.      */
  600.     public void updateReferenceTrajectory(final DSSTPropagator propagator) {

  601.         dsstPropagator = propagator;

  602.         // Equation name
  603.         final String equationName = SemiAnalyticalKalmanEstimator.class.getName() + "-derivatives-";

  604.         // Mean state
  605.         final SpacecraftState meanState = dsstPropagator.initialIsOsculating() ?
  606.                        DSSTPropagator.computeMeanState(dsstPropagator.getInitialState(), dsstPropagator.getAttitudeProvider(), dsstPropagator.getAllForceModels()) :
  607.                        dsstPropagator.getInitialState();

  608.         // Update the jacobian harvester
  609.         dsstPropagator.setInitialState(meanState, PropagationType.MEAN);
  610.         harvester = (DSSTHarvester) dsstPropagator.setupMatricesComputation(equationName, null, null);

  611.     }

  612.     /** Update the DSST short periodic terms.
  613.      * @param state current mean state
  614.      */
  615.     public void updateShortPeriods(final SpacecraftState state) {
  616.         // Loop on DSST force models
  617.         for (final DSSTForceModel model : builder.getAllForceModels()) {
  618.             model.updateShortPeriodTerms(model.getParameters(), state);
  619.         }
  620.         harvester.updateFieldShortPeriodTerms(state);
  621.     }

  622.     /** Initialize the short periodic terms for the Kalman Filter.
  623.      * @param meanState mean state for auxiliary elements
  624.      */
  625.     public void initializeShortPeriodicTerms(final SpacecraftState meanState) {
  626.         final List<ShortPeriodTerms> shortPeriodTerms = new ArrayList<ShortPeriodTerms>();
  627.         for (final DSSTForceModel force :  builder.getAllForceModels()) {
  628.             shortPeriodTerms.addAll(force.initializeShortPeriodTerms(new AuxiliaryElements(meanState.getOrbit(), 1), PropagationType.OSCULATING, force.getParameters()));
  629.         }
  630.         dsstPropagator.setShortPeriodTerms(shortPeriodTerms);
  631.     }

  632.     /** Get the normalized state transition matrix (STM) from previous point to current point.
  633.      * The STM contains the partial derivatives of current state with respect to previous state.
  634.      * The  STM is an mxm matrix where m is the size of the state vector.
  635.      * m = nbOrb + nbPropag + nbMeas
  636.      * @return the normalized error state transition matrix
  637.      */
  638.     private RealMatrix getErrorStateTransitionMatrix() {

  639.         /* The state transition matrix is obtained as follows, with:
  640.          *  - Phi(k, k-1) : Transitional orbital matrix
  641.          *  - Psi(k, k-1) : Transitional propagation parameters matrix
  642.          *
  643.          *       |             |             |   .    |
  644.          *       | Phi(k, k-1) | Psi(k, k-1) | ..0..  |
  645.          *       |             |             |   .    |
  646.          *       |-------------|-------------|--------|
  647.          *       |      .      |    1 0 0    |   .    |
  648.          * STM = |    ..0..    |    0 1 0    | ..0..  |
  649.          *       |      .      |    0 0 1    |   .    |
  650.          *       |-------------|-------------|--------|
  651.          *       |      .      |      .      | 1 0 0..|
  652.          *       |    ..0..    |    ..0..    | 0 1 0..|
  653.          *       |      .      |      .      | 0 0 1..|
  654.          */

  655.         // Initialize to the proper size identity matrix
  656.         final RealMatrix stm = MatrixUtils.createRealIdentityMatrix(correctedEstimate.getState().getDimension());

  657.         // Derivatives of the state vector with respect to initial state vector
  658.         final int nbOrb = getNumberSelectedOrbitalDrivers();
  659.         final RealMatrix dYdY0 = harvester.getB2(nominalMeanSpacecraftState);

  660.         // Calculate transitional orbital matrix (See Ref [1], Eq. 3.4a)
  661.         final RealMatrix phi = dYdY0.multiply(phiS);

  662.         // Fill the state transition matrix with the orbital drivers
  663.         final List<DelegatingDriver> drivers = builder.getOrbitalParametersDrivers().getDrivers();
  664.         for (int i = 0; i < nbOrb; ++i) {
  665.             if (drivers.get(i).isSelected()) {
  666.                 int jOrb = 0;
  667.                 for (int j = 0; j < nbOrb; ++j) {
  668.                     if (drivers.get(j).isSelected()) {
  669.                         stm.setEntry(i, jOrb++, phi.getEntry(i, j));
  670.                     }
  671.                 }
  672.             }
  673.         }

  674.         // Update PhiS
  675.         phiS = new QRDecomposition(dYdY0).getSolver().getInverse();

  676.         // Derivatives of the state vector with respect to propagation parameters
  677.         if (psiS != null) {

  678.             final int nbProp = getNumberSelectedPropagationDrivers();
  679.             final RealMatrix dYdPp = harvester.getB3(nominalMeanSpacecraftState);

  680.             // Calculate transitional parameters matrix (See Ref [1], Eq. 3.4b)
  681.             final RealMatrix psi = dYdPp.subtract(phi.multiply(psiS));

  682.             // Fill 1st row, 2nd column (dY/dPp)
  683.             for (int i = 0; i < nbOrb; ++i) {
  684.                 for (int j = 0; j < nbProp; ++j) {
  685.                     stm.setEntry(i, j + nbOrb, psi.getEntry(i, j));
  686.                 }
  687.             }

  688.             // Update PsiS
  689.             psiS = dYdPp;

  690.         }

  691.         // Normalization of the STM
  692.         // normalized(STM)ij = STMij*Sj/Si
  693.         for (int i = 0; i < scale.length; i++) {
  694.             for (int j = 0; j < scale.length; j++ ) {
  695.                 stm.setEntry(i, j, stm.getEntry(i, j) * scale[j] / scale[i]);
  696.             }
  697.         }

  698.         // Return the error state transition matrix
  699.         return stm;

  700.     }

  701.     /** Get the normalized measurement matrix H.
  702.      * H contains the partial derivatives of the measurement with respect to the state.
  703.      * H is an nxm matrix where n is the size of the measurement vector and m the size of the state vector.
  704.      * @return the normalized measurement matrix H
  705.      */
  706.     private RealMatrix getMeasurementMatrix() {

  707.         // Observed measurement characteristics
  708.         final SpacecraftState        evaluationState     = predictedMeasurement.getStates()[0];
  709.         final ObservedMeasurement<?> observedMeasurement = predictedMeasurement.getObservedMeasurement();
  710.         final double[] sigma  = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();

  711.         // Initialize measurement matrix H: nxm
  712.         // n: Number of measurements in current measurement
  713.         // m: State vector size
  714.         final RealMatrix measurementMatrix = MatrixUtils.
  715.                 createRealMatrix(observedMeasurement.getDimension(),
  716.                                  correctedEstimate.getState().getDimension());

  717.         // Predicted orbit
  718.         final Orbit predictedOrbit = evaluationState.getOrbit();

  719.         // Measurement matrix's columns related to orbital and propagation parameters
  720.         // ----------------------------------------------------------

  721.         // Partial derivatives of the current Cartesian coordinates with respect to current orbital state
  722.         final int nbOrb  = getNumberSelectedOrbitalDrivers();
  723.         final int nbProp = getNumberSelectedPropagationDrivers();
  724.         final double[][] aCY = new double[nbOrb][nbOrb];
  725.         predictedOrbit.getJacobianWrtParameters(builder.getPositionAngle(), aCY);
  726.         final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);

  727.         // Jacobian of the measurement with respect to current Cartesian coordinates
  728.         final RealMatrix dMdC = new Array2DRowRealMatrix(predictedMeasurement.getStateDerivatives(0), false);

  729.         // Jacobian of the measurement with respect to current orbital state
  730.         RealMatrix dMdY = dMdC.multiply(dCdY);

  731.         // Compute factor dShortPeriod_dMeanState = I+B1 | B4
  732.         final RealMatrix IpB1B4 = MatrixUtils.createRealMatrix(nbOrb, nbOrb + nbProp);

  733.         // B1
  734.         final RealMatrix B1 = harvester.getB1();

  735.         // I + B1
  736.         final RealMatrix I = MatrixUtils.createRealIdentityMatrix(nbOrb);
  737.         final RealMatrix IpB1 = I.add(B1);
  738.         IpB1B4.setSubMatrix(IpB1.getData(), 0, 0);

  739.         // If there are not propagation parameters, B4 is null
  740.         if (psiS != null) {
  741.             final RealMatrix B4 = harvester.getB4();
  742.             IpB1B4.setSubMatrix(B4.getData(), 0, nbOrb);
  743.         }

  744.         // Ref [1], Eq. 3.10
  745.         dMdY = dMdY.multiply(IpB1B4);

  746.         for (int i = 0; i < dMdY.getRowDimension(); i++) {
  747.             for (int j = 0; j < nbOrb; j++) {
  748.                 final double driverScale = builder.getOrbitalParametersDrivers().getDrivers().get(j).getScale();
  749.                 measurementMatrix.setEntry(i, j, dMdY.getEntry(i, j) / sigma[i] * driverScale);
  750.             }
  751.             for (int j = 0; j < nbProp; j++) {
  752.                 final double driverScale = estimatedPropagationParameters.getDrivers().get(j).getScale();
  753.                 measurementMatrix.setEntry(i, j + nbOrb,
  754.                                            dMdY.getEntry(i, j + nbOrb) / sigma[i] * driverScale);
  755.             }
  756.         }

  757.         // Normalized measurement matrix's columns related to measurement parameters
  758.         // --------------------------------------------------------------

  759.         // Jacobian of the measurement with respect to measurement parameters
  760.         // Gather the measurement parameters linked to current measurement
  761.         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
  762.             if (driver.isSelected()) {
  763.                 // Derivatives of current measurement w/r to selected measurement parameter
  764.                 final double[] aMPm = predictedMeasurement.getParameterDerivatives(driver);

  765.                 // Check that the measurement parameter is managed by the filter
  766.                 if (measurementParameterColumns.get(driver.getName()) != null) {
  767.                     // Column of the driver in the measurement matrix
  768.                     final int driverColumn = measurementParameterColumns.get(driver.getName());

  769.                     // Fill the corresponding indexes of the measurement matrix
  770.                     for (int i = 0; i < aMPm.length; ++i) {
  771.                         measurementMatrix.setEntry(i, driverColumn, aMPm[i] / sigma[i] * driver.getScale());
  772.                     }
  773.                 }
  774.             }
  775.         }

  776.         return measurementMatrix;
  777.     }

  778.     /** Predict the filter correction for the new observation.
  779.      * @param stm normalized state transition matrix
  780.      * @return the predicted filter correction for the new observation
  781.      */
  782.     private RealVector predictFilterCorrection(final RealMatrix stm) {
  783.         // Ref [1], Eq. 3.5a and 3.5b
  784.         return stm.operate(correctedFilterCorrection);
  785.     }

  786.     /** Compute the predicted osculating elements.
  787.      * @param filterCorrection kalman filter correction
  788.      * @return the predicted osculating element
  789.      */
  790.     private double[] computeOsculatingElements(final RealVector filterCorrection) {

  791.         // Number of estimated orbital parameters
  792.         final int nbOrb = getNumberSelectedOrbitalDrivers();

  793.         // B1
  794.         final RealMatrix B1 = harvester.getB1();

  795.         // Short periodic terms
  796.         final double[] shortPeriodTerms = dsstPropagator.getShortPeriodTermsValue(nominalMeanSpacecraftState);

  797.         // Physical filter correction
  798.         final RealVector physicalFilterCorrection = MatrixUtils.createRealVector(nbOrb);
  799.         for (int index = 0; index < nbOrb; index++) {
  800.             physicalFilterCorrection.addToEntry(index, filterCorrection.getEntry(index) * scale[index]);
  801.         }

  802.         // B1 * physicalCorrection
  803.         final RealVector B1Correction = B1.operate(physicalFilterCorrection);

  804.         // Nominal mean elements
  805.         final double[] nominalMeanElements = new double[nbOrb];
  806.         OrbitType.EQUINOCTIAL.mapOrbitToArray(nominalMeanSpacecraftState.getOrbit(), builder.getPositionAngle(), nominalMeanElements, null);

  807.         // Ref [1] Eq. 3.6
  808.         final double[] osculatingElements = new double[nbOrb];
  809.         for (int i = 0; i < nbOrb; i++) {
  810.             osculatingElements[i] = nominalMeanElements[i] +
  811.                                     physicalFilterCorrection.getEntry(i) +
  812.                                     shortPeriodTerms[i] +
  813.                                     B1Correction.getEntry(i);
  814.         }

  815.         // Return
  816.         return osculatingElements;

  817.     }

  818.     /** Analytical computation of derivatives.
  819.      * This method allow to compute analytical derivatives.
  820.      * @param state mean state used to calculate short period perturbations
  821.      */
  822.     private void analyticalDerivativeComputations(final SpacecraftState state) {
  823.         harvester.setReferenceState(state);
  824.     }

  825.     /** Normalize a covariance matrix.
  826.      * The covariance P is an mxm matrix where m = nbOrb + nbPropag + nbMeas
  827.      * For each element [i,j] of P the corresponding normalized value is:
  828.      * Pn[i,j] = P[i,j] / (scale[i]*scale[j])
  829.      * @param physicalCovarianceMatrix The "physical" covariance matrix in input
  830.      * @return the normalized covariance matrix
  831.      */
  832.     private RealMatrix normalizeCovarianceMatrix(final RealMatrix physicalCovarianceMatrix) {

  833.         // Initialize output matrix
  834.         final int nbParams = physicalCovarianceMatrix.getRowDimension();
  835.         final RealMatrix normalizedCovarianceMatrix = MatrixUtils.createRealMatrix(nbParams, nbParams);

  836.         // Normalize the state matrix
  837.         for (int i = 0; i < nbParams; ++i) {
  838.             for (int j = 0; j < nbParams; ++j) {
  839.                 normalizedCovarianceMatrix.setEntry(i, j,
  840.                                                     physicalCovarianceMatrix.getEntry(i, j) /
  841.                                                     (scale[i] * scale[j]));
  842.             }
  843.         }
  844.         return normalizedCovarianceMatrix;
  845.     }

  846.     /** Check dimension.
  847.      * @param dimension dimension to check
  848.      * @param orbitalParameters orbital parameters
  849.      * @param propagationParameters propagation parameters
  850.      * @param measurementParameters measurements parameters
  851.      */
  852.     private void checkDimension(final int dimension,
  853.                                 final ParameterDriversList orbitalParameters,
  854.                                 final ParameterDriversList propagationParameters,
  855.                                 final ParameterDriversList measurementParameters) {

  856.         // count parameters, taking care of counting all orbital parameters
  857.         // regardless of them being estimated or not
  858.         int requiredDimension = orbitalParameters.getNbParams();
  859.         for (final ParameterDriver driver : propagationParameters.getDrivers()) {
  860.             if (driver.isSelected()) {
  861.                 ++requiredDimension;
  862.             }
  863.         }
  864.         for (final ParameterDriver driver : measurementParameters.getDrivers()) {
  865.             if (driver.isSelected()) {
  866.                 ++requiredDimension;
  867.             }
  868.         }

  869.         if (dimension != requiredDimension) {
  870.             // there is a problem, set up an explicit error message
  871.             final StringBuilder strBuilder = new StringBuilder();
  872.             for (final ParameterDriver driver : orbitalParameters.getDrivers()) {
  873.                 if (strBuilder.length() > 0) {
  874.                     strBuilder.append(", ");
  875.                 }
  876.                 strBuilder.append(driver.getName());
  877.             }
  878.             for (final ParameterDriver driver : propagationParameters.getDrivers()) {
  879.                 if (driver.isSelected()) {
  880.                     strBuilder.append(driver.getName());
  881.                 }
  882.             }
  883.             for (final ParameterDriver driver : measurementParameters.getDrivers()) {
  884.                 if (driver.isSelected()) {
  885.                     strBuilder.append(driver.getName());
  886.                 }
  887.             }
  888.             throw new OrekitException(OrekitMessages.DIMENSION_INCONSISTENT_WITH_PARAMETERS,
  889.                                       dimension, strBuilder.toString());
  890.         }

  891.     }

  892.     /** Set and apply a dynamic outlier filter on a measurement.<p>
  893.      * Loop on the modifiers to see if a dynamic outlier filter needs to be applied.<p>
  894.      * Compute the sigma array using the matrix in input and set the filter.<p>
  895.      * Apply the filter by calling the modify method on the estimated measurement.<p>
  896.      * Reset the filter.
  897.      * @param measurement measurement to filter
  898.      * @param innovationCovarianceMatrix So called innovation covariance matrix S, with:<p>
  899.      *        S = H.Ppred.Ht + R<p>
  900.      *        Where:<p>
  901.      *         - H is the normalized measurement matrix (Ht its transpose)<p>
  902.      *         - Ppred is the normalized predicted covariance matrix<p>
  903.      *         - R is the normalized measurement noise matrix
  904.      * @param <T> the type of measurement
  905.      */
  906.     private <T extends ObservedMeasurement<T>> void applyDynamicOutlierFilter(final EstimatedMeasurement<T> measurement,
  907.                                                                               final RealMatrix innovationCovarianceMatrix) {

  908.         // Observed measurement associated to the predicted measurement
  909.         final ObservedMeasurement<T> observedMeasurement = measurement.getObservedMeasurement();

  910.         // Check if a dynamic filter was added to the measurement
  911.         // If so, update its sigma value and apply it
  912.         for (EstimationModifier<T> modifier : observedMeasurement.getModifiers()) {
  913.             if (modifier instanceof DynamicOutlierFilter<?>) {
  914.                 final DynamicOutlierFilter<T> dynamicOutlierFilter = (DynamicOutlierFilter<T>) modifier;

  915.                 // Initialize the values of the sigma array used in the dynamic filter
  916.                 final double[] sigmaDynamic     = new double[innovationCovarianceMatrix.getColumnDimension()];
  917.                 final double[] sigmaMeasurement = observedMeasurement.getTheoreticalStandardDeviation();

  918.                 // Set the sigma value for each element of the measurement
  919.                 // Here we do use the value suggested by David A. Vallado (see [1]ยง10.6):
  920.                 // sigmaDynamic[i] = sqrt(diag(S))*sigma[i]
  921.                 // With S = H.Ppred.Ht + R
  922.                 // Where:
  923.                 //  - S is the measurement error matrix in input
  924.                 //  - H is the normalized measurement matrix (Ht its transpose)
  925.                 //  - Ppred is the normalized predicted covariance matrix
  926.                 //  - R is the normalized measurement noise matrix
  927.                 //  - sigma[i] is the theoretical standard deviation of the ith component of the measurement.
  928.                 //    It is used here to un-normalize the value before it is filtered
  929.                 for (int i = 0; i < sigmaDynamic.length; i++) {
  930.                     sigmaDynamic[i] = FastMath.sqrt(innovationCovarianceMatrix.getEntry(i, i)) * sigmaMeasurement[i];
  931.                 }
  932.                 dynamicOutlierFilter.setSigma(sigmaDynamic);

  933.                 // Apply the modifier on the estimated measurement
  934.                 modifier.modify(measurement);

  935.                 // Re-initialize the value of the filter for the next measurement of the same type
  936.                 dynamicOutlierFilter.setSigma(null);
  937.             }
  938.         }
  939.     }

  940.     /** Get the number of estimated orbital parameters.
  941.      * @return the number of estimated orbital parameters
  942.      */
  943.     private int getNumberSelectedOrbitalDrivers() {
  944.         return estimatedOrbitalParameters.getNbParams();
  945.     }

  946.     /** Get the number of estimated propagation parameters.
  947.      * @return the number of estimated propagation parameters
  948.      */
  949.     private int getNumberSelectedPropagationDrivers() {
  950.         return estimatedPropagationParameters.getNbParams();
  951.     }

  952.     /** Get the number of estimated measurement parameters.
  953.      * @return the number of estimated measurement parameters
  954.      */
  955.     private int getNumberSelectedMeasurementDrivers() {
  956.         return estimatedMeasurementsParameters.getNbParams();
  957.     }

  958.     /** Update the estimated parameters after the correction phase of the filter.
  959.      * The min/max allowed values are handled by the parameter themselves.
  960.      */
  961.     private void updateParameters() {
  962.         final RealVector correctedState = correctedEstimate.getState();
  963.         int i = 0;
  964.         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
  965.             // let the parameter handle min/max clipping
  966.             driver.setNormalizedValue(driver.getNormalizedValue() + correctedState.getEntry(i++));
  967.         }
  968.         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
  969.             // let the parameter handle min/max clipping
  970.             driver.setNormalizedValue(driver.getNormalizedValue() + correctedState.getEntry(i++));
  971.         }
  972.         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
  973.             // let the parameter handle min/max clipping
  974.             driver.setNormalizedValue(driver.getNormalizedValue() + correctedState.getEntry(i++));
  975.         }
  976.     }

  977. }