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  
19  import java.util.List;
20  
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.orekit.errors.OrekitException;
26  import org.orekit.estimation.measurements.ObservedMeasurement;
27  import org.orekit.propagation.Propagator;
28  import org.orekit.propagation.analytical.BrouwerLyddanePropagator;
29  import org.orekit.propagation.analytical.EcksteinHechlerPropagator;
30  import org.orekit.propagation.analytical.Ephemeris;
31  import org.orekit.propagation.analytical.KeplerianPropagator;
32  import org.orekit.propagation.analytical.tle.TLEPropagator;
33  import org.orekit.propagation.conversion.PropagatorBuilder;
34  import org.orekit.propagation.numerical.NumericalPropagator;
35  import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
36  import org.orekit.time.AbsoluteDate;
37  import org.orekit.utils.ParameterDriver;
38  import org.orekit.utils.ParameterDriversList;
39  
40  
41  /**
42   * Implementation of a Kalman filter to perform orbit determination.
43   * <p>
44   * The filter uses a {@link PropagatorBuilder} to initialize its reference trajectory.
45   * The Kalman estimator can be used with a {@link NumericalPropagator}, {@link TLEPropagator},
46   * {@link BrouwerLyddanePropagator}, {@link EcksteinHechlerPropagator}, {@link KeplerianPropagator},
47   * or {@link Ephemeris}.
48   * </p>
49   * <p>
50   * Kalman estimation using a {@link DSSTPropagator semi-analytical orbit propagator} must be done using
51   * the {@link SemiAnalyticalKalmanEstimator}.
52   * </p>
53   * <p>
54   * The estimated parameters are driven by {@link ParameterDriver} objects. They are of 3 different types:<ol>
55   *   <li><b>Orbital parameters</b>:The position and velocity of the spacecraft, or, more generally, its orbit.<br>
56   *       These parameters are retrieved from the reference trajectory propagator builder when the filter is initialized.</li>
57   *   <li><b>Propagation parameters</b>: Some parameters modelling physical processes (SRP or drag coefficients etc...).<br>
58   *       They are also retrieved from the propagator builder during the initialization phase.</li>
59   *   <li><b>Measurements parameters</b>: Parameters related to measurements (station biases, positions etc...).<br>
60   *       They are passed down to the filter in its constructor.</li>
61   * </ol>
62   * <p>
63   * The total number of estimated parameters is m, the size of the state vector.
64   * </p>
65   * <p>
66   * The Kalman filter implementation used is provided by the underlying mathematical library Hipparchus.
67   * All the variables seen by Hipparchus (states, covariances, measurement matrices...) are normalized
68   * using a specific scale for each estimated parameters or standard deviation noise for each measurement components.
69   * </p>
70   *
71   * <p>A {@link KalmanEstimator} object is built using the {@link KalmanEstimatorBuilder#build() build}
72   * method of a {@link KalmanEstimatorBuilder}.</p>
73   *
74   * @author Romain Gerbaud
75   * @author Maxime Journot
76   * @author Luc Maisonobe
77   * @since 9.2
78   */
79  public class KalmanEstimator extends AbstractKalmanEstimator {
80  
81      /** Reference date. */
82      private final AbsoluteDate referenceDate;
83  
84      /** Kalman filter process model. */
85      private final KalmanModel processModel;
86  
87      /** Filter. */
88      private final ExtendedKalmanFilter<MeasurementDecorator> filter;
89  
90      /** Observer to retrieve current estimation info. */
91      private KalmanObserver observer;
92  
93      /** Kalman filter estimator constructor (package private).
94       * @param decomposer decomposer to use for the correction phase
95       * @param propagatorBuilders propagators builders used to evaluate the orbit.
96       * @param processNoiseMatricesProviders providers for process noise matrices
97       * @param estimatedMeasurementParameters measurement parameters to estimate
98       * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
99       * @since 10.3
100      */
101     KalmanEstimator(final MatrixDecomposer decomposer,
102                     final List<PropagatorBuilder> propagatorBuilders,
103                     final List<CovarianceMatrixProvider> processNoiseMatricesProviders,
104                     final ParameterDriversList estimatedMeasurementParameters,
105                     final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
106         super(propagatorBuilders);
107         this.referenceDate      = propagatorBuilders.get(0).getInitialOrbitDate();
108         this.observer           = null;
109 
110         // Build the process model and measurement model
111         this.processModel = new KalmanModel(propagatorBuilders,
112                                             processNoiseMatricesProviders,
113                                             estimatedMeasurementParameters,
114                                             measurementProcessNoiseMatrix);
115 
116         this.filter = new ExtendedKalmanFilter<>(decomposer, processModel, processModel.getEstimate());
117 
118     }
119 
120     /** {@inheritDoc}. */
121     @Override
122     protected KalmanEstimation getKalmanEstimation() {
123         return processModel;
124     }
125 
126     /** Set the observer.
127      * @param observer the observer
128      */
129     public void setObserver(final KalmanObserver observer) {
130         this.observer = observer;
131     }
132 
133     /** Process a single measurement.
134      * <p>
135      * Update the filter with the new measurement by calling the estimate method.
136      * </p>
137      * @param observedMeasurement the measurement to process
138      * @return estimated propagators
139      */
140     public Propagator[] estimationStep(final ObservedMeasurement<?> observedMeasurement) {
141         try {
142             final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurement, referenceDate));
143             processModel.finalizeEstimation(observedMeasurement, estimate);
144             if (observer != null) {
145                 observer.evaluationPerformed(processModel);
146             }
147             return processModel.getEstimatedPropagators();
148         } catch (MathRuntimeException mrte) {
149             throw new OrekitException(mrte);
150         }
151     }
152 
153     /** Process several measurements.
154      * @param observedMeasurements the measurements to process in <em>chronologically sorted</em> order
155      * @return estimated propagators
156      */
157     public Propagator[] processMeasurements(final Iterable<ObservedMeasurement<?>> observedMeasurements) {
158         Propagator[] propagators = null;
159         for (ObservedMeasurement<?> observedMeasurement : observedMeasurements) {
160             propagators = estimationStep(observedMeasurement);
161         }
162         return propagators;
163     }
164 
165 }