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 org.hipparchus.filtering.kalman.KalmanSmoother;
20  import org.hipparchus.filtering.kalman.ProcessEstimate;
21  import org.hipparchus.linear.MatrixUtils;
22  import org.hipparchus.linear.RealMatrix;
23  import org.hipparchus.linear.RealVector;
24  import org.orekit.time.AbsoluteDate;
25  import org.orekit.utils.ParameterDriver;
26  import org.orekit.utils.ParameterDriversList;
27  
28  import java.util.ArrayList;
29  import java.util.List;
30  
31  /** Perform an RTS (Rauch-Tung-Striebel) smoothing step over results from a sequential estimator.
32   *
33   * <p>The Kalman and Unscented sequential estimators produce a state (mean and covariance) after processing each
34   * measurement.  This state is a statistical summary of all the information provided to the filter, from the
35   * measurements and model of the spacecraft motion, up until the latest measurement.  A smoother produces estimates that
36   * are summaries of information over <em>all</em> measurements, both past and future.</p>
37   *
38   * <p>For example, if a filter processes
39   * measurements from time 1 to 10, then the filter state at time 5 uses measurement information up to time 5, while
40   * the smoother state at time 5 uses measurement information from the entire interval, times 1 to 10.  This typically
41   * results in more accurate estimates, with more information reducing the uncertainty.</p>
42   *
43   * <p>This smoother is implemented using the {@link KalmanObserver} mechanism.  The
44   * smoother collects data from the forward estimation over the measurements, then applies a backward pass to
45   * calculate the smoothed estimates.  Smoothed estimates are collected into a list of
46   * {@link PhysicalEstimatedState}, containing a timestamp, mean and covariance over all estimated parameters
47   * (orbital, propagation and measurement).  The order of the parameters in these states is the same as the
48   * underlying sequential estimator, for example from a call to {@link KalmanEstimator#getPhysicalEstimatedState()}.</p>
49   *
50   * <p>The smoother is compatible with the Kalman and Unscented sequential estimators, but does not support the
51   * semi-analytical equivalents.</p>
52   *
53   * <p>The following code snippet demonstrates how to attach the smoother to a filter and retrieve smoothed states:</p>
54   *
55   * <pre>
56   *     // Build the Kalman filter
57   *     final KalmanEstimator kalmanEstimator = new KalmanEstimatorBuilder().
58   *         addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
59   *         build();
60   *
61   *     // Add smoother observer to filter
62   *     final RtsSmoother rtsSmoother = new RtsSmoother(kalmanEstimator);
63   *     kalmanEstimator.setObserver(rtsSmoother);
64   *
65   *     // Perform forward filtering over the measurements
66   *     Propagator[] estimated = kalmanEstimator.processMeasurements(measurements);
67   *
68   *     // Perform backwards smoothing and collect the results
69   *     rtsSmoother.backwardsSmooth();
70   * </pre>
71   *
72   * <p>Note that the smoother stores data from every filter step, leading to high memory usage for long-duration runs
73   * with numerous measurements.</p>
74   *
75   * @see KalmanEstimatorBuilder
76   * @see UnscentedKalmanEstimatorBuilder
77   * @see "S&auml;rkk&auml; S. Bayesian Filtering and Smoothing. Cambridge University Press, 2013."
78   * @author Mark Rutten
79   * @since 13.0
80   */
81  public class RtsSmoother implements KalmanObserver {
82  
83      /** Smoother. */
84      private final KalmanSmoother smoother;
85  
86      /** Estimator reference date. */
87      private final AbsoluteDate referenceDate;
88  
89      /** Covariance scales for unnormalising estimates. */
90      private final double[] covarianceScale;
91  
92      /** Estimated orbital parameters. */
93      private final ParameterDriversList estimatedOrbitalParameters;
94  
95      /** Estimated propagation drivers. */
96      private final ParameterDriversList estimatedPropagationParameters;
97  
98      /** Estimated measurements parameters. */
99      private final ParameterDriversList estimatedMeasurementsParameters;
100 
101     /** Reference states for unnormalising estimates. */
102     private final List<RealVector> referenceStates;
103 
104     /** Smoother observer constructor from a sequential estimator.
105      * This smoother constructor requires access to the underlying estimator to initialise some information not
106      * available from {@link KalmanEstimation} during {@link RtsSmoother#init}, including the estimated parameters
107      * drivers (orbital, propagation and measurements).
108      * @param estimator the Kalman estimator
109      */
110     public RtsSmoother(final AbstractKalmanEstimator estimator) {
111         this.smoother = new KalmanSmoother(estimator.getMatrixDecomposer());
112         this.referenceDate = estimator.getReferenceDate();
113         this.covarianceScale = estimator.getScale();
114         this.estimatedOrbitalParameters = estimator.getOrbitalParametersDrivers(true);
115         this.estimatedPropagationParameters = estimator.getPropagationParametersDrivers(true);
116         this.estimatedMeasurementsParameters = estimator.getEstimatedMeasurementsParameters();
117         this.referenceStates = new ArrayList<>();
118 
119         // Add smoother observer to underlying filter
120         estimator.getKalmanFilter().setObserver(smoother);
121     }
122 
123     /** {@inheritDoc}
124      */
125     @Override
126     public void init(final KalmanEstimation estimation) {
127         // Get the first reference state
128         referenceStates.add(getReferenceState());
129     }
130 
131     /** {@inheritDoc} This accumulates the filter states as the sequential estimator processes measurements.
132      */
133     @Override
134     public void evaluationPerformed(final KalmanEstimation estimation) {
135         referenceStates.add(getReferenceState());
136     }
137 
138     /** Perform a RTS backwards smoothing recursion over the filtered states collected by the observer.
139      * @return a list of {@link PhysicalEstimatedState}
140      */
141     public List<PhysicalEstimatedState> backwardsSmooth() {
142 
143         // Backwards smoothing step
144         final List<ProcessEstimate> normalisedStates = smoother.backwardsSmooth();
145 
146         // Convert to physical states
147         final List<PhysicalEstimatedState> smoothedStates = new ArrayList<>();
148         for (int timeIndex = 0; timeIndex < normalisedStates.size(); ++timeIndex) {
149             final ProcessEstimate normalisedState =  normalisedStates.get(timeIndex);
150             final RealVector physicalState =
151                     getPhysicalState(normalisedState.getState(), referenceStates.get(timeIndex));
152             final RealMatrix physicalCovariance =
153                     KalmanEstimatorUtil.unnormalizeCovarianceMatrix(normalisedState.getCovariance(), covarianceScale);
154             smoothedStates.add(new PhysicalEstimatedState(
155                     referenceDate.shiftedBy(normalisedState.getTime()),
156                     physicalState,
157                     physicalCovariance
158             ));
159         }
160         return smoothedStates;
161     }
162 
163     /** Get reference values from the estimation parameter drivers.
164      * @return the reference state
165      */
166     private RealVector getReferenceState() {
167         final RealVector referenceState = MatrixUtils.createRealVector(covarianceScale.length);
168         int i = 0;
169         for (final ParameterDriversList.DelegatingDriver driver : estimatedOrbitalParameters.getDrivers()) {
170             referenceState.setEntry(i++, driver.getReferenceValue());
171         }
172         for (final ParameterDriversList.DelegatingDriver driver : estimatedPropagationParameters.getDrivers()) {
173             referenceState.setEntry(i++, driver.getReferenceValue());
174         }
175         for (final ParameterDriversList.DelegatingDriver driver : estimatedMeasurementsParameters.getDrivers()) {
176             referenceState.setEntry(i++, driver.getReferenceValue());
177         }
178         return referenceState;
179     }
180 
181 
182     /** Get reference values from the estimation parameter drivers.
183      * @param normalisedState the normalised state
184      * @param referenceState the reference state
185      * @return the reference state
186      */
187     private RealVector getPhysicalState(final RealVector normalisedState, final RealVector referenceState) {
188         final RealVector physicalState = MatrixUtils.createRealVector(covarianceScale.length);
189         int i = 0;
190         for (final ParameterDriversList.DelegatingDriver driver : estimatedOrbitalParameters.getDrivers()) {
191             physicalState.setEntry(i, setResetDriver(driver, referenceState.getEntry(i), normalisedState.getEntry(i)));
192             i += 1;
193         }
194         for (final ParameterDriversList.DelegatingDriver driver : estimatedPropagationParameters.getDrivers()) {
195             physicalState.setEntry(i, setResetDriver(driver, referenceState.getEntry(i), normalisedState.getEntry(i)));
196             i += 1;
197         }
198         for (final ParameterDriversList.DelegatingDriver driver : estimatedMeasurementsParameters.getDrivers()) {
199             physicalState.setEntry(i, setResetDriver(driver, referenceState.getEntry(i), normalisedState.getEntry(i)));
200             i += 1;
201         }
202         return physicalState;
203     }
204 
205     /** Use a driver to extract a new value, given reference and normalised values,
206      * resetting the state of the driver afterward.
207      * @param driver the parameter driver
208      * @param referenceValue the new reference value
209      * @param normalisedVale the new normalised value
210      * @return the reference state
211      */
212     private double setResetDriver(final ParameterDriver driver,
213                                   final double referenceValue,
214                                   final double normalisedVale) {
215         // Record old driver parameters
216         final double oldReference = driver.getReferenceValue();
217         final double oldValue = driver.getNormalizedValue();
218 
219         // Set driver to new parameters
220         driver.setReferenceValue(referenceValue);
221         driver.setNormalizedValue(normalisedVale);
222         final double physicalValue = driver.getValue();
223 
224         // Reset driver to old
225         driver.setReferenceValue(oldReference);
226         driver.setNormalizedValue(oldValue);
227 
228         return physicalValue;
229     }
230 
231 }