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;
18  
19  import java.util.List;
20  
21  import org.hipparchus.exception.MathRuntimeException;
22  import org.hipparchus.filtering.kalman.KalmanFilter;
23  import org.hipparchus.filtering.kalman.ProcessEstimate;
24  import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
25  import org.hipparchus.linear.MatrixDecomposer;
26  import org.orekit.errors.OrekitException;
27  import org.orekit.estimation.measurements.ObservedMeasurement;
28  import org.orekit.propagation.Propagator;
29  import org.orekit.propagation.analytical.BrouwerLyddanePropagator;
30  import org.orekit.propagation.analytical.EcksteinHechlerPropagator;
31  import org.orekit.propagation.analytical.Ephemeris;
32  import org.orekit.propagation.analytical.KeplerianPropagator;
33  import org.orekit.propagation.analytical.tle.TLEPropagator;
34  import org.orekit.propagation.conversion.PropagatorBuilder;
35  import org.orekit.propagation.numerical.NumericalPropagator;
36  import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
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      /** Kalman filter process model. */
82      private final KalmanModel processModel;
83  
84      /** Filter. */
85      private final KalmanFilter<MeasurementDecorator> filter;
86  
87      /** Kalman filter estimator constructor (package private).
88       * @param decomposer decomposer to use for the correction phase
89       * @param propagatorBuilders propagators builders used to evaluate the orbit.
90       * @param processNoiseMatricesProviders providers for process noise matrices
91       * @param estimatedMeasurementParameters measurement parameters to estimate
92       * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
93       * @since 10.3
94       */
95      KalmanEstimator(final MatrixDecomposer decomposer,
96                      final List<PropagatorBuilder> propagatorBuilders,
97                      final List<CovarianceMatrixProvider> processNoiseMatricesProviders,
98                      final ParameterDriversList estimatedMeasurementParameters,
99                      final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
100         super(decomposer, propagatorBuilders);
101 
102         // Build the process model and measurement model
103         this.processModel = new KalmanModel(propagatorBuilders,
104                                             processNoiseMatricesProviders,
105                                             estimatedMeasurementParameters,
106                                             measurementProcessNoiseMatrix);
107 
108         this.filter = new ExtendedKalmanFilter<>(decomposer, processModel, processModel.getEstimate());
109 
110     }
111 
112     /** {@inheritDoc}. */
113     @Override
114     protected KalmanEstimation getKalmanEstimation() {
115         return processModel;
116     }
117 
118     /** {@inheritDoc}. */
119     @Override
120     protected KalmanFilter<MeasurementDecorator> getKalmanFilter() {
121         return filter;
122     }
123 
124     /** {@inheritDoc}. */
125     @Override
126     protected double[] getScale() {
127         return processModel.getScale();
128     }
129 
130     /** Process a single measurement.
131      * <p>
132      * Update the filter with the new measurement by calling the estimate method.
133      * </p>
134      * @param observedMeasurement the measurement to process
135      * @return estimated propagators
136      */
137     public Propagator[] estimationStep(final ObservedMeasurement<?> observedMeasurement) {
138         try {
139             final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurement, getReferenceDate()));
140             processModel.finalizeEstimation(observedMeasurement, estimate);
141             if (getObserver() != null) {
142                 getObserver().evaluationPerformed(processModel);
143             }
144             return processModel.getEstimatedPropagators();
145         } catch (MathRuntimeException mrte) {
146             throw new OrekitException(mrte);
147         }
148     }
149 
150     /** Process several measurements.
151      * @param observedMeasurements the measurements to process in <em>chronologically sorted</em> order
152      * @return estimated propagators
153      */
154     public Propagator[] processMeasurements(final Iterable<ObservedMeasurement<?>> observedMeasurements) {
155         Propagator[] propagators = null;
156         for (ObservedMeasurement<?> observedMeasurement : observedMeasurements) {
157             propagators = estimationStep(observedMeasurement);
158         }
159         return propagators;
160     }
161 
162 }