KalmanEstimator.java

  1. /* Copyright 2002-2020 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.List;

  21. import org.hipparchus.exception.MathRuntimeException;
  22. import org.hipparchus.filtering.kalman.ProcessEstimate;
  23. import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
  24. import org.hipparchus.linear.MatrixDecomposer;
  25. import org.hipparchus.linear.MatrixUtils;
  26. import org.hipparchus.linear.RealMatrix;
  27. import org.hipparchus.linear.RealVector;
  28. import org.orekit.errors.OrekitException;
  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.propagation.conversion.IntegratedPropagatorBuilder;
  33. import org.orekit.propagation.conversion.PropagatorBuilder;
  34. import org.orekit.propagation.integration.AbstractIntegratedPropagator;
  35. import org.orekit.propagation.numerical.NumericalPropagator;
  36. import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
  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;


  43. /**
  44.  * Implementation of a Kalman filter to perform orbit determination.
  45.  * <p>
  46.  * The filter uses a {@link IntegratedPropagatorBuilder} to initialize its reference trajectory {@link NumericalPropagator}
  47.  * or {@link DSSTPropagator} .
  48.  * </p>
  49.  * <p>
  50.  * The estimated parameters are driven by {@link ParameterDriver} objects. They are of 3 different types:<ol>
  51.  *   <li><b>Orbital parameters</b>:The position and velocity of the spacecraft, or, more generally, its orbit.<br>
  52.  *       These parameters are retrieved from the reference trajectory propagator builder when the filter is initialized.</li>
  53.  *   <li><b>Propagation parameters</b>: Some parameters modelling physical processes (SRP or drag coefficients etc...).<br>
  54.  *       They are also retrieved from the propagator builder during the initialization phase.</li>
  55.  *   <li><b>Measurements parameters</b>: Parameters related to measurements (station biases, positions etc...).<br>
  56.  *       They are passed down to the filter in its constructor.</li>
  57.  * </ol>
  58.  * <p>
  59.  * The total number of estimated parameters is m, the size of the state vector.
  60.  * </p>
  61.  * <p>
  62.  * The Kalman filter implementation used is provided by the underlying mathematical library Hipparchus.
  63.  * All the variables seen by Hipparchus (states, covariances, measurement matrices...) are normalized
  64.  * using a specific scale for each estimated parameters or standard deviation noise for each measurement components.
  65.  * </p>
  66.  *
  67.  * <p>A {@link KalmanEstimator} object is built using the {@link KalmanEstimatorBuilder#build() build}
  68.  * method of a {@link KalmanEstimatorBuilder}.</p>
  69.  *
  70.  * @author Romain Gerbaud
  71.  * @author Maxime Journot
  72.  * @author Luc Maisonobe
  73.  * @since 9.2
  74.  */
  75. public class KalmanEstimator {

  77.     /** Builders for orbit propagators. */
  78.     private List<IntegratedPropagatorBuilder> propagatorBuilders;

  80.     /** Reference date. */
  81.     private final AbsoluteDate referenceDate;

  83.     /** Kalman filter process model. */
  84.     private final KalmanODModel processModel;

  86.     /** Filter. */
  87.     private final ExtendedKalmanFilter<MeasurementDecorator> filter;

  89.     /** Observer to retrieve current estimation info. */
  90.     private KalmanObserver observer;

  92.     /** Kalman filter estimator constructor (package private).
  93.      * @param decomposer decomposer to use for the correction phase
  94.      * @param propagatorBuilders propagators builders used to evaluate the orbit.
  95.      * @param processNoiseMatricesProviders providers for process noise matrices
  96.      * @param estimatedMeasurementParameters measurement parameters to estimate
  97.      * @deprecated since 10.3, replaced by
  98.      * {@link #KalmanEstimator(MatrixDecomposer, List, List, ParameterDriversList, CovarianceMatrixProvider)}
  99.      */
  100.     @Deprecated
  101.     KalmanEstimator(final MatrixDecomposer decomposer,
  102.                     final List<IntegratedPropagatorBuilder> propagatorBuilders,
  103.                     final List<CovarianceMatrixProvider> processNoiseMatricesProviders,
  104.                     final ParameterDriversList estimatedMeasurementParameters) {
  105.         this(decomposer, propagatorBuilders, processNoiseMatricesProviders,
  106.              estimatedMeasurementParameters, null);
  107.     }

  109.     /** Kalman filter estimator constructor (package private).
  110.      * @param decomposer decomposer to use for the correction phase
  111.      * @param propagatorBuilders propagators builders used to evaluate the orbit.
  112.      * @param processNoiseMatricesProviders providers for process noise matrices
  113.      * @param estimatedMeasurementParameters measurement parameters to estimate
  114.      * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
  115.      * @since 10.3
  116.      */
  117.     KalmanEstimator(final MatrixDecomposer decomposer,
  118.                     final List<IntegratedPropagatorBuilder> propagatorBuilders,
  119.                     final List<CovarianceMatrixProvider> processNoiseMatricesProviders,
  120.                     final ParameterDriversList estimatedMeasurementParameters,
  121.                     final CovarianceMatrixProvider measurementProcessNoiseMatrix) {

  123.         this.propagatorBuilders = propagatorBuilders;
  124.         this.referenceDate      = propagatorBuilders.get(0).getInitialOrbitDate();
  125.         this.observer           = null;

  127.         // Build the process model and measurement model
  128.         this.processModel = propagatorBuilders.get(0).buildKalmanModel(propagatorBuilders,
  129.                                                                    processNoiseMatricesProviders,
  130.                                                                    estimatedMeasurementParameters,
  131.                                                                    measurementProcessNoiseMatrix);

  133.         this.filter = new ExtendedKalmanFilter<>(decomposer, processModel, processModel.getEstimate());

  135.     }

  137.     /** Set the observer.
  138.      * @param observer the observer
  139.      */
  140.     public void setObserver(final KalmanObserver observer) {
  141.         this.observer = observer;
  142.     }

  144.     /** Get the current measurement number.
  145.      * @return current measurement number
  146.      */
  147.     public int getCurrentMeasurementNumber() {
  148.         return processModel.getCurrentMeasurementNumber();
  149.     }

  151.     /** Get the current date.
  152.      * @return current date
  153.      */
  154.     public AbsoluteDate getCurrentDate() {
  155.         return processModel.getCurrentDate();
  156.     }

  158.     /** Get the "physical" estimated state (i.e. not normalized)
  159.      * @return the "physical" estimated state
  160.      */
  161.     public RealVector getPhysicalEstimatedState() {
  162.         return processModel.getPhysicalEstimatedState();
  163.     }

  165.     /** Get the "physical" estimated covariance matrix (i.e. not normalized)
  166.      * @return the "physical" estimated covariance matrix
  167.      */
  168.     public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
  169.         return processModel.getPhysicalEstimatedCovarianceMatrix();
  170.     }

  172.     /** Get the orbital parameters supported by this estimator.
  173.      * <p>
  174.      * If there are more than one propagator builder, then the names
  175.      * of the drivers have an index marker in square brackets appended
  176.      * to them in order to distinguish the various orbits. So for example
  177.      * with one builder generating Keplerian orbits the names would be
  178.      * simply "a", "e", "i"... but if there are several builders the
  179.      * names would be "a[0]", "e[0]", "i[0]"..."a[1]", "e[1]", "i[1]"...
  180.      * </p>
  181.      * @param estimatedOnly if true, only estimated parameters are returned
  182.      * @return orbital parameters supported by this estimator
  183.      */
  184.     public ParameterDriversList getOrbitalParametersDrivers(final boolean estimatedOnly) {

  186.         final ParameterDriversList estimated = new ParameterDriversList();
  187.         for (int i = 0; i < propagatorBuilders.size(); ++i) {
  188.             final String suffix = propagatorBuilders.size() > 1 ? "[" + i + "]" : null;
  189.             for (final ParameterDriver driver : propagatorBuilders.get(i).getOrbitalParametersDrivers().getDrivers()) {
  190.                 if (driver.isSelected() || !estimatedOnly) {
  191.                     if (suffix != null && !driver.getName().endsWith(suffix)) {
  192.                         // we add suffix only conditionally because the method may already have been called
  193.                         // and suffixes may have already been appended
  194.                         driver.setName(driver.getName() + suffix);
  195.                     }
  196.                     estimated.add(driver);
  197.                 }
  198.             }
  199.         }
  200.         return estimated;
  201.     }

  203.     /** Get the propagator parameters supported by this estimator.
  204.      * @param estimatedOnly if true, only estimated parameters are returned
  205.      * @return propagator parameters supported by this estimator
  206.      */
  207.     public ParameterDriversList getPropagationParametersDrivers(final boolean estimatedOnly) {

  209.         final ParameterDriversList estimated = new ParameterDriversList();
  210.         for (PropagatorBuilder builder : propagatorBuilders) {
  211.             for (final DelegatingDriver delegating : builder.getPropagationParametersDrivers().getDrivers()) {
  212.                 if (delegating.isSelected() || !estimatedOnly) {
  213.                     for (final ParameterDriver driver : delegating.getRawDrivers()) {
  214.                         estimated.add(driver);
  215.                     }
  216.                 }
  217.             }
  218.         }
  219.         return estimated;
  220.     }

  222.     /** Get the list of estimated measurements parameters.
  223.      * @return the list of estimated measurements parameters
  224.      */
  225.     public ParameterDriversList getEstimatedMeasurementsParameters() {
  226.         return processModel.getEstimatedMeasurementsParameters();
  227.     }

  229.     /** Process a single measurement.
  230.      * <p>
  231.      * Update the filter with the new measurement by calling the estimate method.
  232.      * </p>
  233.      * @param observedMeasurement the measurement to process
  234.      * @return estimated propagators
  235.      */
  236.     public AbstractIntegratedPropagator[] estimationStep(final ObservedMeasurement<?> observedMeasurement) {
  237.         try {
  238.             final ProcessEstimate estimate = filter.estimationStep(decorate(observedMeasurement));
  239.             processModel.finalizeEstimation(observedMeasurement, estimate);
  240.             if (observer != null) {
  241.                 observer.evaluationPerformed(processModel);
  242.             }
  243.             return processModel.getEstimatedPropagators();
  244.         } catch (MathRuntimeException mrte) {
  245.             throw new OrekitException(mrte);
  246.         }
  247.     }

  249.     /** Process several measurements.
  250.      * @param observedMeasurements the measurements to process in <em>chronologically sorted</em> order
  251.      * @return estimated propagators
  252.      */
  253.     public AbstractIntegratedPropagator[] processMeasurements(final Iterable<ObservedMeasurement<?>> observedMeasurements) {
  254.         AbstractIntegratedPropagator[] propagators = null;
  255.         for (ObservedMeasurement<?> observedMeasurement : observedMeasurements) {
  256.             propagators = estimationStep(observedMeasurement);
  257.         }
  258.         return propagators;
  259.     }

  261.     /** Decorate an observed measurement.
  262.      * <p>
  263.      * The "physical" measurement noise matrix is the covariance matrix of the measurement.
  264.      * Normalizing it consists in applying the following equation: Rn[i,j] =  R[i,j]/σ[i]/σ[j]
  265.      * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
  266.      * between the different components of the measurement.
  267.      * </p>
  268.      * @param observedMeasurement the measurement
  269.      * @return decorated measurement
  270.      */
  271.     private MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement) {

  273.         // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
  274.         // of the measurement on its non-diagonal elements.
  275.         // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
  276.         // Normalizing it leaves us with the matrix of the correlation coefficients
  277.         final RealMatrix covariance;
  278.         if (observedMeasurement instanceof PV) {
  279.             // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
  280.             final PV pv = (PV) observedMeasurement;
  281.             covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
  282.         } else if (observedMeasurement instanceof Position) {
  283.             // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
  284.             final Position position = (Position) observedMeasurement;
  285.             covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
  286.         } else {
  287.             // For other measurements we do not have a covariance matrix.
  288.             // Thus the correlation coefficients matrix is an identity matrix.
  289.             covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
  290.         }

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

  294.     }

  296. }
Created with JaCoCo 0.8.6.202009150832