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.filtering.kalman.ProcessEstimate;
22  import org.hipparchus.filtering.kalman.unscented.UnscentedKalmanFilter;
23  import org.hipparchus.linear.MatrixDecomposer;
24  import org.hipparchus.util.UnscentedTransformProvider;
25  import org.orekit.estimation.measurements.ObservedMeasurement;
26  import org.orekit.propagation.Propagator;
27  import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
28  import org.orekit.propagation.conversion.PropagatorBuilder;
29  import org.orekit.time.AbsoluteDate;
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      /** Reference date. */
68      private final AbsoluteDate referenceDate;
69  
70      /** Unscented Kalman filter process model. */
71      private final UnscentedKalmanModel processModel;
72  
73      /** Filter. */
74      private final UnscentedKalmanFilter<MeasurementDecorator> filter;
75  
76      /** Observer to retrieve current estimation info. */
77      private KalmanObserver observer;
78  
79      /** Unscented Kalman filter estimator constructor (package private).
80       * @param decomposer decomposer to use for the correction phase
81       * @param propagatorBuilders propagators builders used to evaluate the orbit.
82       * @param processNoiseMatricesProviders providers for process noise matrices
83       * @param estimatedMeasurementParameters measurement parameters to estimate
84       * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
85       * @param utProvider provider for the unscented transform.
86       */
87      UnscentedKalmanEstimator(final MatrixDecomposer decomposer,
88                               final List<PropagatorBuilder> propagatorBuilders,
89                               final List<CovarianceMatrixProvider> processNoiseMatricesProviders,
90                               final ParameterDriversList estimatedMeasurementParameters,
91                               final CovarianceMatrixProvider measurementProcessNoiseMatrix,
92                               final UnscentedTransformProvider utProvider) {
93          super(propagatorBuilders);
94          this.referenceDate = propagatorBuilders.get(0).getInitialOrbitDate();
95          this.observer      = null;
96  
97          // Build the process model and measurement model
98          this.processModel = new UnscentedKalmanModel(propagatorBuilders, processNoiseMatricesProviders,
99                                                       estimatedMeasurementParameters, measurementProcessNoiseMatrix);
100 
101         this.filter = new UnscentedKalmanFilter<>(decomposer, processModel, processModel.getEstimate(), utProvider);
102 
103     }
104 
105     /** {@inheritDoc}. */
106     @Override
107     protected KalmanEstimation getKalmanEstimation() {
108         return processModel;
109     }
110 
111     /** Set the observer.
112      * @param observer the observer
113      */
114     public void setObserver(final KalmanObserver observer) {
115         this.observer = observer;
116     }
117 
118     /** Process a single measurement.
119      * <p>
120      * Update the filter with the new measurement by calling the estimate method.
121      * </p>
122      * @param observedMeasurement the measurement to process
123      * @return estimated propagator
124      */
125     public Propagator[] estimationStep(final ObservedMeasurement<?> observedMeasurement) {
126         final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurement, referenceDate));
127         processModel.finalizeEstimation(observedMeasurement, estimate);
128         if (observer != null) {
129             observer.evaluationPerformed(processModel);
130         }
131         return processModel.getEstimatedPropagators();
132     }
133 
134     /** Process several measurements.
135      * @param observedMeasurements the measurements to process in <em>chronologically sorted</em> order
136      * @return estimated propagator
137      */
138     public Propagator[] processMeasurements(final Iterable<ObservedMeasurement<?>> observedMeasurements) {
139         Propagator[] propagators = null;
140         for (ObservedMeasurement<?> observedMeasurement : observedMeasurements) {
141             propagators = estimationStep(observedMeasurement);
142         }
143         return propagators;
144     }
145 
146 }