1   /* Copyright 2002-2022 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.exception.MathRuntimeException;
22  import org.hipparchus.filtering.kalman.ProcessEstimate;
23  import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
24  import org.hipparchus.linear.MatrixUtils;
25  import org.hipparchus.linear.RealMatrix;
26  import org.orekit.errors.OrekitException;
27  import org.orekit.estimation.measurements.ObservedMeasurement;
28  import org.orekit.estimation.measurements.PV;
29  import org.orekit.estimation.measurements.Position;
30  import org.orekit.propagation.SpacecraftState;
31  import org.orekit.propagation.sampling.OrekitStepHandler;
32  import org.orekit.propagation.sampling.OrekitStepInterpolator;
33  import org.orekit.time.AbsoluteDate;
34  
35  /** {@link org.orekit.propagation.sampling.OrekitStepHandler Step handler} picking up
36   * {@link ObservedMeasurement measurements} for the {@link SemiAnalyticalKalmanEstimator}.
37   * @author Julie Bayard
38   * @author Bryan Cazabonne
39   * @author Maxime Journot
40   * @since 11.1
41   */
42  public class EskfMeasurementHandler implements OrekitStepHandler {
43  
44      /** Least squares model. */
45      private final SemiAnalyticalKalmanModel model;
46  
47      /** Extended Kalman Filter. */
48      private final ExtendedKalmanFilter<MeasurementDecorator> filter;
49  
50      /** Underlying measurements. */
51      private final List<ObservedMeasurement<?>> observedMeasurements;
52  
53      /** Index of the next measurement component in the model. */
54      private int index;
55  
56      /** Reference date. */
57      private AbsoluteDate referenceDate;
58  
59      /** Observer to retrieve current estimation info. */
60      private KalmanObserver observer;
61  
62      /** Simple constructor.
63       * @param model semi-analytical kalman model
64       * @param filter kalman filter instance
65       * @param observedMeasurements list of observed measurements
66       * @param referenceDate reference date
67       */
68      public EskfMeasurementHandler(final SemiAnalyticalKalmanModel model,
69                                    final ExtendedKalmanFilter<MeasurementDecorator> filter,
70                                    final List<ObservedMeasurement<?>> observedMeasurements,
71                                    final AbsoluteDate referenceDate) {
72          this.model                = model;
73          this.filter               = filter;
74          this.observer             = model.getObserver();
75          this.observedMeasurements = observedMeasurements;
76          this.referenceDate        = referenceDate;
77      }
78  
79      /** {@inheritDoc} */
80      @Override
81      public void init(final SpacecraftState s0, final AbsoluteDate t) {
82          this.index = 0;
83          // Initialize short periodic terms.
84          model.initializeShortPeriodicTerms(s0);
85          model.updateShortPeriods(s0);
86      }
87  
88      /** {@inheritDoc} */
89      @Override
90      public void handleStep(final OrekitStepInterpolator interpolator) {
91  
92          // Current date
93          final AbsoluteDate currentDate = interpolator.getCurrentState().getDate();
94  
95          // Update the short period terms with the current MEAN state
96          model.updateShortPeriods(interpolator.getCurrentState());
97  
98          // Process the measurements between previous step and current step
99          while (index < observedMeasurements.size() && observedMeasurements.get(index).getDate().compareTo(currentDate) < 0) {
100 
101             try {
102 
103                 // Update the norminal state with the interpolated parameters
104                 model.updateNominalSpacecraftState(interpolator.getInterpolatedState(observedMeasurements.get(index).getDate()));
105 
106                 // Process the current observation
107                 final ProcessEstimate estimate = filter.estimationStep(decorate(observedMeasurements.get(index)));
108 
109                 // Finalize the estimation
110                 model.finalizeEstimation(observedMeasurements.get(index), estimate);
111 
112                 // Call the observer if the user add one
113                 if (observer != null) {
114                     observer.evaluationPerformed(model);
115                 }
116 
117             } catch (MathRuntimeException mrte) {
118                 throw new OrekitException(mrte);
119             }
120 
121             // Increment the measurement index
122             index += 1;
123 
124         }
125 
126         // Reset the initial state of the propagator
127         model.finalizeOperationsObservationGrid();
128 
129     }
130 
131     /** Decorate an observed measurement.
132      * <p>
133      * The "physical" measurement noise matrix is the covariance matrix of the measurement.
134      * Normalizing it consists in applying the following equation: Rn[i,j] =  R[i,j]/σ[i]/σ[j]
135      * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
136      * between the different components of the measurement.
137      * </p>
138      * @param observedMeasurement the measurement
139      * @return decorated measurement
140      */
141     private MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement) {
142 
143         // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
144         // of the measurement on its non-diagonal elements.
145         // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
146         // Normalizing it leaves us with the matrix of the correlation coefficients
147         final RealMatrix covariance;
148         if (observedMeasurement instanceof PV) {
149             // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
150             final PV pv = (PV) observedMeasurement;
151             covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
152         } else if (observedMeasurement instanceof Position) {
153             // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
154             final Position position = (Position) observedMeasurement;
155             covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
156         } else {
157             // For other measurements we do not have a covariance matrix.
158             // Thus the correlation coefficients matrix is an identity matrix.
159             covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
160         }
161 
162         return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
163 
164     }
165 
166 }