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.exception.MathRuntimeException;
20  import org.hipparchus.filtering.kalman.ProcessEstimate;
21  import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
22  import org.hipparchus.filtering.kalman.extended.NonLinearEvolution;
23  import org.hipparchus.filtering.kalman.extended.NonLinearProcess;
24  import org.hipparchus.linear.Array2DRowRealMatrix;
25  import org.hipparchus.linear.ArrayRealVector;
26  import org.hipparchus.linear.MatrixUtils;
27  import org.hipparchus.linear.QRDecomposition;
28  import org.hipparchus.linear.RealMatrix;
29  import org.hipparchus.linear.RealVector;
30  import org.hipparchus.util.FastMath;
31  import org.orekit.errors.OrekitException;
32  import org.orekit.estimation.measurements.EstimatedMeasurement;
33  import org.orekit.estimation.measurements.ObservedMeasurement;
34  import org.orekit.orbits.Orbit;
35  import org.orekit.orbits.OrbitType;
36  import org.orekit.propagation.PropagationType;
37  import org.orekit.propagation.SpacecraftState;
38  import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
39  import org.orekit.propagation.semianalytical.dsst.DSSTHarvester;
40  import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
41  import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
42  import org.orekit.propagation.semianalytical.dsst.forces.ShortPeriodTerms;
43  import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
44  import org.orekit.time.AbsoluteDate;
45  import org.orekit.time.ChronologicalComparator;
46  import org.orekit.utils.ParameterDriver;
47  import org.orekit.utils.ParameterDriversList;
48  import org.orekit.utils.ParameterDriversList.DelegatingDriver;
49  import org.orekit.utils.TimeSpanMap.Span;
50  
51  import java.util.ArrayList;
52  import java.util.Comparator;
53  import java.util.HashMap;
54  import java.util.List;
55  import java.util.Map;
56  
57  /** Process model to use with a {@link SemiAnalyticalKalmanEstimator}.
58   *
59   * @see "Folcik Z., Orbit Determination Using Modern Filters/Smoothers and Continuous Thrust Modeling,
60   *       Master of Science Thesis, Department of Aeronautics and Astronautics, MIT, June, 2008."
61   *
62   * @see "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
63   *       Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
64   *       Specialist Conference, Big Sky, August 2021."
65   *
66   * @author Julie Bayard
67   * @author Bryan Cazabonne
68   * @author Maxime Journot
69   * @since 11.1
70   */
71  public  class SemiAnalyticalKalmanModel implements KalmanEstimation, NonLinearProcess<MeasurementDecorator>, SemiAnalyticalProcess {
72  
73      /** Builders for DSST propagator. */
74      private final DSSTPropagatorBuilder builder;
75  
76      /** Estimated orbital parameters. */
77      private final ParameterDriversList estimatedOrbitalParameters;
78  
79      /** Per-builder estimated propagation drivers. */
80      private final ParameterDriversList estimatedPropagationParameters;
81  
82      /** Estimated measurements parameters. */
83      private final ParameterDriversList estimatedMeasurementsParameters;
84  
85      /** Map for propagation parameters columns. */
86      private final Map<String, Integer> propagationParameterColumns;
87  
88      /** Map for measurements parameters columns. */
89      private final Map<String, Integer> measurementParameterColumns;
90  
91      /** Scaling factors. */
92      private final double[] scale;
93  
94      /** Provider for covariance matrix. */
95      private final CovarianceMatrixProvider covarianceMatrixProvider;
96  
97      /** Process noise matrix provider for measurement parameters. */
98      private final CovarianceMatrixProvider measurementProcessNoiseMatrix;
99  
100     /** Harvester between two-dimensional Jacobian matrices and one-dimensional additional state arrays. */
101     private DSSTHarvester harvester;
102 
103     /** Propagators for the reference trajectories, up to current date. */
104     private DSSTPropagator dsstPropagator;
105 
106     /** Observer to retrieve current estimation info. */
107     private KalmanObserver observer;
108 
109     /** Current number of measurement. */
110     private int currentMeasurementNumber;
111 
112     /** Current date. */
113     private AbsoluteDate currentDate;
114 
115     /** Predicted mean element filter correction. */
116     private RealVector predictedFilterCorrection;
117 
118     /** Corrected mean element filter correction. */
119     private RealVector correctedFilterCorrection;
120 
121     /** Predicted measurement. */
122     private EstimatedMeasurement<?> predictedMeasurement;
123 
124     /** Corrected measurement. */
125     private EstimatedMeasurement<?> correctedMeasurement;
126 
127     /** Nominal mean spacecraft state. */
128     private SpacecraftState nominalMeanSpacecraftState;
129 
130     /** Previous nominal mean spacecraft state. */
131     private SpacecraftState previousNominalMeanSpacecraftState;
132 
133     /** Current corrected estimate. */
134     private ProcessEstimate correctedEstimate;
135 
136     /** Inverse of the orbital part of the state transition matrix. */
137     private RealMatrix phiS;
138 
139     /** Propagation parameters part of the state transition matrix. */
140     private RealMatrix psiS;
141 
142     /** Kalman process model constructor (package private).
143      * @param propagatorBuilder propagators builders used to evaluate the orbits.
144      * @param covarianceMatrixProvider provider for covariance matrix
145      * @param estimatedMeasurementParameters measurement parameters to estimate
146      * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
147      */
148     protected SemiAnalyticalKalmanModel(final DSSTPropagatorBuilder propagatorBuilder,
149                                         final CovarianceMatrixProvider covarianceMatrixProvider,
150                                         final ParameterDriversList estimatedMeasurementParameters,
151                                         final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
152 
153         this.builder                         = propagatorBuilder;
154         this.estimatedMeasurementsParameters = estimatedMeasurementParameters;
155         this.measurementParameterColumns     = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
156         this.observer                        = null;
157         this.currentMeasurementNumber        = 0;
158         this.currentDate                     = propagatorBuilder.getInitialOrbitDate();
159         this.covarianceMatrixProvider        = covarianceMatrixProvider;
160         this.measurementProcessNoiseMatrix   = measurementProcessNoiseMatrix;
161 
162         // Number of estimated parameters
163         int columns = 0;
164 
165         // Set estimated orbital parameters
166         estimatedOrbitalParameters = new ParameterDriversList();
167         for (final ParameterDriver driver : builder.getOrbitalParametersDrivers().getDrivers()) {
168 
169             // Verify if the driver reference date has been set
170             if (driver.getReferenceDate() == null) {
171                 driver.setReferenceDate(currentDate);
172             }
173 
174             // Verify if the driver is selected
175             if (driver.isSelected()) {
176                 estimatedOrbitalParameters.add(driver);
177                 columns++;
178             }
179 
180         }
181 
182         // Set estimated propagation parameters
183         estimatedPropagationParameters = new ParameterDriversList();
184         final List<String> estimatedPropagationParametersNames = new ArrayList<>();
185         for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
186 
187             // Verify if the driver reference date has been set
188             if (driver.getReferenceDate() == null) {
189                 driver.setReferenceDate(currentDate);
190             }
191 
192             // Verify if the driver is selected
193             if (driver.isSelected()) {
194                 estimatedPropagationParameters.add(driver);
195                 // Add the driver name if it has not been added yet
196                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
197 
198                     if (!estimatedPropagationParametersNames.contains(span.getData())) {
199                         estimatedPropagationParametersNames.add(span.getData());
200                     }
201                 }
202             }
203 
204         }
205         estimatedPropagationParametersNames.sort(Comparator.naturalOrder());
206 
207         // Populate the map of propagation drivers' columns and update the total number of columns
208         propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
209         for (final String driverName : estimatedPropagationParametersNames) {
210             propagationParameterColumns.put(driverName, columns);
211             ++columns;
212         }
213 
214         // Set the estimated measurement parameters
215         for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
216             if (parameter.getReferenceDate() == null) {
217                 parameter.setReferenceDate(currentDate);
218             }
219             for (Span<String> span = parameter.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
220                 measurementParameterColumns.put(span.getData(), columns);
221                 ++columns;
222             }
223         }
224 
225         // Compute the scale factors
226         this.scale = new double[columns];
227         int index = 0;
228         for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
229             scale[index++] = driver.getScale();
230         }
231         for (final ParameterDriver driver : estimatedPropagationParameters.getDrivers()) {
232             for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
233                 scale[index++] = driver.getScale();
234             }
235         }
236         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
237             for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
238                 scale[index++] = driver.getScale();
239             }
240         }
241 
242         // Build the reference propagator and add its partial derivatives equations implementation
243         updateReferenceTrajectory(getEstimatedPropagator());
244         this.nominalMeanSpacecraftState = dsstPropagator.getInitialState();
245         this.previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;
246 
247         // Initialize "field" short periodic terms
248         harvester.initializeFieldShortPeriodTerms(nominalMeanSpacecraftState);
249 
250         // Initialize the estimated normalized mean element filter correction (See Ref [1], Eq. 3.2a)
251         this.predictedFilterCorrection = MatrixUtils.createRealVector(columns);
252         this.correctedFilterCorrection = predictedFilterCorrection;
253 
254         // Initialize propagation parameters part of the state transition matrix (See Ref [1], Eq. 3.2c)
255         this.psiS = null;
256         if (estimatedPropagationParameters.getNbParams() != 0) {
257             this.psiS = MatrixUtils.createRealMatrix(getNumberSelectedOrbitalDriversValuesToEstimate(),
258                                                      getNumberSelectedPropagationDriversValuesToEstimate());
259         }
260 
261         // Initialize inverse of the orbital part of the state transition matrix (See Ref [1], Eq. 3.2d)
262         this.phiS = MatrixUtils.createRealIdentityMatrix(getNumberSelectedOrbitalDriversValuesToEstimate());
263 
264         // Number of estimated measurement parameters
265         final int nbMeas = getNumberSelectedMeasurementDriversValuesToEstimate();
266 
267         // Number of estimated dynamic parameters (orbital + propagation)
268         final int nbDyn  = getNumberSelectedOrbitalDriversValuesToEstimate() + getNumberSelectedPropagationDriversValuesToEstimate();
269 
270         // Covariance matrix
271         final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
272         final RealMatrix noiseP = covarianceMatrixProvider.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
273         noiseK.setSubMatrix(noiseP.getData(), 0, 0);
274         if (measurementProcessNoiseMatrix != null) {
275             final RealMatrix noiseM = measurementProcessNoiseMatrix.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
276             noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
277         }
278 
279         // Verify dimension
280         KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
281                                            builder.getOrbitalParametersDrivers(),
282                                            builder.getPropagationParametersDrivers(),
283                                            estimatedMeasurementsParameters);
284 
285         final RealMatrix correctedCovariance = KalmanEstimatorUtil.normalizeCovarianceMatrix(noiseK, scale);
286 
287         // Initialize corrected estimate
288         this.correctedEstimate = new ProcessEstimate(0.0, correctedFilterCorrection, correctedCovariance);
289 
290     }
291 
292     /** {@inheritDoc} */
293     @Override
294     public KalmanObserver getObserver() {
295         return observer;
296     }
297 
298     /** Set the observer.
299      * @param observer the observer
300      */
301     public void setObserver(final KalmanObserver observer) {
302         this.observer = observer;
303     }
304 
305     /** Get the current corrected estimate.
306      * @return current corrected estimate
307      */
308     public ProcessEstimate getEstimate() {
309         return correctedEstimate;
310     }
311 
312     /** Getter for the scale.
313      * @return the scale
314      */
315     protected double[] getScale() {
316         return scale;
317     }
318 
319     /** Process a single measurement.
320      * <p>
321      * Update the filter with the new measurements.
322      * </p>
323      * @param observedMeasurements the list of measurements to process
324      * @param filter Extended Kalman Filter
325      * @return estimated propagator
326      */
327     public DSSTPropagator processMeasurements(final List<ObservedMeasurement<?>> observedMeasurements,
328                                               final ExtendedKalmanFilter<MeasurementDecorator> filter) {
329         try {
330 
331             // Sort the measurement
332             observedMeasurements.sort(new ChronologicalComparator());
333             final AbsoluteDate tStart             = observedMeasurements.get(0).getDate();
334             final AbsoluteDate tEnd               = observedMeasurements.get(observedMeasurements.size() - 1).getDate();
335             final double       overshootTimeRange = FastMath.nextAfter(tEnd.durationFrom(tStart),
336                                                     Double.POSITIVE_INFINITY);
337 
338             // Initialize step handler and set it to the propagator
339             final SemiAnalyticalMeasurementHandler stepHandler = new SemiAnalyticalMeasurementHandler(this, filter, observedMeasurements, builder.getInitialOrbitDate());
340             dsstPropagator.getMultiplexer().add(stepHandler);
341             dsstPropagator.propagate(tStart, tStart.shiftedBy(overshootTimeRange));
342 
343             // Return the last estimated propagator
344             return getEstimatedPropagator();
345 
346         } catch (MathRuntimeException mrte) {
347             throw new OrekitException(mrte);
348         }
349     }
350 
351     /** Get the propagator estimated with the values set in the propagator builder.
352      * @return propagator based on the current values in the builder
353      */
354     public DSSTPropagator getEstimatedPropagator() {
355         // Return propagator built with current instantiation of the propagator builder
356         return (DSSTPropagator) builder.buildPropagator();
357     }
358 
359     /** {@inheritDoc} */
360     @Override
361     public NonLinearEvolution getEvolution(final double previousTime, final RealVector previousState,
362                                            final MeasurementDecorator measurement) {
363 
364         // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
365         final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
366         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
367             if (driver.getReferenceDate() == null) {
368                 driver.setReferenceDate(builder.getInitialOrbitDate());
369             }
370         }
371 
372         // Increment measurement number
373         ++currentMeasurementNumber;
374 
375         // Update the current date
376         currentDate = measurement.getObservedMeasurement().getDate();
377 
378         // Normalized state transition matrix
379         final RealMatrix stm = getErrorStateTransitionMatrix();
380 
381         // Predict filter correction
382         predictedFilterCorrection = predictFilterCorrection(stm);
383 
384         // Short period term derivatives
385         analyticalDerivativeComputations(nominalMeanSpacecraftState);
386 
387         // Calculate the predicted osculating elements
388         final double[] osculating = computeOsculatingElements(predictedFilterCorrection);
389         final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, builder.getPositionAngleType(),
390                                                                             currentDate, nominalMeanSpacecraftState.getOrbit().getMu(),
391                                                                             nominalMeanSpacecraftState.getFrame());
392 
393         // Compute the predicted measurements  (See Ref [1], Eq. 3.8)
394         predictedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
395                                                             currentMeasurementNumber,
396                                                             new SpacecraftState[] {
397                                                                 new SpacecraftState(osculatingOrbit,
398                                                                                     nominalMeanSpacecraftState.getAttitude(),
399                                                                                     nominalMeanSpacecraftState.getMass(),
400                                                                                     nominalMeanSpacecraftState.getAdditionalDataValues(),
401                                                                                     nominalMeanSpacecraftState.getAdditionalStatesDerivatives())
402                                                             });
403 
404         // Normalized measurement matrix
405         final RealMatrix measurementMatrix = getMeasurementMatrix();
406 
407         // Number of estimated measurement parameters
408         final int nbMeas = getNumberSelectedMeasurementDriversValuesToEstimate();
409 
410         // Number of estimated dynamic parameters (orbital + propagation)
411         final int nbDyn  = getNumberSelectedOrbitalDriversValuesToEstimate() + getNumberSelectedPropagationDriversValuesToEstimate();
412 
413         // Covariance matrix
414         final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
415         final RealMatrix noiseP = covarianceMatrixProvider.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
416         noiseK.setSubMatrix(noiseP.getData(), 0, 0);
417         if (measurementProcessNoiseMatrix != null) {
418             final RealMatrix noiseM = measurementProcessNoiseMatrix.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
419             noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
420         }
421 
422         // Verify dimension
423         KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
424                                            builder.getOrbitalParametersDrivers(),
425                                            builder.getPropagationParametersDrivers(),
426                                            estimatedMeasurementsParameters);
427 
428         final RealMatrix normalizedProcessNoise = KalmanEstimatorUtil.normalizeCovarianceMatrix(noiseK, scale);
429 
430         // Return
431         return new NonLinearEvolution(measurement.getTime(), predictedFilterCorrection, stm,
432                                       normalizedProcessNoise, measurementMatrix);
433     }
434 
435     /** {@inheritDoc} */
436     @Override
437     public RealVector getInnovation(final MeasurementDecorator measurement, final NonLinearEvolution evolution,
438                                     final RealMatrix innovationCovarianceMatrix) {
439 
440         // Apply the dynamic outlier filter, if it exists
441         KalmanEstimatorUtil.applyDynamicOutlierFilter(predictedMeasurement, innovationCovarianceMatrix);
442         // Compute the innovation vector
443         return KalmanEstimatorUtil.computeInnovationVector(predictedMeasurement, predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
444     }
445 
446     /** {@inheritDoc} */
447     @Override
448     public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
449                                    final ProcessEstimate estimate) {
450         // Update the process estimate
451         correctedEstimate = estimate;
452         // Corrected filter correction
453         correctedFilterCorrection = estimate.getState();
454         // Update the previous nominal mean spacecraft state
455         previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;
456         // Calculate the corrected osculating elements
457         final double[] osculating = computeOsculatingElements(correctedFilterCorrection);
458         final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, builder.getPositionAngleType(),
459                                                                             currentDate, nominalMeanSpacecraftState.getOrbit().getMu(),
460                                                                             nominalMeanSpacecraftState.getFrame());
461 
462         // Compute the corrected measurements
463         correctedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
464                                                             currentMeasurementNumber,
465                                                             new SpacecraftState[] {
466                                                                 new SpacecraftState(osculatingOrbit,
467                                                                                     nominalMeanSpacecraftState.getAttitude(),
468                                                                                     nominalMeanSpacecraftState.getMass(),
469                                                                                     nominalMeanSpacecraftState.getAdditionalDataValues(),
470                                                                                     nominalMeanSpacecraftState.getAdditionalStatesDerivatives())
471                                                             });
472         // Call the observer if the user add one
473         if (observer != null) {
474             observer.evaluationPerformed(this);
475         }
476     }
477 
478     /** {@inheritDoc} */
479     @Override
480     public void finalizeOperationsObservationGrid() {
481         // Update parameters
482         updateParameters();
483     }
484 
485     /** {@inheritDoc} */
486     @Override
487     public ParameterDriversList getEstimatedOrbitalParameters() {
488         return estimatedOrbitalParameters;
489     }
490 
491     /** {@inheritDoc} */
492     @Override
493     public ParameterDriversList getEstimatedPropagationParameters() {
494         return estimatedPropagationParameters;
495     }
496 
497     /** {@inheritDoc} */
498     @Override
499     public ParameterDriversList getEstimatedMeasurementsParameters() {
500         return estimatedMeasurementsParameters;
501     }
502 
503     /** {@inheritDoc} */
504     @Override
505     public SpacecraftState[] getPredictedSpacecraftStates() {
506         return new SpacecraftState[] {nominalMeanSpacecraftState};
507     }
508 
509     /** {@inheritDoc} */
510     @Override
511     public SpacecraftState[] getCorrectedSpacecraftStates() {
512         return new SpacecraftState[] {getEstimatedPropagator().getInitialState()};
513     }
514 
515     /** {@inheritDoc} */
516     @Override
517     public RealVector getPhysicalEstimatedState() {
518         // Method {@link ParameterDriver#getValue()} is used to get
519         // the physical values of the state.
520         // The scales'array is used to get the size of the state vector
521         final RealVector physicalEstimatedState = new ArrayRealVector(scale.length);
522         int i = 0;
523         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
524             for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
525                 physicalEstimatedState.setEntry(i++, span.getData());
526             }
527         }
528         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
529             for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
530                 physicalEstimatedState.setEntry(i++, span.getData());
531             }
532         }
533         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
534             for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
535                 physicalEstimatedState.setEntry(i++, span.getData());
536             }
537         }
538 
539         return physicalEstimatedState;
540     }
541 
542     /** {@inheritDoc} */
543     @Override
544     public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
545         // Un-normalize the estimated covariance matrix (P) from Hipparchus and return it.
546         // The covariance P is an mxm matrix where m = nbOrb + nbPropag + nbMeas
547         // For each element [i,j] of P the corresponding normalized value is:
548         // Pn[i,j] = P[i,j] / (scale[i]*scale[j])
549         // Consequently: P[i,j] = Pn[i,j] * scale[i] * scale[j]
550         return KalmanEstimatorUtil.unnormalizeCovarianceMatrix(correctedEstimate.getCovariance(), scale);
551     }
552 
553     /** {@inheritDoc} */
554     @Override
555     public RealMatrix getPhysicalStateTransitionMatrix() {
556         //  Un-normalize the state transition matrix (φ) from Hipparchus and return it.
557         // φ is an mxm matrix where m = nbOrb + nbPropag + nbMeas
558         // For each element [i,j] of normalized φ (φn), the corresponding physical value is:
559         // φ[i,j] = φn[i,j] * scale[i] / scale[j]
560         return correctedEstimate.getStateTransitionMatrix() == null ?
561                 null : KalmanEstimatorUtil.unnormalizeStateTransitionMatrix(correctedEstimate.getStateTransitionMatrix(), scale);
562     }
563 
564     /** {@inheritDoc} */
565     @Override
566     public RealMatrix getPhysicalMeasurementJacobian() {
567         // Un-normalize the measurement matrix (H) from Hipparchus and return it.
568         // H is an nxm matrix where:
569         //  - m = nbOrb + nbPropag + nbMeas is the number of estimated parameters
570         //  - n is the size of the measurement being processed by the filter
571         // For each element [i,j] of normalized H (Hn) the corresponding physical value is:
572         // H[i,j] = Hn[i,j] * σ[i] / scale[j]
573         return correctedEstimate.getMeasurementJacobian() == null ?
574                 null : KalmanEstimatorUtil.unnormalizeMeasurementJacobian(correctedEstimate.getMeasurementJacobian(),
575                                                                           scale,
576                                                                           correctedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
577     }
578 
579     /** {@inheritDoc} */
580     @Override
581     public RealMatrix getPhysicalInnovationCovarianceMatrix() {
582         // Un-normalize the innovation covariance matrix (S) from Hipparchus and return it.
583         // S is an nxn matrix where n is the size of the measurement being processed by the filter
584         // For each element [i,j] of normalized S (Sn) the corresponding physical value is:
585         // S[i,j] = Sn[i,j] * σ[i] * σ[j]
586         return correctedEstimate.getInnovationCovariance() == null ?
587                 null : KalmanEstimatorUtil.unnormalizeInnovationCovarianceMatrix(correctedEstimate.getInnovationCovariance(),
588                                                                                  predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
589     }
590 
591     /** {@inheritDoc} */
592     @Override
593     public RealMatrix getPhysicalKalmanGain() {
594         // Un-normalize the Kalman gain (K) from Hipparchus and return it.
595         // K is an mxn matrix where:
596         //  - m = nbOrb + nbPropag + nbMeas is the number of estimated parameters
597         //  - n is the size of the measurement being processed by the filter
598         // For each element [i,j] of normalized K (Kn) the corresponding physical value is:
599         // K[i,j] = Kn[i,j] * scale[i] / σ[j]
600         return correctedEstimate.getKalmanGain() == null ?
601                 null : KalmanEstimatorUtil.unnormalizeKalmanGainMatrix(correctedEstimate.getKalmanGain(),
602                                                                        scale,
603                                                                        correctedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
604     }
605 
606     /** {@inheritDoc} */
607     @Override
608     public int getCurrentMeasurementNumber() {
609         return currentMeasurementNumber;
610     }
611 
612     /** {@inheritDoc} */
613     @Override
614     public AbsoluteDate getCurrentDate() {
615         return currentDate;
616     }
617 
618     /** {@inheritDoc} */
619     @Override
620     public EstimatedMeasurement<?> getPredictedMeasurement() {
621         return predictedMeasurement;
622     }
623 
624     /** {@inheritDoc} */
625     @Override
626     public EstimatedMeasurement<?> getCorrectedMeasurement() {
627         return correctedMeasurement;
628     }
629 
630     /** {@inheritDoc} */
631     @Override
632     public void updateNominalSpacecraftState(final SpacecraftState nominal) {
633         this.nominalMeanSpacecraftState = nominal;
634         // Update the builder with the nominal mean elements orbit
635         builder.resetOrbit(nominal.getOrbit(), PropagationType.MEAN);
636 
637         // Additionally, update the builder with the predicted mass value.
638         // If any mass changes have occurred during this estimation step, such as maneuvers,
639         // the updated mass value must be carried over so that new Propagators from this builder start with the updated mass.
640         builder.setMass(nominal.getMass());
641     }
642 
643     /** Update the reference trajectories using the propagator as input.
644      * @param propagator The new propagator to use
645      */
646     public void updateReferenceTrajectory(final DSSTPropagator propagator) {
647 
648         dsstPropagator = propagator;
649 
650         // Equation name
651         final String equationName = SemiAnalyticalKalmanEstimator.class.getName() + "-derivatives-";
652 
653         // Mean state
654         final SpacecraftState meanState = dsstPropagator.initialIsOsculating() ?
655                        DSSTPropagator.computeMeanState(dsstPropagator.getInitialState(), dsstPropagator.getAttitudeProvider(), dsstPropagator.getAllForceModels()) :
656                        dsstPropagator.getInitialState();
657 
658         // Update the jacobian harvester
659         dsstPropagator.setInitialState(meanState, PropagationType.MEAN);
660         harvester = dsstPropagator.setupMatricesComputation(equationName, null, null);
661 
662     }
663 
664     /** {@inheritDoc} */
665     @Override
666     public void updateShortPeriods(final SpacecraftState state) {
667         // Loop on DSST force models
668         for (final DSSTForceModel model : builder.getAllForceModels()) {
669             model.updateShortPeriodTerms(model.getParametersAllValues(), state);
670         }
671         harvester.updateFieldShortPeriodTerms(state);
672     }
673 
674     /** {@inheritDoc} */
675     @Override
676     public void initializeShortPeriodicTerms(final SpacecraftState meanState) {
677         final List<ShortPeriodTerms> shortPeriodTerms = new ArrayList<>();
678         // initialize ForceModels in OSCULATING mode even if propagation is MEAN
679         final PropagationType type = PropagationType.OSCULATING;
680         for (final DSSTForceModel force :  builder.getAllForceModels()) {
681             shortPeriodTerms.addAll(force.initializeShortPeriodTerms(new AuxiliaryElements(meanState.getOrbit(), 1), type, force.getParameters(meanState.getDate())));
682         }
683         dsstPropagator.setShortPeriodTerms(shortPeriodTerms);
684         // also need to initialize the Field terms in the same mode
685         harvester.initializeFieldShortPeriodTerms(meanState, type);
686     }
687 
688     /** Get the normalized state transition matrix (STM) from previous point to current point.
689      * The STM contains the partial derivatives of current state with respect to previous state.
690      * The  STM is an mxm matrix where m is the size of the state vector.
691      * m = nbOrb + nbPropag + nbMeas
692      * @return the normalized error state transition matrix
693      */
694     private RealMatrix getErrorStateTransitionMatrix() {
695 
696         /* The state transition matrix is obtained as follows, with:
697          *  - Phi(k, k-1) : Transitional orbital matrix
698          *  - Psi(k, k-1) : Transitional propagation parameters matrix
699          *
700          *       |             |             |   .    |
701          *       | Phi(k, k-1) | Psi(k, k-1) | ..0..  |
702          *       |             |             |   .    |
703          *       |-------------|-------------|--------|
704          *       |      .      |    1 0 0    |   .    |
705          * STM = |    ..0..    |    0 1 0    | ..0..  |
706          *       |      .      |    0 0 1    |   .    |
707          *       |-------------|-------------|--------|
708          *       |      .      |      .      | 1 0 0..|
709          *       |    ..0..    |    ..0..    | 0 1 0..|
710          *       |      .      |      .      | 0 0 1..|
711          */
712 
713         // Initialize to the proper size identity matrix
714         final RealMatrix stm = MatrixUtils.createRealIdentityMatrix(correctedEstimate.getState().getDimension());
715 
716         // Derivatives of the state vector with respect to initial state vector
717         final int nbOrb = getNumberSelectedOrbitalDriversValuesToEstimate();
718         final RealMatrix dYdY0 = harvester.getB2(nominalMeanSpacecraftState);
719 
720         // Calculate transitional orbital matrix (See Ref [1], Eq. 3.4a)
721         final RealMatrix phi = dYdY0.multiply(phiS);
722 
723         // Fill the state transition matrix with the orbital drivers
724         final List<DelegatingDriver> drivers = builder.getOrbitalParametersDrivers().getDrivers();
725         for (int i = 0; i < nbOrb; ++i) {
726             if (drivers.get(i).isSelected()) {
727                 int jOrb = 0;
728                 for (int j = 0; j < nbOrb; ++j) {
729                     if (drivers.get(j).isSelected()) {
730                         stm.setEntry(i, jOrb++, phi.getEntry(i, j));
731                     }
732                 }
733             }
734         }
735 
736         // Update PhiS
737         phiS = new QRDecomposition(dYdY0).getSolver().getInverse();
738 
739         // Derivatives of the state vector with respect to propagation parameters
740         if (psiS != null) {
741 
742             final int nbProp = getNumberSelectedPropagationDriversValuesToEstimate();
743             final RealMatrix dYdPp = harvester.getB3(nominalMeanSpacecraftState);
744 
745             // Calculate transitional parameters matrix (See Ref [1], Eq. 3.4b)
746             final RealMatrix psi = dYdPp.subtract(phi.multiply(psiS));
747 
748             // Fill 1st row, 2nd column (dY/dPp)
749             for (int i = 0; i < nbOrb; ++i) {
750                 for (int j = 0; j < nbProp; ++j) {
751                     stm.setEntry(i, j + nbOrb, psi.getEntry(i, j));
752                 }
753             }
754 
755             // Update PsiS
756             psiS = dYdPp;
757 
758         }
759 
760         // Normalization of the STM
761         // normalized(STM)ij = STMij*Sj/Si
762         for (int i = 0; i < scale.length; i++) {
763             for (int j = 0; j < scale.length; j++ ) {
764                 stm.setEntry(i, j, stm.getEntry(i, j) * scale[j] / scale[i]);
765             }
766         }
767 
768         // Return the error state transition matrix
769         return stm;
770 
771     }
772 
773     /** Get the normalized measurement matrix H.
774      * H contains the partial derivatives of the measurement with respect to the state.
775      * H is an nxm matrix where n is the size of the measurement vector and m the size of the state vector.
776      * @return the normalized measurement matrix H
777      */
778     private RealMatrix getMeasurementMatrix() {
779 
780         // Observed measurement characteristics
781         final SpacecraftState        evaluationState     = predictedMeasurement.getStates()[0];
782         final ObservedMeasurement<?> observedMeasurement = predictedMeasurement.getObservedMeasurement();
783         final double[] sigma  = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();
784 
785         // Initialize measurement matrix H: nxm
786         // n: Number of measurements in current measurement
787         // m: State vector size
788         final RealMatrix measurementMatrix = MatrixUtils.
789                 createRealMatrix(observedMeasurement.getDimension(),
790                                  correctedEstimate.getState().getDimension());
791 
792         // Predicted orbit
793         final Orbit predictedOrbit = evaluationState.getOrbit();
794 
795         // Measurement matrix's columns related to orbital and propagation parameters
796         // ----------------------------------------------------------
797 
798         // Partial derivatives of the current Cartesian coordinates with respect to current orbital state
799         final int nbOrb  = getNumberSelectedOrbitalDrivers();
800         final int nbProp = getNumberSelectedPropagationDrivers();
801         final double[][] aCY = new double[nbOrb][nbOrb];
802         predictedOrbit.getJacobianWrtParameters(builder.getPositionAngleType(), aCY);
803         final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
804 
805         // Jacobian of the measurement with respect to current Cartesian coordinates
806         final RealMatrix dMdC = new Array2DRowRealMatrix(predictedMeasurement.getStateDerivatives(0), false);
807 
808         // Jacobian of the measurement with respect to current orbital state
809         RealMatrix dMdY = dMdC.multiply(dCdY);
810 
811         // Compute factor dShortPeriod_dMeanState = I+B1 | B4
812         final RealMatrix IpB1B4 = MatrixUtils.createRealMatrix(nbOrb, nbOrb + nbProp);
813 
814         // B1
815         final RealMatrix B1 = harvester.getB1();
816 
817         // I + B1
818         final RealMatrix I = MatrixUtils.createRealIdentityMatrix(nbOrb);
819         final RealMatrix IpB1 = I.add(B1);
820         IpB1B4.setSubMatrix(IpB1.getData(), 0, 0);
821 
822         // If there are not propagation parameters, B4 is null
823         if (psiS != null) {
824             final RealMatrix B4 = harvester.getB4();
825             IpB1B4.setSubMatrix(B4.getData(), 0, nbOrb);
826         }
827 
828         // Ref [1], Eq. 3.10
829         dMdY = dMdY.multiply(IpB1B4);
830 
831         for (int i = 0; i < dMdY.getRowDimension(); i++) {
832             for (int j = 0; j < nbOrb; j++) {
833                 final double driverScale = builder.getOrbitalParametersDrivers().getDrivers().get(j).getScale();
834                 measurementMatrix.setEntry(i, j, dMdY.getEntry(i, j) / sigma[i] * driverScale);
835             }
836 
837             int col = 0;
838             for (int j = 0; j < nbProp; j++) {
839                 final double driverScale = estimatedPropagationParameters.getDrivers().get(j).getScale();
840                 for (Span<Double> span = estimatedPropagationParameters.getDrivers().get(j).getValueSpanMap().getFirstSpan();
841                                   span != null; span = span.next()) {
842 
843                     measurementMatrix.setEntry(i, col + nbOrb,
844                                                dMdY.getEntry(i, col + nbOrb) / sigma[i] * driverScale);
845                     col++;
846                 }
847             }
848         }
849 
850         // Normalized measurement matrix's columns related to measurement parameters
851         // --------------------------------------------------------------
852 
853         // Jacobian of the measurement with respect to measurement parameters
854         // Gather the measurement parameters linked to current measurement
855         for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
856             if (driver.isSelected()) {
857                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
858                     // Derivatives of current measurement w/r to selected measurement parameter
859                     final double[] aMPm = predictedMeasurement.getParameterDerivatives(driver, span.getStart());
860 
861                     // Check that the measurement parameter is managed by the filter
862                     if (measurementParameterColumns.get(span.getData()) != null) {
863                         // Column of the driver in the measurement matrix
864                         final int driverColumn = measurementParameterColumns.get(span.getData());
865 
866                         // Fill the corresponding indexes of the measurement matrix
867                         for (int i = 0; i < aMPm.length; ++i) {
868                             measurementMatrix.setEntry(i, driverColumn, aMPm[i] / sigma[i] * driver.getScale());
869                         }
870                     }
871                 }
872             }
873         }
874 
875         return measurementMatrix;
876     }
877 
878     /** Predict the filter correction for the new observation.
879      * @param stm normalized state transition matrix
880      * @return the predicted filter correction for the new observation
881      */
882     private RealVector predictFilterCorrection(final RealMatrix stm) {
883         // Ref [1], Eq. 3.5a and 3.5b
884         return stm.operate(correctedFilterCorrection);
885     }
886 
887     /** Compute the predicted osculating elements.
888      * @param filterCorrection kalman filter correction
889      * @return the predicted osculating element
890      */
891     private double[] computeOsculatingElements(final RealVector filterCorrection) {
892 
893         // Number of estimated orbital parameters
894         final int nbOrb = getNumberSelectedOrbitalDrivers();
895 
896         // B1
897         final RealMatrix B1 = harvester.getB1();
898 
899         // Short periodic terms
900         final double[] shortPeriodTerms = dsstPropagator.getShortPeriodTermsValue(nominalMeanSpacecraftState);
901 
902         // Physical filter correction
903         final RealVector physicalFilterCorrection = MatrixUtils.createRealVector(nbOrb);
904         for (int index = 0; index < nbOrb; index++) {
905             physicalFilterCorrection.addToEntry(index, filterCorrection.getEntry(index) * scale[index]);
906         }
907 
908         // B1 * physicalCorrection
909         final RealVector B1Correction = B1.operate(physicalFilterCorrection);
910 
911         // Nominal mean elements
912         final double[] nominalMeanElements = new double[nbOrb];
913         OrbitType.EQUINOCTIAL.mapOrbitToArray(nominalMeanSpacecraftState.getOrbit(), builder.getPositionAngleType(), nominalMeanElements, null);
914 
915         // Ref [1] Eq. 3.6
916         final double[] osculatingElements = new double[nbOrb];
917         for (int i = 0; i < nbOrb; i++) {
918             osculatingElements[i] = nominalMeanElements[i] +
919                                     physicalFilterCorrection.getEntry(i) +
920                                     shortPeriodTerms[i] +
921                                     B1Correction.getEntry(i);
922         }
923 
924         // Return
925         return osculatingElements;
926 
927     }
928 
929     /** Analytical computation of derivatives.
930      * This method allow to compute analytical derivatives.
931      * @param state mean state used to calculate short period perturbations
932      */
933     private void analyticalDerivativeComputations(final SpacecraftState state) {
934         harvester.setReferenceState(state);
935     }
936 
937     /** Get the number of estimated orbital parameters.
938      * @return the number of estimated orbital parameters
939      */
940     private int getNumberSelectedOrbitalDrivers() {
941         return estimatedOrbitalParameters.getNbParams();
942     }
943 
944     /** Get the number of estimated propagation parameters.
945      * @return the number of estimated propagation parameters
946      */
947     private int getNumberSelectedPropagationDrivers() {
948         return estimatedPropagationParameters.getNbParams();
949     }
950 
951     /** Get the number of estimated orbital parameters values (some parameter
952      * driver may have several values to estimate for different time range
953      * {@link ParameterDriver}.
954      * @return the number of estimated values for , orbital parameters
955      */
956     private int getNumberSelectedOrbitalDriversValuesToEstimate() {
957         int nbOrbitalValuesToEstimate = 0;
958         for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
959             nbOrbitalValuesToEstimate += driver.getNbOfValues();
960         }
961         return nbOrbitalValuesToEstimate;
962     }
963 
964     /** Get the number of estimated propagation parameters values (some parameter
965      * driver may have several values to estimate for different time range
966      * {@link ParameterDriver}.
967      * @return the number of estimated values for propagation parameters
968      */
969     private int getNumberSelectedPropagationDriversValuesToEstimate() {
970         int nbPropagationValuesToEstimate = 0;
971         for (final ParameterDriver driver : estimatedPropagationParameters.getDrivers()) {
972             nbPropagationValuesToEstimate += driver.getNbOfValues();
973         }
974         return nbPropagationValuesToEstimate;
975     }
976 
977     /** Get the number of estimated measurement parameters values (some parameter
978      * driver may have several values to estimate for different time range
979      * {@link ParameterDriver}.
980      * @return the number of estimated values for measurement parameters
981      */
982     private int getNumberSelectedMeasurementDriversValuesToEstimate() {
983         int nbMeasurementValuesToEstimate = 0;
984         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
985             nbMeasurementValuesToEstimate += driver.getNbOfValues();
986         }
987         return nbMeasurementValuesToEstimate;
988     }
989 
990     /** Update the estimated parameters after the correction phase of the filter.
991      * The min/max allowed values are handled by the parameter themselves.
992      */
993     private void updateParameters() {
994         final RealVector correctedState = correctedEstimate.getState();
995         int i = 0;
996         // Orbital parameters
997         for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
998             // let the parameter handle min/max clipping
999             for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
1000                 driver.setNormalizedValue(driver.getNormalizedValue(span.getStart()) + correctedState.getEntry(i++), span.getStart());
1001             }
1002         }
1003 
1004         // Propagation parameters
1005         for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
1006             // let the parameter handle min/max clipping
1007             // If the parameter driver contains only 1 value to estimate over the all time range
1008             for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
1009                 driver.setNormalizedValue(driver.getNormalizedValue(span.getStart()) + correctedState.getEntry(i++), span.getStart());
1010             }
1011         }
1012 
1013         // Measurements parameters
1014         for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
1015             // let the parameter handle min/max clipping
1016             for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
1017                 driver.setNormalizedValue(driver.getNormalizedValue(span.getStart()) + correctedState.getEntry(i++), span.getStart());
1018             }
1019         }
1020     }
1021 
1022 }