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 java.util.List;
20
21 import org.hipparchus.exception.MathRuntimeException;
22 import org.hipparchus.filtering.kalman.KalmanFilter;
23 import org.hipparchus.filtering.kalman.ProcessEstimate;
24 import org.orekit.errors.OrekitException;
25 import org.orekit.estimation.measurements.ObservedMeasurement;
26 import org.orekit.propagation.SpacecraftState;
27 import org.orekit.propagation.sampling.OrekitStepHandler;
28 import org.orekit.propagation.sampling.OrekitStepInterpolator;
29 import org.orekit.time.AbsoluteDate;
30
31 /** {@link org.orekit.propagation.sampling.OrekitStepHandler Step handler} picking up
32 * {@link ObservedMeasurement measurements} for both {@link SemiAnalyticalUnscentedKalmanEstimator} and {@link SemiAnalyticalKalmanEstimator}.
33 * @author Gaëtan Pierre
34 * @author Bryan Cazabonne
35 * @author Julie Bayard
36 * @author Maxime Journot
37 * @since 11.3
38 */
39 public class SemiAnalyticalMeasurementHandler implements OrekitStepHandler {
40
41 /** Index of the next measurement component in the model. */
42 private int index;
43
44 /** Reference date. */
45 private AbsoluteDate referenceDate;
46
47 /** Kalman model. */
48 private final SemiAnalyticalProcess model;
49
50 /** Kalman Filter. */
51 private final KalmanFilter<MeasurementDecorator> filter;
52
53 /** Underlying measurements. */
54 private final List<ObservedMeasurement<?>> observedMeasurements;
55
56 /** Flag indicating if the handler is used by a unscented kalman filter. */
57 private final boolean isUnscented;
58
59 /** Simple constructor.
60 * <p>
61 * Using this constructor, the Kalman filter is supposed to be extended.
62 * </p>
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 * @see #SemiAnalyticalMeasurementHandler(SemiAnalyticalProcess, KalmanFilter, List, AbsoluteDate, boolean)
68 */
69 public SemiAnalyticalMeasurementHandler(final SemiAnalyticalProcess model,
70 final KalmanFilter<MeasurementDecorator> filter,
71 final List<ObservedMeasurement<?>> observedMeasurements,
72 final AbsoluteDate referenceDate) {
73 this(model, filter, observedMeasurements, referenceDate, false);
74 }
75
76 /** Simple constructor.
77 * @param model semi-analytical kalman model
78 * @param filter kalman filter instance
79 * @param observedMeasurements list of observed measurements
80 * @param referenceDate reference date
81 * @param isUnscented true if the Kalman filter is unscented
82 * @since 11.3.2
83 */
84 public SemiAnalyticalMeasurementHandler(final SemiAnalyticalProcess model,
85 final KalmanFilter<MeasurementDecorator> filter,
86 final List<ObservedMeasurement<?>> observedMeasurements,
87 final AbsoluteDate referenceDate,
88 final boolean isUnscented) {
89 this.model = model;
90 this.filter = filter;
91 this.observedMeasurements = observedMeasurements;
92 this.referenceDate = referenceDate;
93 this.isUnscented = isUnscented;
94 }
95
96 /** {@inheritDoc} */
97 @Override
98 public void init(final SpacecraftState s0, final AbsoluteDate t) {
99 this.index = 0;
100 // Initialize short periodic terms.
101 model.initializeShortPeriodicTerms(s0);
102 model.updateShortPeriods(s0);
103 }
104
105 /** {@inheritDoc} */
106 @Override
107 public void handleStep(final OrekitStepInterpolator interpolator) {
108
109 // Current date
110 final AbsoluteDate currentDate = interpolator.getCurrentState().getDate();
111
112 // Update the short period terms with the current MEAN state
113 model.updateShortPeriods(interpolator.getCurrentState());
114
115 // Process the measurements between previous step and current step
116 while (index < observedMeasurements.size() && observedMeasurements.get(index).getDate().compareTo(currentDate) < 0) {
117
118 try {
119
120 // Update predicted spacecraft state
121 model.updateNominalSpacecraftState(interpolator.getInterpolatedState(observedMeasurements.get(index).getDate()));
122
123 // Process the current observation
124 final MeasurementDecorator decorated = isUnscented ?
125 KalmanEstimatorUtil.decorateUnscented(observedMeasurements.get(index), referenceDate) :
126 KalmanEstimatorUtil.decorate(observedMeasurements.get(index), referenceDate);
127 final ProcessEstimate estimate = filter.estimationStep(decorated);
128
129 // Finalize the estimation
130 model.finalizeEstimation(observedMeasurements.get(index), estimate);
131
132 } catch (MathRuntimeException mrte) {
133 throw new OrekitException(mrte);
134 }
135
136 // Increment the measurement index
137 index += 1;
138
139 }
140
141 // Reset the initial state of the propagator
142 model.finalizeOperationsObservationGrid();
143
144 }
145
146 }