KalmanEstimatorUtil.java

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

  18. import java.util.Arrays;
  19. import java.util.List;

  20. import org.hipparchus.linear.MatrixUtils;
  21. import org.hipparchus.linear.RealMatrix;
  22. import org.hipparchus.linear.RealVector;
  23. import org.hipparchus.util.FastMath;
  24. import org.orekit.errors.OrekitException;
  25. import org.orekit.errors.OrekitMessages;
  26. import org.orekit.estimation.measurements.EstimatedMeasurement;
  27. import org.orekit.estimation.measurements.EstimationModifier;
  28. import org.orekit.estimation.measurements.ObservableSatellite;
  29. import org.orekit.estimation.measurements.ObservedMeasurement;
  30. import org.orekit.estimation.measurements.PV;
  31. import org.orekit.estimation.measurements.Position;
  32. import org.orekit.estimation.measurements.modifiers.DynamicOutlierFilter;
  33. import org.orekit.propagation.SpacecraftState;
  34. import org.orekit.time.AbsoluteDate;
  35. import org.orekit.utils.ParameterDriver;
  36. import org.orekit.utils.ParameterDriversList;

  37. /**
  38.  * Utility class for Kalman Filter.
  39.  * <p>
  40.  * This class includes common methods used by the different Kalman
  41.  * models in Orekit (i.e., Extended, Unscented, and Semi-analytical)
  42.  * </p>
  43.  * @since 11.3
  44.  */
  45. public class KalmanEstimatorUtil {

  46.     /** Private constructor.
  47.      * <p>This class is a utility class, it should neither have a public
  48.      * nor a default constructor. This private constructor prevents
  49.      * the compiler from generating one automatically.</p>
  50.      */
  51.     private KalmanEstimatorUtil() {
  52.     }

  53.     /** Decorate an observed measurement.
  54.      * <p>
  55.      * The "physical" measurement noise matrix is the covariance matrix of the measurement.
  56.      * Normalizing it consists in applying the following equation: Rn[i,j] =  R[i,j]/σ[i]/σ[j]
  57.      * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
  58.      * between the different components of the measurement.
  59.      * </p>
  60.      * @param observedMeasurement the measurement
  61.      * @param referenceDate reference date
  62.      * @return decorated measurement
  63.      */
  64.     public static MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement,
  65.                                                 final AbsoluteDate referenceDate) {

  66.         // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
  67.         // of the measurement on its non-diagonal elements.
  68.         // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
  69.         // Normalizing it leaves us with the matrix of the correlation coefficients
  70.         final RealMatrix covariance;
  71.         if (observedMeasurement.getMeasurementType().equals(PV.MEASUREMENT_TYPE)) {
  72.             // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
  73.             final PV pv = (PV) observedMeasurement;
  74.             covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
  75.         } else if (observedMeasurement.getMeasurementType().equals(Position.MEASUREMENT_TYPE)) {
  76.             // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
  77.             final Position position = (Position) observedMeasurement;
  78.             covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
  79.         } else {
  80.             // For other measurements we do not have a covariance matrix.
  81.             // Thus the correlation coefficients matrix is an identity matrix.
  82.             covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
  83.         }

  84.         return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);

  85.     }

  86.     /** Decorate an observed measurement for an Unscented Kalman Filter.
  87.      * <p>
  88.      * This method uses directly the measurement's covariance matrix, without any normalization.
  89.      * </p>
  90.      * @param observedMeasurement the measurement
  91.      * @param referenceDate reference date
  92.      * @return decorated measurement
  93.      * @since 11.3.2
  94.      */
  95.     public static MeasurementDecorator decorateUnscented(final ObservedMeasurement<?> observedMeasurement,
  96.                                                          final AbsoluteDate referenceDate) {

  97.         // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
  98.         // of the measurement on its non-diagonal elements.
  99.         // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement

  100.         final RealMatrix covariance;
  101.         if (observedMeasurement.getMeasurementType().equals(PV.MEASUREMENT_TYPE)) {
  102.             // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
  103.             final PV pv = (PV) observedMeasurement;
  104.             covariance = MatrixUtils.createRealMatrix(pv.getCovarianceMatrix());
  105.         } else if (observedMeasurement.getMeasurementType().equals(Position.MEASUREMENT_TYPE)) {
  106.             // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
  107.             final Position position = (Position) observedMeasurement;
  108.             covariance = MatrixUtils.createRealMatrix(position.getCovarianceMatrix());
  109.         } else {
  110.             // For other measurements we do not have a covariance matrix.
  111.             // Thus the correlation coefficients matrix is an identity matrix.
  112.             covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
  113.             final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
  114.             for (int i = 0; i < sigma.length; i++) {
  115.                 covariance.setEntry(i, i, sigma[i] * sigma[i]);
  116.             }

  117.         }

  118.         return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);

  119.     }

  120.     /** Check dimension.
  121.      * @param dimension dimension to check
  122.      * @param orbitalParameters orbital parameters
  123.      * @param propagationParameters propagation parameters
  124.      * @param measurementParameters measurements parameters
  125.      */
  126.     public static void checkDimension(final int dimension,
  127.                                       final ParameterDriversList orbitalParameters,
  128.                                       final ParameterDriversList propagationParameters,
  129.                                       final ParameterDriversList measurementParameters) {

  130.         // count parameters
  131.         int requiredDimension = 0;
  132.         for (final ParameterDriver driver : orbitalParameters.getDrivers()) {
  133.             if (driver.isSelected()) {
  134.                 ++requiredDimension;
  135.             }
  136.         }
  137.         for (final ParameterDriver driver : propagationParameters.getDrivers()) {
  138.             if (driver.isSelected()) {
  139.                 ++requiredDimension;
  140.             }
  141.         }
  142.         for (final ParameterDriver driver : measurementParameters.getDrivers()) {
  143.             if (driver.isSelected()) {
  144.                 ++requiredDimension;
  145.             }
  146.         }

  147.         if (dimension != requiredDimension) {
  148.             // there is a problem, set up an explicit error message
  149.             final StringBuilder sBuilder = new StringBuilder();
  150.             for (final ParameterDriver driver : orbitalParameters.getDrivers()) {
  151.                 if (sBuilder.length() > 0) {
  152.                     sBuilder.append(", ");
  153.                 }
  154.                 sBuilder.append(driver.getName());
  155.             }
  156.             for (final ParameterDriver driver : propagationParameters.getDrivers()) {
  157.                 if (driver.isSelected()) {
  158.                     sBuilder.append(driver.getName());
  159.                 }
  160.             }
  161.             for (final ParameterDriver driver : measurementParameters.getDrivers()) {
  162.                 if (driver.isSelected()) {
  163.                     sBuilder.append(driver.getName());
  164.                 }
  165.             }
  166.             throw new OrekitException(OrekitMessages.DIMENSION_INCONSISTENT_WITH_PARAMETERS,
  167.                                       dimension, sBuilder.toString());
  168.         }

  169.     }

  170.     /** Filter relevant states for a measurement.
  171.      * @param observedMeasurement measurement to consider
  172.      * @param allStates all states
  173.      * @return array containing only the states relevant to the measurement
  174.      */
  175.     public static SpacecraftState[] filterRelevant(final ObservedMeasurement<?> observedMeasurement,
  176.                                                    final SpacecraftState[] allStates) {
  177.         final List<ObservableSatellite> satellites = observedMeasurement.getSatellites();
  178.         final SpacecraftState[] relevantStates = new SpacecraftState[satellites.size()];
  179.         for (int i = 0; i < relevantStates.length; ++i) {
  180.             relevantStates[i] = allStates[satellites.get(i).getPropagatorIndex()];
  181.         }
  182.         return relevantStates;
  183.     }

  184.     /** Set and apply a dynamic outlier filter on a measurement.<p>
  185.      * Loop on the modifiers to see if a dynamic outlier filter needs to be applied.<p>
  186.      * Compute the sigma array using the matrix in input and set the filter.<p>
  187.      * Apply the filter by calling the modify method on the estimated measurement.<p>
  188.      * Reset the filter.
  189.      * @param measurement measurement to filter
  190.      * @param innovationCovarianceMatrix So called innovation covariance matrix S, with:<p>
  191.      *        S = H.Ppred.Ht + R<p>
  192.      *        Where:<p>
  193.      *         - H is the normalized measurement matrix (Ht its transpose)<p>
  194.      *         - Ppred is the normalized predicted covariance matrix<p>
  195.      *         - R is the normalized measurement noise matrix
  196.      * @param <T> the type of measurement
  197.      */
  198.     public static <T extends ObservedMeasurement<T>> void applyDynamicOutlierFilter(final EstimatedMeasurement<T> measurement,
  199.                                                                                     final RealMatrix innovationCovarianceMatrix) {

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

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

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

  210.                 // Set the sigma value for each element of the measurement
  211.                 // Here we do use the value suggested by David A. Vallado (see [1]§10.6):
  212.                 // sigmaDynamic[i] = sqrt(diag(S))*sigma[i]
  213.                 // With S = H.Ppred.Ht + R
  214.                 // Where:
  215.                 //  - S is the measurement error matrix in input
  216.                 //  - H is the normalized measurement matrix (Ht its transpose)
  217.                 //  - Ppred is the normalized predicted covariance matrix
  218.                 //  - R is the normalized measurement noise matrix
  219.                 //  - sigma[i] is the theoretical standard deviation of the ith component of the measurement.
  220.                 //    It is used here to un-normalize the value before it is filtered
  221.                 for (int i = 0; i < sigmaDynamic.length; i++) {
  222.                     sigmaDynamic[i] = FastMath.sqrt(innovationCovarianceMatrix.getEntry(i, i)) * sigmaMeasurement[i];
  223.                 }
  224.                 dynamicOutlierFilter.setSigma(sigmaDynamic);

  225.                 // Apply the modifier on the estimated measurement
  226.                 modifier.modify(measurement);

  227.                 // Re-initialize the value of the filter for the next measurement of the same type
  228.                 dynamicOutlierFilter.setSigma(null);
  229.             }
  230.         }
  231.     }

  232.     /**
  233.      * Compute the unnormalized innovation vector from the given predicted measurement.
  234.      * @param predicted predicted measurement
  235.      * @return the innovation vector
  236.      */
  237.     public static RealVector computeInnovationVector(final EstimatedMeasurement<?> predicted) {
  238.         final double[] sigmas = new double[predicted.getObservedMeasurement().getDimension()];
  239.         Arrays.fill(sigmas, 1.0);
  240.         return computeInnovationVector(predicted, sigmas);
  241.     }

  242.     /**
  243.      * Compute the normalized innovation vector from the given predicted measurement.
  244.      * @param predicted predicted measurement
  245.      * @param sigma measurement standard deviation
  246.      * @return the innovation vector
  247.      */
  248.     public static RealVector computeInnovationVector(final EstimatedMeasurement<?> predicted, final double[] sigma) {

  249.         if (predicted.getStatus() == EstimatedMeasurement.Status.REJECTED)  {
  250.             // set innovation to null to notify filter measurement is rejected
  251.             return null;
  252.         } else {
  253.             // Normalized innovation of the measurement (Nx1)
  254.             final double[] observed  = predicted.getObservedMeasurement().getObservedValue();
  255.             final double[] estimated = predicted.getEstimatedValue();
  256.             final double[] residuals = new double[observed.length];

  257.             for (int i = 0; i < observed.length; i++) {
  258.                 residuals[i] = (observed[i] - estimated[i]) / sigma[i];
  259.             }
  260.             return MatrixUtils.createRealVector(residuals);
  261.         }

  262.     }

  263.     /**
  264.      * Normalize a covariance matrix.
  265.      * @param physicalP "physical" covariance matrix in input
  266.      * @param parameterScales scale factor of estimated parameters
  267.      * @return the normalized covariance matrix
  268.      */
  269.     public static RealMatrix normalizeCovarianceMatrix(final RealMatrix physicalP,
  270.                                                        final double[] parameterScales) {

  271.         // Initialize output matrix
  272.         final int nbParams = physicalP.getRowDimension();
  273.         final RealMatrix normalizedP = MatrixUtils.createRealMatrix(nbParams, nbParams);

  274.         // Normalize the state matrix
  275.         for (int i = 0; i < nbParams; ++i) {
  276.             for (int j = 0; j < nbParams; ++j) {
  277.                 normalizedP.setEntry(i, j, physicalP.getEntry(i, j) / (parameterScales[i] * parameterScales[j]));
  278.             }
  279.         }
  280.         return normalizedP;
  281.     }

  282.     /**
  283.      * Un-nomalized the covariance matrix.
  284.      * @param normalizedP normalized covariance matrix
  285.      * @param parameterScales scale factor of estimated parameters
  286.      * @return the un-normalized covariance matrix
  287.      */
  288.     public static RealMatrix unnormalizeCovarianceMatrix(final RealMatrix normalizedP,
  289.                                                          final double[] parameterScales) {
  290.         // Initialize physical covariance matrix
  291.         final int nbParams = normalizedP.getRowDimension();
  292.         final RealMatrix physicalP = MatrixUtils.createRealMatrix(nbParams, nbParams);

  293.         // Un-normalize the covairance matrix
  294.         for (int i = 0; i < nbParams; ++i) {
  295.             for (int j = 0; j < nbParams; ++j) {
  296.                 physicalP.setEntry(i, j, normalizedP.getEntry(i, j) * parameterScales[i] * parameterScales[j]);
  297.             }
  298.         }
  299.         return physicalP;
  300.     }

  301.     /**
  302.      * Un-nomalized the state transition matrix.
  303.      * @param normalizedSTM normalized state transition matrix
  304.      * @param parameterScales scale factor of estimated parameters
  305.      * @return the un-normalized state transition matrix
  306.      */
  307.     public static RealMatrix unnormalizeStateTransitionMatrix(final RealMatrix normalizedSTM,
  308.                                                               final double[] parameterScales) {
  309.         // Initialize physical matrix
  310.         final int nbParams = normalizedSTM.getRowDimension();
  311.         final RealMatrix physicalSTM = MatrixUtils.createRealMatrix(nbParams, nbParams);

  312.         // Un-normalize the matrix
  313.         for (int i = 0; i < nbParams; ++i) {
  314.             for (int j = 0; j < nbParams; ++j) {
  315.                 physicalSTM.setEntry(i, j,
  316.                         normalizedSTM.getEntry(i, j) * parameterScales[i] / parameterScales[j]);
  317.             }
  318.         }
  319.         return physicalSTM;
  320.     }

  321.     /**
  322.      * Un-normalize the measurement matrix.
  323.      * @param normalizedH normalized measurement matrix
  324.      * @param parameterScales scale factor of estimated parameters
  325.      * @param sigmas measurement theoretical standard deviation
  326.      * @return the un-normalized measurement matrix
  327.      */
  328.     public static RealMatrix unnormalizeMeasurementJacobian(final RealMatrix normalizedH,
  329.                                                             final double[] parameterScales,
  330.                                                             final double[] sigmas) {
  331.         // Initialize physical matrix
  332.         final int nbLine = normalizedH.getRowDimension();
  333.         final int nbCol  = normalizedH.getColumnDimension();
  334.         final RealMatrix physicalH = MatrixUtils.createRealMatrix(nbLine, nbCol);

  335.         // Un-normalize the matrix
  336.         for (int i = 0; i < nbLine; ++i) {
  337.             for (int j = 0; j < nbCol; ++j) {
  338.                 physicalH.setEntry(i, j, normalizedH.getEntry(i, j) * sigmas[i] / parameterScales[j]);
  339.             }
  340.         }
  341.         return physicalH;
  342.     }

  343.     /**
  344.      * Un-normalize the innovation covariance matrix.
  345.      * @param normalizedS normalized innovation covariance matrix
  346.      * @param sigmas measurement theoretical standard deviation
  347.      * @return the un-normalized innovation covariance matrix
  348.      */
  349.     public static RealMatrix unnormalizeInnovationCovarianceMatrix(final RealMatrix normalizedS,
  350.                                                                    final double[] sigmas) {
  351.         // Initialize physical matrix
  352.         final int nbMeas = sigmas.length;
  353.         final RealMatrix physicalS = MatrixUtils.createRealMatrix(nbMeas, nbMeas);

  354.         // Un-normalize the matrix
  355.         for (int i = 0; i < nbMeas; ++i) {
  356.             for (int j = 0; j < nbMeas; ++j) {
  357.                 physicalS.setEntry(i, j, normalizedS.getEntry(i, j) * sigmas[i] *   sigmas[j]);
  358.             }
  359.         }
  360.         return physicalS;
  361.     }

  362.     /**
  363.      * Un-normalize the Kalman gain matrix.
  364.      * @param normalizedK normalized Kalman gain matrix
  365.      * @param parameterScales scale factor of estimated parameters
  366.      * @param sigmas measurement theoretical standard deviation
  367.      * @return the un-normalized Kalman gain matrix
  368.      */
  369.     public static RealMatrix unnormalizeKalmanGainMatrix(final RealMatrix normalizedK,
  370.                                                          final double[] parameterScales,
  371.                                                          final double[] sigmas) {
  372.         // Initialize physical matrix
  373.         final int nbLine = normalizedK.getRowDimension();
  374.         final int nbCol  = normalizedK.getColumnDimension();
  375.         final RealMatrix physicalK = MatrixUtils.createRealMatrix(nbLine, nbCol);

  376.         // Un-normalize the matrix
  377.         for (int i = 0; i < nbLine; ++i) {
  378.             for (int j = 0; j < nbCol; ++j) {
  379.                 physicalK.setEntry(i, j, normalizedK.getEntry(i, j) * parameterScales[i] / sigmas[j]);
  380.             }
  381.         }
  382.         return physicalK;
  383.     }

  384. }