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.filtering.kalman.KalmanFilter;
22  import org.hipparchus.filtering.kalman.ProcessEstimate;
23  import org.hipparchus.filtering.kalman.unscented.UnscentedKalmanFilter;
24  import org.hipparchus.linear.MatrixDecomposer;
25  import org.hipparchus.util.UnscentedTransformProvider;
26  import org.orekit.estimation.measurements.ObservedMeasurement;
27  import org.orekit.propagation.Propagator;
28  import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
29  import org.orekit.propagation.conversion.PropagatorBuilder;
30  import org.orekit.utils.ParameterDriver;
31  import org.orekit.utils.ParameterDriversList;
32  
33  /**
34   * Implementation of an Unscented Kalman filter to perform orbit determination.
35   * <p>
36   * The filter uses a {@link PropagatorBuilder} to initialize its reference trajectory.
37   * </p>
38   * <p>
39   * The estimated parameters are driven by {@link ParameterDriver} objects. They are of 3 different types:<ol>
40   *   <li><b>Orbital parameters</b>:The position and velocity of the spacecraft, or, more generally, its orbit.<br>
41   *       These parameters are retrieved from the reference trajectory propagator builder when the filter is initialized.</li>
42   *   <li><b>Propagation parameters</b>: Some parameters modelling physical processes (SRP or drag coefficients etc...).<br>
43   *       They are also retrieved from the propagator builder during the initialization phase.</li>
44   *   <li><b>Measurements parameters</b>: Parameters related to measurements (station biases, positions etc...).<br>
45   *       They are passed down to the filter in its constructor.</li>
46   * </ol>
47   * <p>
48   * The total number of estimated parameters is m, the size of the state vector.
49   * </p>
50   * <p>
51   * The Kalman filter implementation used is provided by the underlying mathematical library Hipparchus.
52   * </p>
53   *
54   * <p>An {@link UnscentedKalmanEstimator} object is built using the {@link UnscentedKalmanEstimatorBuilder#build() build}
55   * method of a {@link UnscentedKalmanEstimatorBuilder}. The builder is generalized to accept any {@link PropagatorBuilder}.
56   * Howerver, it is absolutely not recommended to use a {@link DSSTPropagatorBuilder}.
57   * A specific {@link SemiAnalyticalUnscentedKalmanEstimatorBuilder semi-analytical unscented Kalman Filter} is implemented
58   * and shall be used.
59   * </p>
60   *
61   * @author Gaëtan Pierre
62   * @author Bryan Cazabonne
63   * @since 11.3
64   */
65  public class UnscentedKalmanEstimator extends AbstractKalmanEstimator {
66  
67      /** Unscented Kalman filter process model. */
68      private final UnscentedKalmanModel processModel;
69  
70      /** Filter. */
71      private final UnscentedKalmanFilter<MeasurementDecorator> filter;
72  
73      /** Unscented Kalman filter estimator constructor (package private).
74       * @param decomposer decomposer to use for the correction phase
75       * @param propagatorBuilders propagators builders used to evaluate the orbit.
76       * @param processNoiseMatricesProviders providers for process noise matrices
77       * @param estimatedMeasurementParameters measurement parameters to estimate
78       * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
79       * @param utProvider provider for the unscented transform.
80       */
81      UnscentedKalmanEstimator(final MatrixDecomposer decomposer,
82                               final List<PropagatorBuilder> propagatorBuilders,
83                               final List<CovarianceMatrixProvider> processNoiseMatricesProviders,
84                               final ParameterDriversList estimatedMeasurementParameters,
85                               final CovarianceMatrixProvider measurementProcessNoiseMatrix,
86                               final UnscentedTransformProvider utProvider) {
87          super(decomposer, propagatorBuilders);
88  
89          // Build the process model and measurement model
90          this.processModel = new UnscentedKalmanModel(propagatorBuilders, processNoiseMatricesProviders,
91                                                       estimatedMeasurementParameters, measurementProcessNoiseMatrix);
92  
93          this.filter = new UnscentedKalmanFilter<>(decomposer, processModel, processModel.getEstimate(), utProvider);
94  
95      }
96  
97      /** {@inheritDoc}. */
98      @Override
99      protected KalmanEstimation getKalmanEstimation() {
100         return processModel;
101     }
102 
103     /** {@inheritDoc}. */
104     @Override
105     protected KalmanFilter<MeasurementDecorator> getKalmanFilter() {
106         return filter;
107     }
108 
109     /** {@inheritDoc}. */
110     @Override
111     protected double[] getScale() {
112         return processModel.getScale();
113     }
114 
115     /** Process a single measurement.
116      * <p>
117      * Update the filter with the new measurement by calling the estimate method.
118      * </p>
119      * @param observedMeasurement the measurement to process
120      * @return estimated propagator
121      */
122     public Propagator[] estimationStep(final ObservedMeasurement<?> observedMeasurement) {
123         final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurement, getReferenceDate()));
124         processModel.finalizeEstimation(observedMeasurement, estimate);
125         if (getObserver() != null) {
126             getObserver().evaluationPerformed(processModel);
127         }
128         return processModel.getEstimatedPropagators();
129     }
130 
131     /** Process several measurements.
132      * @param observedMeasurements the measurements to process in <em>chronologically sorted</em> order
133      * @return estimated propagator
134      */
135     public Propagator[] processMeasurements(final Iterable<ObservedMeasurement<?>> observedMeasurements) {
136         Propagator[] propagators = null;
137         for (ObservedMeasurement<?> observedMeasurement : observedMeasurements) {
138             propagators = estimationStep(observedMeasurement);
139         }
140         return propagators;
141     }
142 
143 }