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 org.hipparchus.filtering.kalman.ProcessEstimate;
20  import org.hipparchus.filtering.kalman.unscented.UnscentedEvolution;
21  import org.hipparchus.filtering.kalman.unscented.UnscentedProcess;
22  import org.hipparchus.linear.ArrayRealVector;
23  import org.hipparchus.linear.MatrixUtils;
24  import org.hipparchus.linear.RealMatrix;
25  import org.hipparchus.linear.RealVector;
26  import org.orekit.estimation.measurements.EstimatedMeasurement;
27  import org.orekit.estimation.measurements.EstimatedMeasurementBase;
28  import org.orekit.estimation.measurements.ObservedMeasurement;
29  import org.orekit.orbits.CartesianOrbit;
30  import org.orekit.orbits.Orbit;
31  import org.orekit.propagation.Propagator;
32  import org.orekit.propagation.SpacecraftState;
33  import org.orekit.propagation.conversion.AbstractPropagatorBuilder;
34  import org.orekit.propagation.conversion.PropagatorBuilder;
35  import org.orekit.time.AbsoluteDate;
36  import org.orekit.utils.ParameterDriver;
37  import org.orekit.utils.ParameterDriversList;
38  import org.orekit.utils.ParameterDriversList.DelegatingDriver;
39  
40  import java.util.List;
41  
42  /** Class defining the process model dynamics to use with a {@link UnscentedKalmanEstimator}.
43   * @author Gaëtan Pierre
44   * @author Bryan Cazabonne
45   * @since 11.3
46   */
47  public class UnscentedKalmanModel extends AbstractKalmanEstimationCommon implements UnscentedProcess<MeasurementDecorator> {
48  
49      /** Reference values. */
50      private final double[] referenceValues;
51  
52      /** Unscented Kalman process model constructor (package private).
53       * @param propagatorBuilders propagators builders used to evaluate the orbits.
54       * @param covarianceMatricesProviders provider for covariance matrix
55       * @param estimatedMeasurementParameters measurement parameters to estimate
56       * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
57       */
58      protected UnscentedKalmanModel(final List<PropagatorBuilder> propagatorBuilders,
59                                     final List<CovarianceMatrixProvider> covarianceMatricesProviders,
60                                     final ParameterDriversList estimatedMeasurementParameters,
61                                     final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
62  
63          super(propagatorBuilders, covarianceMatricesProviders, estimatedMeasurementParameters, measurementProcessNoiseMatrix);
64  
65          // Record the initial reference values
66          int stateDimension = 0;
67          for (final ParameterDriver ignored : getEstimatedOrbitalParameters().getDrivers()) {
68              stateDimension += 1;
69          }
70          for (final ParameterDriver ignored : getEstimatedPropagationParameters().getDrivers()) {
71              stateDimension += 1;
72          }
73          for (final ParameterDriver ignored : getEstimatedMeasurementsParameters().getDrivers()) {
74              stateDimension += 1;
75          }
76  
77          this.referenceValues = new double[stateDimension];
78          int index = 0;
79          for (final ParameterDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
80              referenceValues[index++] = driver.getReferenceValue();
81          }
82          for (final ParameterDriver driver : getEstimatedPropagationParameters().getDrivers()) {
83              referenceValues[index++] = driver.getReferenceValue();
84          }
85          for (final ParameterDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
86              referenceValues[index++] = driver.getReferenceValue();
87          }
88      }
89  
90      /** {@inheritDoc} */
91      @Override
92      public UnscentedEvolution getEvolution(final double previousTime, final RealVector[] sigmaPoints,
93                                             final MeasurementDecorator measurement) {
94  
95          // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
96          final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
97          for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
98              if (driver.getReferenceDate() == null) {
99                  driver.setReferenceDate(getBuilders().get(0).getInitialOrbitDate());
100             }
101         }
102 
103         // Increment measurement number
104         incrementCurrentMeasurementNumber();
105 
106         // Update the current date
107         setCurrentDate(measurement.getObservedMeasurement().getDate());
108 
109         // Initialize array of predicted sigma points
110         final RealVector[] predictedSigmaPoints = new RealVector[sigmaPoints.length];
111 
112         // Propagate each sigma point
113         //
114         // We need to make a choice about what happens with the non-estimated parts of the orbital states.
115         // Here we've assumed that the zero'th sigma point is roughly the mean and keep those propagated
116         // orbital parameters.  This is why we loop backward through the sigma-points and don't reset the
117         // propagator builders on the last iteration (corresponding to the zero-th sigma point).
118         //
119         // Note that -not- resetting the builders on the last iteration means that their time-stamps correspond
120         // to the prediction time.  The assumption is that the unscented filter calls getEvolution, then
121         // getPredictedMeasurements, then getInnovation.
122         for (int i = sigmaPoints.length - 1; i >= 0; i--) {
123 
124             // Set parameters for this sigma point
125             final RealVector sigmaPoint = sigmaPoints[i].copy();
126             updateParameters(sigmaPoint);
127 
128             // Get propagators
129             final Propagator[] propagators = getEstimatedPropagators();
130 
131             // Do prediction
132             predictedSigmaPoints[i] =
133                     predictState(observedMeasurement.getDate(), sigmaPoint, propagators, i != 0);
134         }
135 
136         // Reset the driver reference values based on the first sigma point
137         int d = 0;
138         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
139             driver.setReferenceValue(referenceValues[d]);
140             driver.setNormalizedValue(predictedSigmaPoints[0].getEntry(d));
141             referenceValues[d] = driver.getValue();
142 
143             // Make remaining sigma points relative to the first
144             for (int i = 1; i < predictedSigmaPoints.length; ++i) {
145                 predictedSigmaPoints[i].setEntry(d, predictedSigmaPoints[i].getEntry(d) - predictedSigmaPoints[0].getEntry(d));
146             }
147             predictedSigmaPoints[0].setEntry(d, 0.0);
148 
149             d += 1;
150         }
151 
152         // Return
153         return new UnscentedEvolution(measurement.getTime(), predictedSigmaPoints);
154     }
155 
156     /** {@inheritDoc} */
157     @Override
158     public RealMatrix getProcessNoiseMatrix(final double previousTime, final RealVector predictedState,
159                                             final MeasurementDecorator measurement) {
160         // Set parameters from predicted state
161         final RealVector predictedStateCopy = predictedState.copy();
162         updateParameters(predictedStateCopy);
163 
164         // Get propagators
165         Propagator[] propagators = getEstimatedPropagators();
166 
167         // "updateParameters" sets the correct orbital info, but doesn't reset the time.
168         for (int k = 0; k < propagators.length; ++k) {
169             final SpacecraftState predicted = propagators[k].getInitialState();
170             final Orbit predictedOrbit = getBuilders().get(k).getOrbitType().convertType(
171                     new CartesianOrbit(predicted.getPVCoordinates(),
172                             predicted.getFrame(),
173                             measurement.getObservedMeasurement().getDate(),
174                             predicted.getOrbit().getMu()
175                     )
176             );
177             getBuilders().get(k).resetOrbit(predictedOrbit);
178         }
179         propagators = getEstimatedPropagators();
180 
181         // Predicted states
182         for (int k = 0; k < propagators.length; ++k) {
183             setPredictedSpacecraftState(propagators[k].getInitialState(), k);
184         }
185 
186         return getNormalizedProcessNoise(predictedState.getDimension());
187     }
188 
189     /** {@inheritDoc} */
190     @Override
191     public RealVector[] getPredictedMeasurements(final RealVector[] predictedSigmaPoints, final MeasurementDecorator measurement) {
192 
193         // Observed measurement
194         final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
195 
196         // Standard deviation as a vector
197         final RealVector theoreticalStandardDeviation =
198                 MatrixUtils.createRealVector(observedMeasurement.getTheoreticalStandardDeviation());
199 
200         // Initialize arrays of predicted states and measurements
201         final RealVector[] predictedMeasurements = new RealVector[predictedSigmaPoints.length];
202 
203         // Loop on sigma points to predict measurements
204         for (int i = 0; i < predictedSigmaPoints.length; ++i) {
205             // Set parameters for this sigma point
206             final RealVector predictedSigmaPoint = predictedSigmaPoints[i].copy();
207             updateParameters(predictedSigmaPoint);
208 
209             // Get propagators
210             final Propagator[] propagators = getEstimatedPropagators();
211 
212             // Predicted states
213             final SpacecraftState[] predictedStates = new SpacecraftState[propagators.length];
214             for (int k = 0; k < propagators.length; ++k) {
215                 predictedStates[k] = propagators[k].getInitialState();
216             }
217 
218             // Calculated estimated measurement from predicted sigma point
219             final EstimatedMeasurement<?> estimated = estimateMeasurement(observedMeasurement, getCurrentMeasurementNumber(),
220                                                                                    KalmanEstimatorUtil.filterRelevant(observedMeasurement,
221                                                                                                                       predictedStates));
222             predictedMeasurements[i] = new ArrayRealVector(estimated.getEstimatedValue())
223                     .ebeDivide(theoreticalStandardDeviation);
224         }
225 
226         // Return the predicted measurements
227         return predictedMeasurements;
228 
229     }
230 
231     /** {@inheritDoc} */
232     @Override
233     public RealVector getInnovation(final MeasurementDecorator measurement, final RealVector predictedMeas,
234                                     final RealVector predictedState, final RealMatrix innovationCovarianceMatrix) {
235         // Standard deviation as a vector
236         final RealVector theoreticalStandardDeviation =
237                 MatrixUtils.createRealVector(measurement.getObservedMeasurement().getTheoreticalStandardDeviation());
238 
239         // Get propagators
240         final Propagator[] propagators = getEstimatedPropagators();
241 
242         // Predicted states
243         for (int k = 0; k < propagators.length; ++k) {
244             setPredictedSpacecraftState(propagators[k].getInitialState(), k);
245         }
246 
247         // set estimated value to the predicted value from the filter
248         final EstimatedMeasurement<?> predictedMeasurement =
249             estimateMeasurement(measurement.getObservedMeasurement(), getCurrentMeasurementNumber(),
250                                 KalmanEstimatorUtil.filterRelevant(measurement.getObservedMeasurement(),
251                                 getPredictedSpacecraftStates()));
252         setPredictedMeasurement(predictedMeasurement);
253         predictedMeasurement.setEstimatedValue(predictedMeas.ebeMultiply(theoreticalStandardDeviation).toArray());
254 
255         // Check for outliers
256         KalmanEstimatorUtil.applyDynamicOutlierFilter(predictedMeasurement, innovationCovarianceMatrix);
257 
258         // Compute the innovation vector
259         return KalmanEstimatorUtil.computeInnovationVector(predictedMeasurement,
260                 predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
261     }
262 
263 
264     private RealVector predictState(final AbsoluteDate date,
265                                     final RealVector previousState,
266                                     final Propagator[] propagators,
267                                     final boolean resetState) {
268 
269         // Initialise predicted state
270         final RealVector predictedState = previousState.copy();
271 
272         // Orbital parameters counter
273         int jOrb = 0;
274 
275         // Loop over propagators
276         for (int k = 0; k < propagators.length; ++k) {
277 
278             // Record original state
279             final SpacecraftState originalState = propagators[k].getInitialState();
280 
281             // Propagate
282             final SpacecraftState predicted = propagators[k].propagate(date);
283 
284             // Update the builder with the predicted orbit
285             // This updates the orbital drivers with the values of the predicted orbit
286             getBuilders().get(k).resetOrbit(predicted.getOrbit());
287 
288             // Additionally, for PropagatorBuilders which use mass, update the builder with the predicted mass value.
289             // If any mass changes have occurred during this estimation step, such as maneuvers,
290             // the updated mass value must be carried over so that new Propagators from this builder start with the updated mass.
291             if (getBuilders().get(k) instanceof AbstractPropagatorBuilder) {
292                 ((AbstractPropagatorBuilder<?>) (getBuilders().get(k))).setMass(predicted.getMass());
293             }
294 
295             // The orbital parameters in the state vector are replaced with their predicted values
296             // The propagation & measurement parameters are not changed by the prediction (i.e. the propagation)
297             // As the propagator builder was previously updated with the predicted orbit,
298             // the selected orbital drivers are already up to date with the prediction
299             for (DelegatingDriver orbitalDriver : getBuilders().get(k).getOrbitalParametersDrivers().getDrivers()) {
300                 if (orbitalDriver.isSelected()) {
301                     orbitalDriver.setReferenceValue(referenceValues[jOrb]);
302                     predictedState.setEntry(jOrb, orbitalDriver.getNormalizedValue());
303 
304                     jOrb += 1;
305                 }
306             }
307 
308             // Set the builder back to the original time
309             if (resetState) {
310                 getBuilders().get(k).resetOrbit(originalState.getOrbit());
311             }
312         }
313 
314         return predictedState;
315     }
316 
317 
318     /** Finalize estimation.
319      * @param observedMeasurement measurement that has just been processed
320      * @param estimate corrected estimate
321      */
322     public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
323                                    final ProcessEstimate estimate) {
324         // Update the parameters with the estimated state
325         // The min/max values of the parameters are handled by the ParameterDriver implementation
326         setCorrectedEstimate(estimate);
327         updateParameters(estimate.getState());
328 
329         // Get the estimated propagator (mirroring parameter update in the builder)
330         // and the estimated spacecraft state
331         final Propagator[] estimatedPropagators = getEstimatedPropagators();
332         for (int k = 0; k < estimatedPropagators.length; ++k) {
333             setCorrectedSpacecraftState(estimatedPropagators[k].getInitialState(), k);
334         }
335 
336         // Corrected measurement
337         setCorrectedMeasurement(estimateMeasurement(observedMeasurement, getCurrentMeasurementNumber(),
338                                                     KalmanEstimatorUtil.filterRelevant(observedMeasurement,
339                                                     getCorrectedSpacecraftStates())));
340     }
341 
342     /**
343      * Estimate measurement (without derivatives).
344      * @param <T> measurement type
345      * @param observedMeasurement observed measurement
346      * @param measurementNumber measurement number
347      * @param spacecraftStates states
348      * @return estimated measurement
349      * @since 12.1
350      */
351     private static <T extends ObservedMeasurement<T>> EstimatedMeasurement<T> estimateMeasurement(final ObservedMeasurement<T> observedMeasurement,
352                                                                                                   final int measurementNumber,
353                                                                                                   final SpacecraftState[] spacecraftStates) {
354         final EstimatedMeasurementBase<T> estimatedMeasurementBase = observedMeasurement.
355                 estimateWithoutDerivatives(measurementNumber, measurementNumber,
356                 KalmanEstimatorUtil.filterRelevant(observedMeasurement, spacecraftStates));
357         return new EstimatedMeasurement<>(estimatedMeasurementBase);
358     }
359 
360     /** Update parameter drivers with a normalised state, adjusting state according to the driver limits.
361      * @param normalizedState the input state
362      * The min/max allowed values are handled by the parameter themselves.
363      */
364     private void updateParameters(final RealVector normalizedState) {
365         int i = 0;
366         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
367             // let the parameter handle min/max clipping
368             driver.setReferenceValue(referenceValues[i]);
369             driver.setNormalizedValue(normalizedState.getEntry(i));
370             normalizedState.setEntry(i++, driver.getNormalizedValue());
371         }
372         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
373             // let the parameter handle min/max clipping
374             driver.setNormalizedValue(normalizedState.getEntry(i));
375             normalizedState.setEntry(i++, driver.getNormalizedValue());
376         }
377         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
378             // let the parameter handle min/max clipping
379             driver.setNormalizedValue(normalizedState.getEntry(i));
380             normalizedState.setEntry(i++, driver.getNormalizedValue());
381         }
382     }
383 }