/* Copyright 2002-2022 CS GROUP
    * Licensed to CS GROUP (CS) under one or more
    * contributor license agreements.  See the NOTICE file distributed with
    * this work for additional information regarding copyright ownership.
    * CS licenses this file to You under the Apache License, Version 2.0
    * (the "License"); you may not use this file except in compliance with
    * the License.  You may obtain a copy of the License at
    *
    *   http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  package org.orekit.estimation.sequential;
  
  import java.util.ArrayList;
  import java.util.Comparator;
  import java.util.HashMap;
  import java.util.List;
  import java.util.Map;
  
  import org.hipparchus.exception.MathRuntimeException;
  import org.hipparchus.filtering.kalman.ProcessEstimate;
  import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
  import org.hipparchus.filtering.kalman.extended.NonLinearEvolution;
  import org.hipparchus.filtering.kalman.extended.NonLinearProcess;
  import org.hipparchus.linear.Array2DRowRealMatrix;
  import org.hipparchus.linear.ArrayRealVector;
  import org.hipparchus.linear.MatrixUtils;
  import org.hipparchus.linear.QRDecomposition;
  import org.hipparchus.linear.RealMatrix;
  import org.hipparchus.linear.RealVector;
  import org.hipparchus.util.FastMath;
  import org.orekit.errors.OrekitException;
  import org.orekit.errors.OrekitMessages;
  import org.orekit.estimation.measurements.EstimatedMeasurement;
  import org.orekit.estimation.measurements.EstimationModifier;
  import org.orekit.estimation.measurements.ObservedMeasurement;
  import org.orekit.estimation.measurements.modifiers.DynamicOutlierFilter;
  import org.orekit.orbits.Orbit;
  import org.orekit.orbits.OrbitType;
  import org.orekit.propagation.PropagationType;
  import org.orekit.propagation.SpacecraftState;
  import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
  import org.orekit.propagation.semianalytical.dsst.DSSTHarvester;
  import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
  import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
  import org.orekit.propagation.semianalytical.dsst.forces.ShortPeriodTerms;
  import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.time.ChronologicalComparator;
  import org.orekit.utils.ParameterDriver;
  import org.orekit.utils.ParameterDriversList;
  import org.orekit.utils.ParameterDriversList.DelegatingDriver;
  
  /** Process model to use with a {@link SemiAnalyticalKalmanEstimator}.
   *
   * @see "Folcik Z., Orbit Determination Using Modern Filters/Smoothers and Continuous Thrust Modeling,
   *       Master of Science Thesis, Department of Aeronautics and Astronautics, MIT, June, 2008."
   *
   * @see "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
   *       Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
   *       Specialist Conference, Big Sky, August 2021."
   *
   * @author Julie Bayard
   * @author Bryan Cazabonne
   * @author Maxime Journot
   * @since 11.1
   */
  public  class SemiAnalyticalKalmanModel implements KalmanEstimation, NonLinearProcess<MeasurementDecorator> {
  
      /** Builders for DSST propagator. */
      private final DSSTPropagatorBuilder builder;
  
      /** Estimated orbital parameters. */
      private final ParameterDriversList estimatedOrbitalParameters;
  
      /** Per-builder estimated propagation drivers. */
      private final ParameterDriversList estimatedPropagationParameters;
  
      /** Estimated measurements parameters. */
      private final ParameterDriversList estimatedMeasurementsParameters;
  
      /** Map for propagation parameters columns. */
      private final Map<String, Integer> propagationParameterColumns;
  
      /** Map for measurements parameters columns. */
      private final Map<String, Integer> measurementParameterColumns;
  
      /** Scaling factors. */
      private final double[] scale;
  
      /** Provider for covariance matrix. */
      private final CovarianceMatrixProvider covarianceMatrixProvider;
  
      /** Process noise matrix provider for measurement parameters. */
     private final CovarianceMatrixProvider measurementProcessNoiseMatrix;
 
     /** Harvester between two-dimensional Jacobian matrices and one-dimensional additional state arrays. */
     private DSSTHarvester harvester;
 
     /** Propagators for the reference trajectories, up to current date. */
     private DSSTPropagator dsstPropagator;
 
     /** Observer to retrieve current estimation info. */
     private KalmanObserver observer;
 
     /** Current number of measurement. */
     private int currentMeasurementNumber;
 
     /** Current date. */
     private AbsoluteDate currentDate;
 
     /** Predicted mean element filter correction. */
     private RealVector predictedFilterCorrection;
 
     /** Corrected mean element filter correction. */
     private RealVector correctedFilterCorrection;
 
     /** Predicted measurement. */
     private EstimatedMeasurement<?> predictedMeasurement;
 
     /** Corrected measurement. */
     private EstimatedMeasurement<?> correctedMeasurement;
 
     /** Nominal mean spacecraft state. */
     private SpacecraftState nominalMeanSpacecraftState;
 
     /** Previous nominal mean spacecraft state. */
     private SpacecraftState previousNominalMeanSpacecraftState;
 
     /** Current corrected estimate. */
     private ProcessEstimate correctedEstimate;
 
     /** Inverse of the orbital part of the state transition matrix. */
     private RealMatrix phiS;
 
     /** Propagation parameters part of the state transition matrix. */
     private RealMatrix psiS;
 
     /** Kalman process model constructor (package private).
      * @param propagatorBuilder propagators builders used to evaluate the orbits.
      * @param covarianceMatrixProvider provider for covariance matrix
      * @param estimatedMeasurementParameters measurement parameters to estimate
      * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
      */
     protected SemiAnalyticalKalmanModel(final DSSTPropagatorBuilder propagatorBuilder,
                                         final CovarianceMatrixProvider covarianceMatrixProvider,
                                         final ParameterDriversList estimatedMeasurementParameters,
                                         final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
 
         this.builder                         = propagatorBuilder;
         this.estimatedMeasurementsParameters = estimatedMeasurementParameters;
         this.measurementParameterColumns     = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
         this.observer                        = null;
         this.currentMeasurementNumber        = 0;
         this.currentDate                     = propagatorBuilder.getInitialOrbitDate();
         this.covarianceMatrixProvider        = covarianceMatrixProvider;
         this.measurementProcessNoiseMatrix   = measurementProcessNoiseMatrix;
 
         // Number of estimated parameters
         int columns = 0;
 
         // Set estimated orbital parameters
         estimatedOrbitalParameters = new ParameterDriversList();
         for (final ParameterDriver driver : builder.getOrbitalParametersDrivers().getDrivers()) {
 
             // Verify if the driver reference date has been set
             if (driver.getReferenceDate() == null) {
                 driver.setReferenceDate(currentDate);
             }
 
             // Verify if the driver is selected
             if (driver.isSelected()) {
                 estimatedOrbitalParameters.add(driver);
                 columns++;
             }
 
         }
 
         // Set estimated propagation parameters
         estimatedPropagationParameters = new ParameterDriversList();
         final List<String> estimatedPropagationParametersNames = new ArrayList<>();
         for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
 
             // Verify if the driver reference date has been set
             if (driver.getReferenceDate() == null) {
                 driver.setReferenceDate(currentDate);
             }
 
             // Verify if the driver is selected
             if (driver.isSelected()) {
                 estimatedPropagationParameters.add(driver);
                 final String driverName = driver.getName();
                 // Add the driver name if it has not been added yet
                 if (!estimatedPropagationParametersNames.contains(driverName)) {
                     estimatedPropagationParametersNames.add(driverName);
                 }
             }
 
         }
         estimatedPropagationParametersNames.sort(Comparator.naturalOrder());
 
         // Populate the map of propagation drivers' columns and update the total number of columns
         propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
         for (final String driverName : estimatedPropagationParametersNames) {
             propagationParameterColumns.put(driverName, columns);
             ++columns;
         }
 
         // Set the estimated measurement parameters
         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
             if (parameter.getReferenceDate() == null) {
                 parameter.setReferenceDate(currentDate);
             }
             measurementParameterColumns.put(parameter.getName(), columns);
             ++columns;
         }
 
         // Compute the scale factors
         this.scale = new double[columns];
         int index = 0;
         for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
             scale[index++] = driver.getScale();
         }
         for (final ParameterDriver driver : estimatedPropagationParameters.getDrivers()) {
             scale[index++] = driver.getScale();
         }
         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
             scale[index++] = driver.getScale();
         }
 
         // Build the reference propagator and add its partial derivatives equations implementation
         updateReferenceTrajectory(getEstimatedPropagator());
         this.nominalMeanSpacecraftState = dsstPropagator.getInitialState();
         this.previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;
 
         // Initialize "field" short periodic terms
         harvester.initializeFieldShortPeriodTerms(nominalMeanSpacecraftState);
 
         // Initialize the estimated normalized mean element filter correction (See Ref [1], Eq. 3.2a)
         this.predictedFilterCorrection = MatrixUtils.createRealVector(columns);
         this.correctedFilterCorrection = predictedFilterCorrection;
 
         // Initialize propagation parameters part of the state transition matrix (See Ref [1], Eq. 3.2c)
         this.psiS = null;
         if (estimatedPropagationParameters.getNbParams() != 0) {
             this.psiS = MatrixUtils.createRealMatrix(getNumberSelectedOrbitalDrivers(),
                                                      getNumberSelectedPropagationDrivers());
         }
 
         // Initialize inverse of the orbital part of the state transition matrix (See Ref [1], Eq. 3.2d)
         this.phiS = MatrixUtils.createRealIdentityMatrix(getNumberSelectedOrbitalDrivers());
 
         // Number of estimated measurement parameters
         final int nbMeas = getNumberSelectedMeasurementDrivers();
 
         // Number of estimated dynamic parameters (orbital + propagation)
         final int nbDyn  = getNumberSelectedOrbitalDrivers() + getNumberSelectedPropagationDrivers();
 
         // Covariance matrix
         final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
         final RealMatrix noiseP = covarianceMatrixProvider.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
         noiseK.setSubMatrix(noiseP.getData(), 0, 0);
         if (measurementProcessNoiseMatrix != null) {
             final RealMatrix noiseM = measurementProcessNoiseMatrix.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
             noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
         }
 
         // Verify dimension
         checkDimension(noiseK.getRowDimension(),
                        builder.getOrbitalParametersDrivers(),
                        builder.getPropagationParametersDrivers(),
                        estimatedMeasurementsParameters);
 
         final RealMatrix correctedCovariance = normalizeCovarianceMatrix(noiseK);
 
         // Initialize corrected estimate
         this.correctedEstimate = new ProcessEstimate(0.0, correctedFilterCorrection, correctedCovariance);
 
     }
 
     /** Get the observer for Kalman Filter estimations.
      * @return the observer for Kalman Filter estimations
      */
     public KalmanObserver getObserver() {
         return observer;
     }
 
     /** Set the observer.
      * @param observer the observer
      */
     public void setObserver(final KalmanObserver observer) {
         this.observer = observer;
     }
 
     /** Get the current corrected estimate.
      * @return current corrected estimate
      */
     public ProcessEstimate getEstimate() {
         return correctedEstimate;
     }
 
     /** Process a single measurement.
      * <p>
      * Update the filter with the new measurements.
      * </p>
      * @param observedMeasurements the list of measurements to process
      * @param filter Extended Kalman Filter
      * @return estimated propagator
      */
     public DSSTPropagator processMeasurements(final List<ObservedMeasurement<?>> observedMeasurements,
                                               final ExtendedKalmanFilter<MeasurementDecorator> filter) {
         try {
 
             // Sort the measurement
             observedMeasurements.sort(new ChronologicalComparator());
 
             // Initialize step handler and set it to the propagator
             final EskfMeasurementHandler stepHandler = new EskfMeasurementHandler(this, filter, observedMeasurements, builder.getInitialOrbitDate());
             dsstPropagator.getMultiplexer().add(stepHandler);
             dsstPropagator.propagate(observedMeasurements.get(0).getDate(), observedMeasurements.get(observedMeasurements.size() - 1).getDate());
 
             // Return the last estimated propagator
             return getEstimatedPropagator();
 
         } catch (MathRuntimeException mrte) {
             throw new OrekitException(mrte);
         }
     }
 
     /** Get the propagator estimated with the values set in the propagator builder.
      * @return propagator based on the current values in the builder
      */
     public DSSTPropagator getEstimatedPropagator() {
         // Return propagator built with current instantiation of the propagator builder
         return builder.buildPropagator(builder.getSelectedNormalizedParameters());
     }
 
     /** {@inheritDoc} */
     @Override
     public NonLinearEvolution getEvolution(final double previousTime, final RealVector previousState,
                                            final MeasurementDecorator measurement) {
 
         // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
         final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
             if (driver.getReferenceDate() == null) {
                 driver.setReferenceDate(builder.getInitialOrbitDate());
             }
         }
 
         // Increment measurement number
         ++currentMeasurementNumber;
 
         // Update the current date
         currentDate = measurement.getObservedMeasurement().getDate();
 
         // Normalized state transition matrix
         final RealMatrix stm = getErrorStateTransitionMatrix();
 
         // Predict filter correction
         predictedFilterCorrection = predictFilterCorrection(stm);
 
         // Short period term derivatives
         analyticalDerivativeComputations(nominalMeanSpacecraftState);
 
         // Calculate the predicted osculating elements
         final double[] osculating = computeOsculatingElements(predictedFilterCorrection);
         final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, builder.getPositionAngle(),
                                                                             currentDate, nominalMeanSpacecraftState.getMu(),
                                                                             nominalMeanSpacecraftState.getFrame());
 
         // Compute the predicted measurements  (See Ref [1], Eq. 3.8)
         predictedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
                                                             currentMeasurementNumber,
                                                             new SpacecraftState[] {
                                                                 new SpacecraftState(osculatingOrbit,
                                                                                     nominalMeanSpacecraftState.getAttitude(),
                                                                                     nominalMeanSpacecraftState.getMass(),
                                                                                     nominalMeanSpacecraftState.getAdditionalStatesValues(),
                                                                                     nominalMeanSpacecraftState.getAdditionalStatesDerivatives())
                                                             });
 
         // Normalized measurement matrix
         final RealMatrix measurementMatrix = getMeasurementMatrix();
 
         // Number of estimated measurement parameters
         final int nbMeas = getNumberSelectedMeasurementDrivers();
 
         // Number of estimated dynamic parameters (orbital + propagation)
         final int nbDyn  = getNumberSelectedOrbitalDrivers() + getNumberSelectedPropagationDrivers();
 
         // Covariance matrix
         final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
         final RealMatrix noiseP = covarianceMatrixProvider.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
         noiseK.setSubMatrix(noiseP.getData(), 0, 0);
         if (measurementProcessNoiseMatrix != null) {
             final RealMatrix noiseM = measurementProcessNoiseMatrix.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
             noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
         }
 
         // Verify dimension
         checkDimension(noiseK.getRowDimension(),
                        builder.getOrbitalParametersDrivers(),
                        builder.getPropagationParametersDrivers(),
                        estimatedMeasurementsParameters);
 
         final RealMatrix normalizedProcessNoise = normalizeCovarianceMatrix(noiseK);
 
         // Return
         return new NonLinearEvolution(measurement.getTime(), predictedFilterCorrection, stm,
                                       normalizedProcessNoise, measurementMatrix);
     }
 
     /** {@inheritDoc} */
     @Override
     public RealVector getInnovation(final MeasurementDecorator measurement, final NonLinearEvolution evolution,
                                     final RealMatrix innovationCovarianceMatrix) {
 
         // Apply the dynamic outlier filter, if it exists
         applyDynamicOutlierFilter(predictedMeasurement, innovationCovarianceMatrix);
         if (predictedMeasurement.getStatus() == EstimatedMeasurement.Status.REJECTED)  {
             // set innovation to null to notify filter measurement is rejected
             return null;
         } else {
             // Normalized innovation of the measurement (Nx1)
             final double[] observed  = predictedMeasurement.getObservedMeasurement().getObservedValue();
             final double[] estimated = predictedMeasurement.getEstimatedValue();
             final double[] sigma     = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();
             final double[] residuals = new double[observed.length];
 
             for (int i = 0; i < observed.length; i++) {
                 residuals[i] = (observed[i] - estimated[i]) / sigma[i];
             }
             return MatrixUtils.createRealVector(residuals);
         }
 
     }
 
     /** Finalize estimation.
      * @param observedMeasurement measurement that has just been processed
      * @param estimate corrected estimate
      */
     public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
                                    final ProcessEstimate estimate) {
         // Update the process estimate
         correctedEstimate = estimate;
         // Corrected filter correction
         correctedFilterCorrection = estimate.getState();
         // Update the previous nominal mean spacecraft state
         previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;
         // Calculate the corrected osculating elements
         final double[] osculating = computeOsculatingElements(correctedFilterCorrection);
         final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, builder.getPositionAngle(),
                                                                             currentDate, nominalMeanSpacecraftState.getMu(),
                                                                             nominalMeanSpacecraftState.getFrame());
 
         // Compute the corrected measurements
         correctedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
                                                             currentMeasurementNumber,
                                                             new SpacecraftState[] {
                                                                 new SpacecraftState(osculatingOrbit,
                                                                                     nominalMeanSpacecraftState.getAttitude(),
                                                                                     nominalMeanSpacecraftState.getMass(),
                                                                                     nominalMeanSpacecraftState.getAdditionalStatesValues(),
                                                                                     nominalMeanSpacecraftState.getAdditionalStatesDerivatives())
                                                             });
     }
 
     /** Finalize estimation operations on the observation grid. */
     public void finalizeOperationsObservationGrid() {
         // Update parameters
         updateParameters();
     }
 
     /** {@inheritDoc} */
     @Override
     public ParameterDriversList getEstimatedOrbitalParameters() {
         return estimatedOrbitalParameters;
     }
 
     /** {@inheritDoc} */
     @Override
     public ParameterDriversList getEstimatedPropagationParameters() {
         return estimatedPropagationParameters;
     }
 
     /** {@inheritDoc} */
     @Override
     public ParameterDriversList getEstimatedMeasurementsParameters() {
         return estimatedMeasurementsParameters;
     }
 
     /** {@inheritDoc} */
     @Override
     public SpacecraftState[] getPredictedSpacecraftStates() {
         return new SpacecraftState[] {nominalMeanSpacecraftState};
     }
 
     /** {@inheritDoc} */
     @Override
     public SpacecraftState[] getCorrectedSpacecraftStates() {
         return new SpacecraftState[] {getEstimatedPropagator().getInitialState()};
     }
 
     /** {@inheritDoc} */
     @Override
     public RealVector getPhysicalEstimatedState() {
         // Method {@link ParameterDriver#getValue()} is used to get
         // the physical values of the state.
         // The scales'array is used to get the size of the state vector
         final RealVector physicalEstimatedState = new ArrayRealVector(scale.length);
         int i = 0;
         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
             physicalEstimatedState.setEntry(i++, driver.getValue());
         }
         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
             physicalEstimatedState.setEntry(i++, driver.getValue());
         }
         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
             physicalEstimatedState.setEntry(i++, driver.getValue());
         }
 
         return physicalEstimatedState;
     }
 
     /** {@inheritDoc} */
     @Override
     public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
         // Un-normalize the estimated covariance matrix (P) from Hipparchus and return it.
         // The covariance P is an mxm matrix where m = nbOrb + nbPropag + nbMeas
         // For each element [i,j] of P the corresponding normalized value is:
         // Pn[i,j] = P[i,j] / (scale[i]*scale[j])
         // Consequently: P[i,j] = Pn[i,j] * scale[i] * scale[j]
 
         // Normalized covariance matrix
         final RealMatrix normalizedP = correctedEstimate.getCovariance();
 
         // Initialize physical covariance matrix
         final int nbParams = normalizedP.getRowDimension();
         final RealMatrix physicalP = MatrixUtils.createRealMatrix(nbParams, nbParams);
 
         // Un-normalize the covairance matrix
         for (int i = 0; i < nbParams; ++i) {
             for (int j = 0; j < nbParams; ++j) {
                 physicalP.setEntry(i, j, normalizedP.getEntry(i, j) * scale[i] * scale[j]);
             }
         }
         return physicalP;
     }
 
     /** {@inheritDoc} */
     @Override
     public RealMatrix getPhysicalStateTransitionMatrix() {
         //  Un-normalize the state transition matrix (φ) from Hipparchus and return it.
         // φ is an mxm matrix where m = nbOrb + nbPropag + nbMeas
         // For each element [i,j] of normalized φ (φn), the corresponding physical value is:
         // φ[i,j] = φn[i,j] * scale[i] / scale[j]
 
         // Normalized matrix
         final RealMatrix normalizedSTM = correctedEstimate.getStateTransitionMatrix();
 
         if (normalizedSTM == null) {
             return null;
         } else {
             // Initialize physical matrix
             final int nbParams = normalizedSTM.getRowDimension();
             final RealMatrix physicalSTM = MatrixUtils.createRealMatrix(nbParams, nbParams);
 
             // Un-normalize the matrix
             for (int i = 0; i < nbParams; ++i) {
                 for (int j = 0; j < nbParams; ++j) {
                     physicalSTM.setEntry(i, j,
                                          normalizedSTM.getEntry(i, j) * scale[i] / scale[j]);
                 }
             }
             return physicalSTM;
         }
     }
 
     /** {@inheritDoc} */
     @Override
     public RealMatrix getPhysicalMeasurementJacobian() {
         // Un-normalize the measurement matrix (H) from Hipparchus and return it.
         // H is an nxm matrix where:
         //  - m = nbOrb + nbPropag + nbMeas is the number of estimated parameters
         //  - n is the size of the measurement being processed by the filter
         // For each element [i,j] of normalized H (Hn) the corresponding physical value is:
         // H[i,j] = Hn[i,j] * σ[i] / scale[j]
 
         // Normalized matrix
         final RealMatrix normalizedH = correctedEstimate.getMeasurementJacobian();
 
         if (normalizedH == null) {
             return null;
         } else {
             // Get current measurement sigmas
             final double[] sigmas = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();
 
             // Initialize physical matrix
             final int nbLine = normalizedH.getRowDimension();
             final int nbCol  = normalizedH.getColumnDimension();
             final RealMatrix physicalH = MatrixUtils.createRealMatrix(nbLine, nbCol);
 
             // Un-normalize the matrix
             for (int i = 0; i < nbLine; ++i) {
                 for (int j = 0; j < nbCol; ++j) {
                     physicalH.setEntry(i, j, normalizedH.getEntry(i, j) * sigmas[i] / scale[j]);
                 }
             }
             return physicalH;
         }
     }
 
     /** {@inheritDoc} */
     @Override
     public RealMatrix getPhysicalInnovationCovarianceMatrix() {
         // Un-normalize the innovation covariance matrix (S) from Hipparchus and return it.
         // S is an nxn matrix where n is the size of the measurement being processed by the filter
         // For each element [i,j] of normalized S (Sn) the corresponding physical value is:
         // S[i,j] = Sn[i,j] * σ[i] * σ[j]
 
         // Normalized matrix
         final RealMatrix normalizedS = correctedEstimate.getInnovationCovariance();
 
         if (normalizedS == null) {
             return null;
         } else {
             // Get current measurement sigmas
             final double[] sigmas = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();
 
             // Initialize physical matrix
             final int nbMeas = sigmas.length;
             final RealMatrix physicalS = MatrixUtils.createRealMatrix(nbMeas, nbMeas);
 
             // Un-normalize the matrix
             for (int i = 0; i < nbMeas; ++i) {
                 for (int j = 0; j < nbMeas; ++j) {
                     physicalS.setEntry(i, j, normalizedS.getEntry(i, j) * sigmas[i] *   sigmas[j]);
                 }
             }
             return physicalS;
         }
     }
 
     /** {@inheritDoc} */
     @Override
     public RealMatrix getPhysicalKalmanGain() {
         // Un-normalize the Kalman gain (K) from Hipparchus and return it.
         // K is an mxn matrix where:
         //  - m = nbOrb + nbPropag + nbMeas is the number of estimated parameters
         //  - n is the size of the measurement being processed by the filter
         // For each element [i,j] of normalized K (Kn) the corresponding physical value is:
         // K[i,j] = Kn[i,j] * scale[i] / σ[j]
 
         // Normalized matrix
         final RealMatrix normalizedK = correctedEstimate.getKalmanGain();
 
         if (normalizedK == null) {
             return null;
         } else {
             // Get current measurement sigmas
             final double[] sigmas = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();
 
             // Initialize physical matrix
             final int nbLine = normalizedK.getRowDimension();
             final int nbCol  = normalizedK.getColumnDimension();
             final RealMatrix physicalK = MatrixUtils.createRealMatrix(nbLine, nbCol);
 
             // Un-normalize the matrix
             for (int i = 0; i < nbLine; ++i) {
                 for (int j = 0; j < nbCol; ++j) {
                     physicalK.setEntry(i, j, normalizedK.getEntry(i, j) * scale[i] / sigmas[j]);
                 }
             }
             return physicalK;
         }
     }
 
     /** {@inheritDoc} */
     @Override
     public int getCurrentMeasurementNumber() {
         return currentMeasurementNumber;
     }
 
     /** {@inheritDoc} */
     @Override
     public AbsoluteDate getCurrentDate() {
         return currentDate;
     }
 
     /** {@inheritDoc} */
     @Override
     public EstimatedMeasurement<?> getPredictedMeasurement() {
         return predictedMeasurement;
     }
 
     /** {@inheritDoc} */
     @Override
     public EstimatedMeasurement<?> getCorrectedMeasurement() {
         return correctedMeasurement;
     }
 
     /** Update the nominal spacecraft state.
      * @param nominal nominal spacecraft state
      */
     public void updateNominalSpacecraftState(final SpacecraftState nominal) {
         this.nominalMeanSpacecraftState = nominal;
         // Update the builder with the nominal mean elements orbit
         builder.resetOrbit(nominal.getOrbit(), PropagationType.MEAN);
     }
 
     /** Update the reference trajectories using the propagator as input.
      * @param propagator The new propagator to use
      */
     public void updateReferenceTrajectory(final DSSTPropagator propagator) {
 
         dsstPropagator = propagator;
 
         // Equation name
         final String equationName = SemiAnalyticalKalmanEstimator.class.getName() + "-derivatives-";
 
         // Mean state
         final SpacecraftState meanState = dsstPropagator.initialIsOsculating() ?
                        DSSTPropagator.computeMeanState(dsstPropagator.getInitialState(), dsstPropagator.getAttitudeProvider(), dsstPropagator.getAllForceModels()) :
                        dsstPropagator.getInitialState();
 
         // Update the jacobian harvester
         dsstPropagator.setInitialState(meanState, PropagationType.MEAN);
         harvester = (DSSTHarvester) dsstPropagator.setupMatricesComputation(equationName, null, null);
 
     }
 
     /** Update the DSST short periodic terms.
      * @param state current mean state
      */
     public void updateShortPeriods(final SpacecraftState state) {
         // Loop on DSST force models
         for (final DSSTForceModel model : builder.getAllForceModels()) {
             model.updateShortPeriodTerms(model.getParameters(), state);
         }
         harvester.updateFieldShortPeriodTerms(state);
     }
 
     /** Initialize the short periodic terms for the Kalman Filter.
      * @param meanState mean state for auxiliary elements
      */
     public void initializeShortPeriodicTerms(final SpacecraftState meanState) {
         final List<ShortPeriodTerms> shortPeriodTerms = new ArrayList<ShortPeriodTerms>();
         for (final DSSTForceModel force :  builder.getAllForceModels()) {
             shortPeriodTerms.addAll(force.initializeShortPeriodTerms(new AuxiliaryElements(meanState.getOrbit(), 1), PropagationType.OSCULATING, force.getParameters()));
         }
         dsstPropagator.setShortPeriodTerms(shortPeriodTerms);
     }
 
     /** Get the normalized state transition matrix (STM) from previous point to current point.
      * The STM contains the partial derivatives of current state with respect to previous state.
      * The  STM is an mxm matrix where m is the size of the state vector.
      * m = nbOrb + nbPropag + nbMeas
      * @return the normalized error state transition matrix
      */
     private RealMatrix getErrorStateTransitionMatrix() {
 
         /* The state transition matrix is obtained as follows, with:
          *  - Phi(k, k-1) : Transitional orbital matrix
          *  - Psi(k, k-1) : Transitional propagation parameters matrix
          *
          *       |             |             |   .    |
          *       | Phi(k, k-1) | Psi(k, k-1) | ..0..  |
          *       |             |             |   .    |
          *       |-------------|-------------|--------|
          *       |      .      |    1 0 0    |   .    |
          * STM = |    ..0..    |    0 1 0    | ..0..  |
          *       |      .      |    0 0 1    |   .    |
          *       |-------------|-------------|--------|
          *       |      .      |      .      | 1 0 0..|
          *       |    ..0..    |    ..0..    | 0 1 0..|
          *       |      .      |      .      | 0 0 1..|
          */
 
         // Initialize to the proper size identity matrix
         final RealMatrix stm = MatrixUtils.createRealIdentityMatrix(correctedEstimate.getState().getDimension());
 
         // Derivatives of the state vector with respect to initial state vector
         final int nbOrb = getNumberSelectedOrbitalDrivers();
         final RealMatrix dYdY0 = harvester.getB2(nominalMeanSpacecraftState);
 
         // Calculate transitional orbital matrix (See Ref [1], Eq. 3.4a)
         final RealMatrix phi = dYdY0.multiply(phiS);
 
         // Fill the state transition matrix with the orbital drivers
         final List<DelegatingDriver> drivers = builder.getOrbitalParametersDrivers().getDrivers();
         for (int i = 0; i < nbOrb; ++i) {
             if (drivers.get(i).isSelected()) {
                 int jOrb = 0;
                 for (int j = 0; j < nbOrb; ++j) {
                     if (drivers.get(j).isSelected()) {
                         stm.setEntry(i, jOrb++, phi.getEntry(i, j));
                     }
                 }
             }
         }
 
         // Update PhiS
         phiS = new QRDecomposition(dYdY0).getSolver().getInverse();
 
         // Derivatives of the state vector with respect to propagation parameters
         if (psiS != null) {
 
             final int nbProp = getNumberSelectedPropagationDrivers();
             final RealMatrix dYdPp = harvester.getB3(nominalMeanSpacecraftState);
 
             // Calculate transitional parameters matrix (See Ref [1], Eq. 3.4b)
             final RealMatrix psi = dYdPp.subtract(phi.multiply(psiS));
 
             // Fill 1st row, 2nd column (dY/dPp)
             for (int i = 0; i < nbOrb; ++i) {
                 for (int j = 0; j < nbProp; ++j) {
                     stm.setEntry(i, j + nbOrb, psi.getEntry(i, j));
                 }
             }
 
             // Update PsiS
             psiS = dYdPp;
 
         }
 
         // Normalization of the STM
         // normalized(STM)ij = STMij*Sj/Si
         for (int i = 0; i < scale.length; i++) {
             for (int j = 0; j < scale.length; j++ ) {
                 stm.setEntry(i, j, stm.getEntry(i, j) * scale[j] / scale[i]);
             }
         }
 
         // Return the error state transition matrix
         return stm;
 
     }
 
     /** Get the normalized measurement matrix H.
      * H contains the partial derivatives of the measurement with respect to the state.
      * H is an nxm matrix where n is the size of the measurement vector and m the size of the state vector.
      * @return the normalized measurement matrix H
      */
     private RealMatrix getMeasurementMatrix() {
 
         // Observed measurement characteristics
         final SpacecraftState        evaluationState     = predictedMeasurement.getStates()[0];
         final ObservedMeasurement<?> observedMeasurement = predictedMeasurement.getObservedMeasurement();
         final double[] sigma  = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();
 
         // Initialize measurement matrix H: nxm
         // n: Number of measurements in current measurement
         // m: State vector size
         final RealMatrix measurementMatrix = MatrixUtils.
                 createRealMatrix(observedMeasurement.getDimension(),
                                  correctedEstimate.getState().getDimension());
 
         // Predicted orbit
         final Orbit predictedOrbit = evaluationState.getOrbit();
 
         // Measurement matrix's columns related to orbital and propagation parameters
         // ----------------------------------------------------------
 
         // Partial derivatives of the current Cartesian coordinates with respect to current orbital state
         final int nbOrb  = getNumberSelectedOrbitalDrivers();
         final int nbProp = getNumberSelectedPropagationDrivers();
         final double[][] aCY = new double[nbOrb][nbOrb];
         predictedOrbit.getJacobianWrtParameters(builder.getPositionAngle(), aCY);
         final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
 
         // Jacobian of the measurement with respect to current Cartesian coordinates
         final RealMatrix dMdC = new Array2DRowRealMatrix(predictedMeasurement.getStateDerivatives(0), false);
 
         // Jacobian of the measurement with respect to current orbital state
         RealMatrix dMdY = dMdC.multiply(dCdY);
 
         // Compute factor dShortPeriod_dMeanState = I+B1 | B4
         final RealMatrix IpB1B4 = MatrixUtils.createRealMatrix(nbOrb, nbOrb + nbProp);
 
         // B1
         final RealMatrix B1 = harvester.getB1();
 
         // I + B1
         final RealMatrix I = MatrixUtils.createRealIdentityMatrix(nbOrb);
         final RealMatrix IpB1 = I.add(B1);
         IpB1B4.setSubMatrix(IpB1.getData(), 0, 0);
 
         // If there are not propagation parameters, B4 is null
         if (psiS != null) {
             final RealMatrix B4 = harvester.getB4();
             IpB1B4.setSubMatrix(B4.getData(), 0, nbOrb);
         }
 
         // Ref [1], Eq. 3.10
         dMdY = dMdY.multiply(IpB1B4);
 
         for (int i = 0; i < dMdY.getRowDimension(); i++) {
             for (int j = 0; j < nbOrb; j++) {
                 final double driverScale = builder.getOrbitalParametersDrivers().getDrivers().get(j).getScale();
                 measurementMatrix.setEntry(i, j, dMdY.getEntry(i, j) / sigma[i] * driverScale);
             }
             for (int j = 0; j < nbProp; j++) {
                 final double driverScale = estimatedPropagationParameters.getDrivers().get(j).getScale();
                 measurementMatrix.setEntry(i, j + nbOrb,
                                            dMdY.getEntry(i, j + nbOrb) / sigma[i] * driverScale);
             }
         }
 
         // Normalized measurement matrix's columns related to measurement parameters
         // --------------------------------------------------------------
 
         // Jacobian of the measurement with respect to measurement parameters
         // Gather the measurement parameters linked to current measurement
         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
             if (driver.isSelected()) {
                 // Derivatives of current measurement w/r to selected measurement parameter
                 final double[] aMPm = predictedMeasurement.getParameterDerivatives(driver);
 
                 // Check that the measurement parameter is managed by the filter
                 if (measurementParameterColumns.get(driver.getName()) != null) {
                     // Column of the driver in the measurement matrix
                     final int driverColumn = measurementParameterColumns.get(driver.getName());
 
                     // Fill the corresponding indexes of the measurement matrix
                     for (int i = 0; i < aMPm.length; ++i) {
                         measurementMatrix.setEntry(i, driverColumn, aMPm[i] / sigma[i] * driver.getScale());
                     }
                 }
             }
         }
 
         return measurementMatrix;
     }
 
     /** Predict the filter correction for the new observation.
      * @param stm normalized state transition matrix
      * @return the predicted filter correction for the new observation
      */
     private RealVector predictFilterCorrection(final RealMatrix stm) {
         // Ref [1], Eq. 3.5a and 3.5b
         return stm.operate(correctedFilterCorrection);
     }
 
     /** Compute the predicted osculating elements.
      * @param filterCorrection kalman filter correction
      * @return the predicted osculating element
      */
     private double[] computeOsculatingElements(final RealVector filterCorrection) {
 
         // Number of estimated orbital parameters
         final int nbOrb = getNumberSelectedOrbitalDrivers();
 
         // B1
         final RealMatrix B1 = harvester.getB1();
 
         // Short periodic terms
         final double[] shortPeriodTerms = dsstPropagator.getShortPeriodTermsValue(nominalMeanSpacecraftState);
 
         // Physical filter correction
         final RealVector physicalFilterCorrection = MatrixUtils.createRealVector(nbOrb);
         for (int index = 0; index < nbOrb; index++) {
             physicalFilterCorrection.addToEntry(index, filterCorrection.getEntry(index) * scale[index]);
         }
 
         // B1 * physicalCorrection
         final RealVector B1Correction = B1.operate(physicalFilterCorrection);
 
         // Nominal mean elements
         final double[] nominalMeanElements = new double[nbOrb];
         OrbitType.EQUINOCTIAL.mapOrbitToArray(nominalMeanSpacecraftState.getOrbit(), builder.getPositionAngle(), nominalMeanElements, null);
 
         // Ref [1] Eq. 3.6
         final double[] osculatingElements = new double[nbOrb];
         for (int i = 0; i < nbOrb; i++) {
             osculatingElements[i] = nominalMeanElements[i] +
                                     physicalFilterCorrection.getEntry(i) +
                                     shortPeriodTerms[i] +
                                     B1Correction.getEntry(i);
         }
 
         // Return
         return osculatingElements;
 
     }
 
     /** Analytical computation of derivatives.
      * This method allow to compute analytical derivatives.
      * @param state mean state used to calculate short period perturbations
      */
     private void analyticalDerivativeComputations(final SpacecraftState state) {
         harvester.setReferenceState(state);
     }
 
     /** Normalize a covariance matrix.
      * The covariance P is an mxm matrix where m = nbOrb + nbPropag + nbMeas
      * For each element [i,j] of P the corresponding normalized value is:
      * Pn[i,j] = P[i,j] / (scale[i]*scale[j])
      * @param physicalCovarianceMatrix The "physical" covariance matrix in input
      * @return the normalized covariance matrix
      */
     private RealMatrix normalizeCovarianceMatrix(final RealMatrix physicalCovarianceMatrix) {
 
         // Initialize output matrix
         final int nbParams = physicalCovarianceMatrix.getRowDimension();
         final RealMatrix normalizedCovarianceMatrix = MatrixUtils.createRealMatrix(nbParams, nbParams);
 
         // Normalize the state matrix
         for (int i = 0; i < nbParams; ++i) {
             for (int j = 0; j < nbParams; ++j) {
                 normalizedCovarianceMatrix.setEntry(i, j,
                                                     physicalCovarianceMatrix.getEntry(i, j) /
                                                     (scale[i] * scale[j]));
             }
         }
         return normalizedCovarianceMatrix;
     }
 
     /** Check dimension.
      * @param dimension dimension to check
      * @param orbitalParameters orbital parameters
      * @param propagationParameters propagation parameters
      * @param measurementParameters measurements parameters
      */
     private void checkDimension(final int dimension,
                                 final ParameterDriversList orbitalParameters,
                                 final ParameterDriversList propagationParameters,
                                 final ParameterDriversList measurementParameters) {
 
         // count parameters, taking care of counting all orbital parameters
         // regardless of them being estimated or not
         int requiredDimension = orbitalParameters.getNbParams();
         for (final ParameterDriver driver : propagationParameters.getDrivers()) {
             if (driver.isSelected()) {
                 ++requiredDimension;
             }
         }
         for (final ParameterDriver driver : measurementParameters.getDrivers()) {
             if (driver.isSelected()) {
                 ++requiredDimension;
             }
         }
 
         if (dimension != requiredDimension) {
             // there is a problem, set up an explicit error message
             final StringBuilder strBuilder = new StringBuilder();
             for (final ParameterDriver driver : orbitalParameters.getDrivers()) {
                 if (strBuilder.length() > 0) {
                     strBuilder.append(", ");
                 }
                 strBuilder.append(driver.getName());
             }
             for (final ParameterDriver driver : propagationParameters.getDrivers()) {
                 if (driver.isSelected()) {
                     strBuilder.append(driver.getName());
                 }
             }
             for (final ParameterDriver driver : measurementParameters.getDrivers()) {
                 if (driver.isSelected()) {
                     strBuilder.append(driver.getName());
                 }
             }
             throw new OrekitException(OrekitMessages.DIMENSION_INCONSISTENT_WITH_PARAMETERS,
                                       dimension, strBuilder.toString());
         }
 
     }
 
     /** Set and apply a dynamic outlier filter on a measurement.<p>
      * Loop on the modifiers to see if a dynamic outlier filter needs to be applied.<p>
      * Compute the sigma array using the matrix in input and set the filter.<p>
      * Apply the filter by calling the modify method on the estimated measurement.<p>
      * Reset the filter.
      * @param measurement measurement to filter
      * @param innovationCovarianceMatrix So called innovation covariance matrix S, with:<p>
      *        S = H.Ppred.Ht + R<p>
      *        Where:<p>
      *         - H is the normalized measurement matrix (Ht its transpose)<p>
      *         - Ppred is the normalized predicted covariance matrix<p>
      *         - R is the normalized measurement noise matrix
      * @param <T> the type of measurement
      */
     private <T extends ObservedMeasurement<T>> void applyDynamicOutlierFilter(final EstimatedMeasurement<T> measurement,
                                                                               final RealMatrix innovationCovarianceMatrix) {
 
         // Observed measurement associated to the predicted measurement
         final ObservedMeasurement<T> observedMeasurement = measurement.getObservedMeasurement();
 
         // Check if a dynamic filter was added to the measurement
         // If so, update its sigma value and apply it
         for (EstimationModifier<T> modifier : observedMeasurement.getModifiers()) {
             if (modifier instanceof DynamicOutlierFilter<?>) {
                 final DynamicOutlierFilter<T> dynamicOutlierFilter = (DynamicOutlierFilter<T>) modifier;
 
                 // Initialize the values of the sigma array used in the dynamic filter
                 final double[] sigmaDynamic     = new double[innovationCovarianceMatrix.getColumnDimension()];
                 final double[] sigmaMeasurement = observedMeasurement.getTheoreticalStandardDeviation();
 
                 // Set the sigma value for each element of the measurement
                 // Here we do use the value suggested by David A. Vallado (see [1]§10.6):
                 // sigmaDynamic[i] = sqrt(diag(S))*sigma[i]
                 // With S = H.Ppred.Ht + R
                 // Where:
                 //  - S is the measurement error matrix in input
                 //  - H is the normalized measurement matrix (Ht its transpose)
                 //  - Ppred is the normalized predicted covariance matrix
                 //  - R is the normalized measurement noise matrix
                 //  - sigma[i] is the theoretical standard deviation of the ith component of the measurement.
                 //    It is used here to un-normalize the value before it is filtered
                 for (int i = 0; i < sigmaDynamic.length; i++) {
                     sigmaDynamic[i] = FastMath.sqrt(innovationCovarianceMatrix.getEntry(i, i)) * sigmaMeasurement[i];
                 }
                 dynamicOutlierFilter.setSigma(sigmaDynamic);
 
                 // Apply the modifier on the estimated measurement
                 modifier.modify(measurement);
 
                 // Re-initialize the value of the filter for the next measurement of the same type
                 dynamicOutlierFilter.setSigma(null);
             }
         }
     }
 
     /** Get the number of estimated orbital parameters.
      * @return the number of estimated orbital parameters
      */
     private int getNumberSelectedOrbitalDrivers() {
         return estimatedOrbitalParameters.getNbParams();
     }
 
     /** Get the number of estimated propagation parameters.
      * @return the number of estimated propagation parameters
      */
     private int getNumberSelectedPropagationDrivers() {
         return estimatedPropagationParameters.getNbParams();
     }
 
     /** Get the number of estimated measurement parameters.
      * @return the number of estimated measurement parameters
      */
     private int getNumberSelectedMeasurementDrivers() {
         return estimatedMeasurementsParameters.getNbParams();
     }
 
     /** Update the estimated parameters after the correction phase of the filter.
      * The min/max allowed values are handled by the parameter themselves.
      */
     private void updateParameters() {
         final RealVector correctedState = correctedEstimate.getState();
         int i = 0;
         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
             // let the parameter handle min/max clipping
             driver.setNormalizedValue(driver.getNormalizedValue() + correctedState.getEntry(i++));
         }
         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
             // let the parameter handle min/max clipping
             driver.setNormalizedValue(driver.getNormalizedValue() + correctedState.getEntry(i++));
         }
         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
             // let the parameter handle min/max clipping
             driver.setNormalizedValue(driver.getNormalizedValue() + correctedState.getEntry(i++));
         }
     }
 
 }