AbstractKalmanEstimationCommon.java

  1. /* Copyright 2002-2025 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.estimation.sequential;

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

  26. import org.hipparchus.filtering.kalman.ProcessEstimate;
  27. import org.hipparchus.linear.ArrayRealVector;
  28. import org.hipparchus.linear.MatrixUtils;
  29. import org.hipparchus.linear.RealMatrix;
  30. import org.hipparchus.linear.RealVector;
  31. import org.orekit.errors.OrekitException;
  32. import org.orekit.errors.OrekitMessages;
  33. import org.orekit.estimation.measurements.EstimatedMeasurement;
  34. import org.orekit.propagation.Propagator;
  35. import org.orekit.propagation.SpacecraftState;
  36. import org.orekit.propagation.conversion.PropagatorBuilder;
  37. import org.orekit.time.AbsoluteDate;
  38. import org.orekit.utils.ParameterDriver;
  39. import org.orekit.utils.ParameterDriversList;
  40. import org.orekit.utils.ParameterDriversList.DelegatingDriver;

  42. /** Class defining the process model dynamics to use with a {@link KalmanEstimator}.
  43.  * @author Romain Gerbaud
  44.  * @author Maxime Journot
  45.  * @since 9.2
  46.  */
  47. abstract class AbstractKalmanEstimationCommon implements KalmanEstimation {

  49.     /** Builders for propagators. */
  50.     private final List<PropagatorBuilder> builders;

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

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

  58.     /** Per-builder estimated orbita parameters drivers.
  59.      * @since 11.1
  60.      */
  61.     private final ParameterDriversList[] estimatedOrbitalParameters;

  63.     /** Per-builder estimated propagation drivers. */
  64.     private final ParameterDriversList[] estimatedPropagationParameters;

  66.     /** Estimated measurements parameters. */
  67.     private final ParameterDriversList estimatedMeasurementsParameters;

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

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

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

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

  81.     /** Providers for covariance matrices. */
  82.     private final List<CovarianceMatrixProvider> covarianceMatricesProviders;

  84.     /** Process noise matrix provider for measurement parameters. */
  85.     private final CovarianceMatrixProvider measurementProcessNoiseMatrix;

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

  90.     /** Scaling factors. */
  91.     private final double[] scale;

  93.     /** Current corrected estimate. */
  94.     private ProcessEstimate correctedEstimate;

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

  99.     /** Reference date. */
  100.     private final AbsoluteDate referenceDate;

  102.     /** Current date. */
  103.     private AbsoluteDate currentDate;

  105.     /** Predicted spacecraft states. */
  106.     private final SpacecraftState[] predictedSpacecraftStates;

  108.     /** Corrected spacecraft states. */
  109.     private final SpacecraftState[] correctedSpacecraftStates;

  111.     /** Predicted measurement. */
  112.     private EstimatedMeasurement<?> predictedMeasurement;

  114.     /** Corrected measurement. */
  115.     private EstimatedMeasurement<?> correctedMeasurement;

  117.     /** Kalman process model constructor.
  118.      * @param propagatorBuilders propagators builders used to evaluate the orbits.
  119.      * @param covarianceMatricesProviders providers for covariance matrices
  120.      * @param estimatedMeasurementParameters measurement parameters to estimate
  121.      * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
  122.      */
  123.     protected AbstractKalmanEstimationCommon(final List<PropagatorBuilder> propagatorBuilders,
  124.                                              final List<CovarianceMatrixProvider> covarianceMatricesProviders,
  125.                                              final ParameterDriversList estimatedMeasurementParameters,
  126.                                              final CovarianceMatrixProvider measurementProcessNoiseMatrix) {

  128.         this.builders                        = propagatorBuilders;
  129.         this.estimatedMeasurementsParameters = estimatedMeasurementParameters;
  130.         this.measurementParameterColumns     = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
  131.         this.currentMeasurementNumber        = 0;
  132.         this.referenceDate                   = propagatorBuilders.get(0).getInitialOrbitDate();
  133.         this.currentDate                     = referenceDate;

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

  163.         // Gather all the propagation drivers names in a list
  164.         allEstimatedPropagationParameters = new ParameterDriversList();
  165.         estimatedPropagationParameters    = new ParameterDriversList[builders.size()];
  166.         final List<String> estimatedPropagationParametersNames = new ArrayList<>();
  167.         for (int k = 0; k < builders.size(); ++k) {
  168.             estimatedPropagationParameters[k] = new ParameterDriversList();
  169.             for (final ParameterDriver driver : builders.get(k).getPropagationParametersDrivers().getDrivers()) {
  170.                 if (driver.getReferenceDate() == null) {
  171.                     driver.setReferenceDate(currentDate);
  172.                 }
  173.                 if (driver.isSelected()) {
  174.                     allEstimatedPropagationParameters.add(driver);
  175.                     estimatedPropagationParameters[k].add(driver);
  176.                     final String driverName = driver.getName();
  177.                     // Add the driver name if it has not been added yet
  178.                     if (!estimatedPropagationParametersNames.contains(driverName)) {
  179.                         estimatedPropagationParametersNames.add(driverName);
  180.                     }
  181.                 }
  182.             }
  183.         }
  184.         estimatedPropagationParametersNames.sort(Comparator.naturalOrder());

  186.         // Populate the map of propagation drivers' columns and update the total number of columns
  187.         propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
  188.         for (final String driverName : estimatedPropagationParametersNames) {
  189.             propagationParameterColumns.put(driverName, columns);
  190.             ++columns;
  191.         }

  193.         // Populate the map of measurement drivers' columns and update the total number of columns
  194.         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
  195.             if (parameter.getReferenceDate() == null) {
  196.                 parameter.setReferenceDate(currentDate);
  197.             }
  198.             measurementParameterColumns.put(parameter.getName(), columns);
  199.             ++columns;
  200.         }

  202.         // Store providers for process noise matrices
  203.         this.covarianceMatricesProviders = covarianceMatricesProviders;
  204.         this.measurementProcessNoiseMatrix = measurementProcessNoiseMatrix;
  205.         this.covarianceIndirection       = new int[builders.size()][columns];
  206.         for (int k = 0; k < covarianceIndirection.length; ++k) {
  207.             final ParameterDriversList orbitDrivers      = builders.get(k).getOrbitalParametersDrivers();
  208.             final ParameterDriversList parametersDrivers = builders.get(k).getPropagationParametersDrivers();
  209.             Arrays.fill(covarianceIndirection[k], -1);
  210.             int i = 0;
  211.             for (final ParameterDriver driver : orbitDrivers.getDrivers()) {
  212.                 final Integer c = orbitalParameterColumns.get(driver.getName());
  213.                 if (c != null) {
  214.                     covarianceIndirection[k][i++] = c;
  215.                 }
  216.             }
  217.             for (final ParameterDriver driver : parametersDrivers.getDrivers()) {
  218.                 final Integer c = propagationParameterColumns.get(driver.getName());
  219.                 if (c != null) {
  220.                     covarianceIndirection[k][i++] = c;
  221.                 }
  222.             }
  223.             for (final ParameterDriver driver : estimatedMeasurementParameters.getDrivers()) {
  224.                 final Integer c = measurementParameterColumns.get(driver.getName());
  225.                 if (c != null) {
  226.                     covarianceIndirection[k][i++] = c;
  227.                 }
  228.             }
  229.         }

  231.         // Compute the scale factors
  232.         this.scale = new double[columns];
  233.         int index = 0;
  234.         for (final ParameterDriver driver : allEstimatedOrbitalParameters.getDrivers()) {
  235.             scale[index++] = driver.getScale();
  236.         }
  237.         for (final ParameterDriver driver : allEstimatedPropagationParameters.getDrivers()) {
  238.             scale[index++] = driver.getScale();
  239.         }
  240.         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
  241.             scale[index++] = driver.getScale();
  242.         }

  244.         // Populate predicted and corrected states
  245.         this.predictedSpacecraftStates = new SpacecraftState[builders.size()];
  246.         for (int i = 0; i < builders.size(); ++i) {
  247.             predictedSpacecraftStates[i] = builders.get(i).buildPropagator().getInitialState();
  248.         }
  249.         this.correctedSpacecraftStates = predictedSpacecraftStates.clone();

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

  254.         int p = 0;
  255.         for (final ParameterDriver driver : allEstimatedOrbitalParameters.getDrivers()) {
  256.             correctedState.setEntry(p++, driver.getNormalizedValue());
  257.         }
  258.         for (final ParameterDriver driver : allEstimatedPropagationParameters.getDrivers()) {
  259.             correctedState.setEntry(p++, driver.getNormalizedValue());
  260.         }
  261.         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
  262.             correctedState.setEntry(p++, driver.getNormalizedValue());
  263.         }

  265.         // Set up initial covariance
  266.         final RealMatrix physicalProcessNoise = MatrixUtils.createRealMatrix(columns, columns);
  267.         for (int k = 0; k < covarianceMatricesProviders.size(); ++k) {

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

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

  276.             // Covariance matrix
  277.             final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
  278.             if (nbDyn > 0) {
  279.                 final RealMatrix noiseP = covarianceMatricesProviders.get(k).
  280.                         getInitialCovarianceMatrix(correctedSpacecraftStates[k]);
  281.                 if (measurementProcessNoiseMatrix == null && noiseP.getRowDimension() != nbDyn + nbMeas) {
  282.                     throw new OrekitException(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION,
  283.                             nbDyn + nbMeas, noiseP.getRowDimension());
  284.                 } else if (measurementProcessNoiseMatrix != null && noiseP.getRowDimension() != nbDyn) {
  285.                     throw new OrekitException(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION,
  286.                             nbDyn, noiseP.getRowDimension());
  287.                 }
  288.                 noiseK.setSubMatrix(noiseP.getData(), 0, 0);
  289.             }
  290.             if (measurementProcessNoiseMatrix != null) {
  291.                 final RealMatrix noiseM = measurementProcessNoiseMatrix.
  292.                         getInitialCovarianceMatrix(correctedSpacecraftStates[k]);
  293.                 if (noiseM.getRowDimension() != nbMeas) {
  294.                     throw new OrekitException(OrekitMessages.WRONG_MEASUREMENT_COVARIANCE_DIMENSION,
  295.                             nbMeas, noiseM.getRowDimension());
  296.                 }
  297.                 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
  298.             }

  300.             KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
  301.                     builders.get(k).getOrbitalParametersDrivers(),
  302.                     builders.get(k).getPropagationParametersDrivers(),
  303.                     estimatedMeasurementsParameters);

  305.             final int[] indK = covarianceIndirection[k];
  306.             for (int i = 0; i < indK.length; ++i) {
  307.                 if (indK[i] >= 0) {
  308.                     for (int j = 0; j < indK.length; ++j) {
  309.                         if (indK[j] >= 0) {
  310.                             physicalProcessNoise.setEntry(indK[i], indK[j], noiseK.getEntry(i, j));
  311.                         }
  312.                     }
  313.                 }
  314.             }

  316.         }
  317.         final RealMatrix correctedCovariance = KalmanEstimatorUtil.normalizeCovarianceMatrix(physicalProcessNoise, scale);

  319.         correctedEstimate = new ProcessEstimate(0.0, correctedState, correctedCovariance);
  320.     }


  323.     /** {@inheritDoc} */
  324.     @Override
  325.     public RealMatrix getPhysicalStateTransitionMatrix() {
  326.         //  Un-normalize the state transition matrix (φ) from Hipparchus and return it.
  327.         // φ is an mxm matrix where m = nbOrb + nbPropag + nbMeas
  328.         // For each element [i,j] of normalized φ (φn), the corresponding physical value is:
  329.         // φ[i,j] = φn[i,j] * scale[i] / scale[j]
  330.         return correctedEstimate.getStateTransitionMatrix() == null ?
  331.                 null : KalmanEstimatorUtil.unnormalizeStateTransitionMatrix(correctedEstimate.getStateTransitionMatrix(), scale);
  332.     }

  334.     /** {@inheritDoc} */
  335.     @Override
  336.     public RealMatrix getPhysicalMeasurementJacobian() {
  337.         // Un-normalize the measurement matrix (H) from Hipparchus and return it.
  338.         // H is an nxm matrix where:
  339.         //  - m = nbOrb + nbPropag + nbMeas is the number of estimated parameters
  340.         //  - n is the size of the measurement being processed by the filter
  341.         // For each element [i,j] of normalized H (Hn) the corresponding physical value is:
  342.         // H[i,j] = Hn[i,j] * σ[i] / scale[j]
  343.         return correctedEstimate.getMeasurementJacobian() == null ?
  344.                 null : KalmanEstimatorUtil.unnormalizeMeasurementJacobian(correctedEstimate.getMeasurementJacobian(),
  345.                 scale,
  346.                 correctedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
  347.     }

  349.     /** {@inheritDoc} */
  350.     @Override
  351.     public RealMatrix getPhysicalInnovationCovarianceMatrix() {
  352.         // Un-normalize the innovation covariance matrix (S) from Hipparchus and return it.
  353.         // S is an nxn matrix where n is the size of the measurement being processed by the filter
  354.         // For each element [i,j] of normalized S (Sn) the corresponding physical value is:
  355.         // S[i,j] = Sn[i,j] * σ[i] * σ[j]
  356.         return correctedEstimate.getInnovationCovariance() == null ?
  357.                 null : KalmanEstimatorUtil.unnormalizeInnovationCovarianceMatrix(correctedEstimate.getInnovationCovariance(),
  358.                 predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
  359.     }

  361.     /** {@inheritDoc} */
  362.     @Override
  363.     public RealMatrix getPhysicalKalmanGain() {
  364.         // Un-normalize the Kalman gain (K) from Hipparchus and return it.
  365.         // K is an mxn matrix where:
  366.         //  - m = nbOrb + nbPropag + nbMeas is the number of estimated parameters
  367.         //  - n is the size of the measurement being processed by the filter
  368.         // For each element [i,j] of normalized K (Kn) the corresponding physical value is:
  369.         // K[i,j] = Kn[i,j] * scale[i] / σ[j]
  370.         return correctedEstimate.getKalmanGain() == null ?
  371.                 null : KalmanEstimatorUtil.unnormalizeKalmanGainMatrix(correctedEstimate.getKalmanGain(),
  372.                 scale,
  373.                 correctedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
  374.     }

  376.     /** {@inheritDoc} */
  377.     @Override
  378.     public SpacecraftState[] getPredictedSpacecraftStates() {
  379.         return predictedSpacecraftStates.clone();
  380.     }

  382.     /** {@inheritDoc} */
  383.     @Override
  384.     public SpacecraftState[] getCorrectedSpacecraftStates() {
  385.         return correctedSpacecraftStates.clone();
  386.     }

  388.     /** {@inheritDoc} */
  389.     @Override
  390.     public int getCurrentMeasurementNumber() {
  391.         return currentMeasurementNumber;
  392.     }

  394.     /** {@inheritDoc} */
  395.     @Override
  396.     public AbsoluteDate getCurrentDate() {
  397.         return currentDate;
  398.     }

  400.     /** {@inheritDoc} */
  401.     @Override
  402.     public EstimatedMeasurement<?> getPredictedMeasurement() {
  403.         return predictedMeasurement;
  404.     }

  406.     /** {@inheritDoc} */
  407.     @Override
  408.     public EstimatedMeasurement<?> getCorrectedMeasurement() {
  409.         return correctedMeasurement;
  410.     }

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

  430.         return physicalEstimatedState;
  431.     }

  433.     /** {@inheritDoc} */
  434.     @Override
  435.     public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
  436.         // Un-normalize the estimated covariance matrix (P) from Hipparchus and return it.
  437.         // The covariance P is an mxm matrix where m = nbOrb + nbPropag + nbMeas
  438.         // For each element [i,j] of P the corresponding normalized value is:
  439.         // Pn[i,j] = P[i,j] / (scale[i]*scale[j])
  440.         // Consequently: P[i,j] = Pn[i,j] * scale[i] * scale[j]
  441.         return KalmanEstimatorUtil.unnormalizeCovarianceMatrix(correctedEstimate.getCovariance(), scale);
  442.     }

  444.     /** {@inheritDoc} */
  445.     @Override
  446.     public ParameterDriversList getEstimatedOrbitalParameters() {
  447.         return allEstimatedOrbitalParameters;
  448.     }

  450.     /** {@inheritDoc} */
  451.     @Override
  452.     public ParameterDriversList getEstimatedPropagationParameters() {
  453.         return allEstimatedPropagationParameters;
  454.     }

  456.     /** {@inheritDoc} */
  457.     @Override
  458.     public ParameterDriversList getEstimatedMeasurementsParameters() {
  459.         return estimatedMeasurementsParameters;
  460.     }

  462.     /** Get the current corrected estimate.
  463.      * @return current corrected estimate
  464.      */
  465.     public ProcessEstimate getEstimate() {
  466.         return correctedEstimate;
  467.     }

  469.     /** Getter for the propagators.
  470.      * @return the propagators
  471.      */
  472.     public List<PropagatorBuilder> getBuilders() {
  473.         return builders;
  474.     }

  476.     /** Get the propagators estimated with the values set in the propagators builders.
  477.      * @return propagators based on the current values in the builder
  478.      */
  479.     public Propagator[] getEstimatedPropagators() {
  480.         // Return propagators built with current instantiation of the propagator builders
  481.         final Propagator[] propagators = new Propagator[getBuilders().size()];
  482.         for (int k = 0; k < getBuilders().size(); ++k) {
  483.             propagators[k] = getBuilders().get(k).buildPropagator();
  484.         }
  485.         return propagators;
  486.     }

  488.     /** Get the normalized process noise matrix.
  489.      *
  490.      * @param stateDimension state dimension
  491.      * @return the normalized process noise matrix
  492.      */
  493.     protected RealMatrix getNormalizedProcessNoise(final int stateDimension) {
  494.         final RealMatrix physicalProcessNoise = MatrixUtils.createRealMatrix(stateDimension, stateDimension);
  495.         for (int k = 0; k < covarianceMatricesProviders.size(); ++k) {

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

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

  504.             // Covariance matrix
  505.             final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
  506.             if (nbDyn > 0) {
  507.                 final RealMatrix noiseP = covarianceMatricesProviders.get(k).
  508.                         getProcessNoiseMatrix(correctedSpacecraftStates[k],
  509.                                 predictedSpacecraftStates[k]);
  510.                 if (measurementProcessNoiseMatrix == null && noiseP.getRowDimension() != nbDyn + nbMeas) {
  511.                     throw new OrekitException(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION,
  512.                             nbDyn + nbMeas, noiseP.getRowDimension());
  513.                 } else if (measurementProcessNoiseMatrix != null && noiseP.getRowDimension() != nbDyn) {
  514.                     throw new OrekitException(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION,
  515.                             nbDyn, noiseP.getRowDimension());
  516.                 }
  517.                 noiseK.setSubMatrix(noiseP.getData(), 0, 0);
  518.             }
  519.             if (measurementProcessNoiseMatrix != null) {
  520.                 final RealMatrix noiseM = measurementProcessNoiseMatrix.
  521.                         getProcessNoiseMatrix(correctedSpacecraftStates[k],
  522.                                 predictedSpacecraftStates[k]);
  523.                 if (noiseM.getRowDimension() != nbMeas) {
  524.                     throw new OrekitException(OrekitMessages.WRONG_MEASUREMENT_COVARIANCE_DIMENSION,
  525.                             nbMeas, noiseM.getRowDimension());
  526.                 }
  527.                 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
  528.             }

  530.             KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
  531.                     builders.get(k).getOrbitalParametersDrivers(),
  532.                     builders.get(k).getPropagationParametersDrivers(),
  533.                     estimatedMeasurementsParameters);

  535.             final int[] indK = covarianceIndirection[k];
  536.             for (int i = 0; i < indK.length; ++i) {
  537.                 if (indK[i] >= 0) {
  538.                     for (int j = 0; j < indK.length; ++j) {
  539.                         if (indK[j] >= 0) {
  540.                             physicalProcessNoise.setEntry(indK[i], indK[j], noiseK.getEntry(i, j));
  541.                         }
  542.                     }
  543.                 }
  544.             }

  546.         }
  547.         return KalmanEstimatorUtil.normalizeCovarianceMatrix(physicalProcessNoise, scale);
  548.     }

  550.     /** Getter for the orbitsStartColumns.
  551.      * @return the orbitsStartColumns
  552.      */
  553.     protected int[] getOrbitsStartColumns() {
  554.         return orbitsStartColumns;
  555.     }

  557.     /** Getter for the propagationParameterColumns.
  558.      * @return the propagationParameterColumns
  559.      */
  560.     protected Map<String, Integer> getPropagationParameterColumns() {
  561.         return propagationParameterColumns;
  562.     }

  564.     /** Getter for the measurementParameterColumns.
  565.      * @return the measurementParameterColumns
  566.      */
  567.     protected Map<String, Integer> getMeasurementParameterColumns() {
  568.         return measurementParameterColumns;
  569.     }

  571.     /** Getter for the estimatedPropagationParameters.
  572.      * @return the estimatedPropagationParameters
  573.      */
  574.     protected ParameterDriversList[] getEstimatedPropagationParametersArray() {
  575.         return estimatedPropagationParameters;
  576.     }

  578.     /** Getter for the estimatedOrbitalParameters.
  579.      * @return the estimatedOrbitalParameters
  580.      */
  581.     protected ParameterDriversList[] getEstimatedOrbitalParametersArray() {
  582.         return estimatedOrbitalParameters;
  583.     }

  585.     /** Getter for the covarianceIndirection.
  586.      * @return the covarianceIndirection
  587.      */
  588.     protected int[][] getCovarianceIndirection() {
  589.         return covarianceIndirection;
  590.     }

  592.     /** Getter for the scale.
  593.      * @return the scale
  594.      */
  595.     protected double[] getScale() {
  596.         return scale;
  597.     }

  599.     /** Getter for the correctedEstimate.
  600.      * @return the correctedEstimate
  601.      */
  602.     protected ProcessEstimate getCorrectedEstimate() {
  603.         return correctedEstimate;
  604.     }

  606.     /** Setter for the correctedEstimate.
  607.      * @param correctedEstimate the correctedEstimate
  608.      */
  609.     protected void setCorrectedEstimate(final ProcessEstimate correctedEstimate) {
  610.         this.correctedEstimate = correctedEstimate;
  611.     }

  613.     /** Getter for the referenceDate.
  614.      * @return the referenceDate
  615.      */
  616.     protected AbsoluteDate getReferenceDate() {
  617.         return referenceDate;
  618.     }

  620.     /** Increment current measurement number. */
  621.     protected void incrementCurrentMeasurementNumber() {
  622.         currentMeasurementNumber += 1;
  623.     }

  625.     /** Setter for the currentDate.
  626.      * @param currentDate the currentDate
  627.      */
  628.     protected void setCurrentDate(final AbsoluteDate currentDate) {
  629.         this.currentDate = currentDate;
  630.     }

  632.     /** Set correctedSpacecraftState at index.
  633.      *
  634.      * @param correctedSpacecraftState corrected S/C state o set
  635.      * @param index index where to set in the array
  636.      */
  637.     protected void setCorrectedSpacecraftState(final SpacecraftState correctedSpacecraftState, final int index) {
  638.         this.correctedSpacecraftStates[index] = correctedSpacecraftState;
  639.     }

  641.     /** Set predictedSpacecraftState at index.
  642.      *
  643.      * @param predictedSpacecraftState predicted S/C state o set
  644.      * @param index index where to set in the array
  645.      */
  646.     protected void setPredictedSpacecraftState(final SpacecraftState predictedSpacecraftState, final int index) {
  647.         this.predictedSpacecraftStates[index] = predictedSpacecraftState;
  648.     }

  650.     /** Setter for the predictedMeasurement.
  651.      * @param predictedMeasurement the predictedMeasurement
  652.      */
  653.     protected void setPredictedMeasurement(final EstimatedMeasurement<?> predictedMeasurement) {
  654.         this.predictedMeasurement = predictedMeasurement;
  655.     }

  657.     /** Setter for the correctedMeasurement.
  658.      * @param correctedMeasurement the correctedMeasurement
  659.      */
  660.     protected void setCorrectedMeasurement(final EstimatedMeasurement<?> correctedMeasurement) {
  661.         this.correctedMeasurement = correctedMeasurement;
  662.     }
  663. }
Created with JaCoCo 0.8.12.202403310830