1 /* Copyright 2002-2026 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.propagation.covariance;
18
19 import org.orekit.frames.Frame;
20 import org.orekit.frames.LOFType;
21 import org.orekit.orbits.Orbit;
22 import org.orekit.orbits.OrbitType;
23 import org.orekit.orbits.PositionAngleType;
24 import org.orekit.time.AbsoluteDate;
25 import org.orekit.time.AbstractTimeInterpolator;
26 import org.orekit.time.TimeInterpolator;
27 import org.orekit.time.TimeStamped;
28 import org.orekit.time.TimeStampedPair;
29 import org.orekit.utils.SortedListTrimmer;
30
31 import java.util.Collections;
32 import java.util.List;
33 import java.util.stream.Collectors;
34 import java.util.stream.Stream;
35
36 /**
37 * Abstract class for orbit and state covariance interpolator.
38 *
39 * @author Vincent Cucchietti
40 * @see Orbit
41 * @see StateCovariance
42 * @see TimeStampedPair
43 */
44 public abstract class AbstractStateCovarianceInterpolator
45 extends AbstractTimeInterpolator<TimeStampedPair<Orbit, StateCovariance>> {
46
47 /** Default position angle for covariance expressed in Cartesian elements. */
48 public static final PositionAngleType DEFAULT_POSITION_ANGLE = PositionAngleType.MEAN;
49
50 /** Default column dimension for position-velocity state covariance. */
51 public static final int COLUMN_DIM = 6;
52
53 /** Default row dimension for position-velocity state covariance. */
54 public static final int ROW_DIM = 6;
55
56 /** Output frame. */
57 private final Frame outFrame;
58
59 /** Output local orbital frame. */
60 private final LOFType outLOF;
61
62 /** Output orbit type. */
63 private final OrbitType outOrbitType;
64
65 /** Output position angle type. */
66 private final PositionAngleType outPositionAngleType;
67
68 /** Orbit interpolator. */
69 private final TimeInterpolator<Orbit> orbitInterpolator;
70
71 /**
72 * Constructor.
73 *
74 * @param interpolationPoints number of interpolation points
75 * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
76 * @param orbitInterpolator orbit interpolator
77 * @param outLOF local orbital frame
78 *
79 * @see Frame
80 * @see OrbitType
81 * @see PositionAngleType
82 */
83 protected AbstractStateCovarianceInterpolator(final int interpolationPoints, final double extrapolationThreshold,
84 final TimeInterpolator<Orbit> orbitInterpolator,
85 final LOFType outLOF) {
86 super(interpolationPoints, extrapolationThreshold);
87 this.orbitInterpolator = orbitInterpolator;
88 this.outLOF = outLOF;
89 this.outFrame = null;
90 this.outOrbitType = OrbitType.CARTESIAN;
91 this.outPositionAngleType = DEFAULT_POSITION_ANGLE;
92 }
93
94 /**
95 * Constructor.
96 *
97 * @param interpolationPoints number of interpolation points
98 * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
99 * @param orbitInterpolator orbit interpolator
100 * @param outFrame desired output covariance frame
101 * @param outPositionAngleType desired output position angle
102 * @param outOrbitType desired output orbit type
103 *
104 * @see Frame
105 * @see OrbitType
106 * @see PositionAngleType
107 */
108 protected AbstractStateCovarianceInterpolator(final int interpolationPoints, final double extrapolationThreshold,
109 final TimeInterpolator<Orbit> orbitInterpolator,
110 final Frame outFrame, final OrbitType outOrbitType,
111 final PositionAngleType outPositionAngleType) {
112 super(interpolationPoints, extrapolationThreshold);
113 this.orbitInterpolator = orbitInterpolator;
114 this.outLOF = null;
115 this.outFrame = outFrame;
116 this.outOrbitType = outOrbitType;
117 this.outPositionAngleType = outPositionAngleType;
118 }
119
120 /** {@inheritDoc} */
121 @Override
122 public List<TimeInterpolator<? extends TimeStamped>> getSubInterpolators() {
123 return Collections.singletonList(orbitInterpolator);
124 }
125
126 /**
127 * Interpolate orbit and associated covariance.
128 *
129 * @param interpolationData interpolation data
130 *
131 * @return interpolated orbit and associated covariance
132 */
133 @Override
134 public TimeStampedPair<Orbit, StateCovariance> interpolate(final InterpolationData interpolationData) {
135
136 // Interpolate orbit at interpolation date
137 final Orbit interpolatedOrbit = interpolateOrbit(interpolationData.getInterpolationDate(),
138 interpolationData.getNeighborList());
139
140 // Ensure that the chosen number of interpolation points is used to interpolate the state covariance
141 final List<TimeStampedPair<Orbit, StateCovariance>> uncertainStates = getNeighborsSubList(interpolationData);
142
143 // Rebuild state covariance
144 final StateCovariance covarianceInOrbitFrame =
145 computeInterpolatedCovarianceInOrbitFrame(uncertainStates, interpolatedOrbit);
146
147 // Output new blended StateCovariance instance in desired output
148 return expressCovarianceInDesiredOutput(interpolatedOrbit, covarianceInOrbitFrame);
149 }
150
151 /**
152 * Extract the correct number of interpolation points for state covariance interpolation/blending. Otherwise,
153 * sub-interpolators may require more samples and state covariance interpolation itself would use the wrong number
154 * of samples.
155 *
156 * @param interpolationData interpolation data
157 * @return Sample specific to state covariance interpolation/blending
158 */
159 private List<TimeStampedPair<Orbit, StateCovariance>> getNeighborsSubList(final InterpolationData interpolationData) {
160 final List<TimeStampedPair<Orbit, StateCovariance>> neighborList = interpolationData.getNeighborList();
161
162 // Handle special case where sub-interpolator uses the same number of interpolation points
163 if (getNbInterpolationPoints() == getInternalNbInterpolationPoints()) {
164 return interpolationData.getNeighborList();
165 }
166
167 // Otherwise, select sublist around interpolation date
168 final AbsoluteDate central = getCentralDate(interpolationData.getInterpolationDate(),
169 neighborList.getFirst().getDate(),
170 neighborList.getLast().getDate(),
171 getExtrapolationThreshold());
172
173 return new SortedListTrimmer(getInternalNbInterpolationPoints()).getNeighborsSubList(central,
174 interpolationData.getNeighborList());
175 }
176
177 /** Get output frame.
178 * @return output frame. Can be null.
179 */
180 public Frame getOutFrame() {
181 return outFrame;
182 }
183
184 /** Get output local orbital frame.
185 * @return output local orbital frame. Can be null.
186 */
187 public LOFType getOutLOF() {
188 return outLOF;
189 }
190
191 /** Get output orbit type.
192 * @return output orbit type.
193 */
194 public OrbitType getOutOrbitType() {
195 return outOrbitType;
196 }
197
198 /** Get output position angle type.
199 * @return output position angle.
200 */
201 public PositionAngleType getOutPositionAngleType() {
202 return outPositionAngleType;
203 }
204
205 /** Get orbit interpolator.
206 * @return orbit interpolator.
207 */
208 public TimeInterpolator<Orbit> getOrbitInterpolator() {
209 return orbitInterpolator;
210 }
211
212 /**
213 * Interpolate orbit at given interpolation date.
214 *
215 * @param interpolationDate interpolation date
216 * @param neighborList neighbor list
217 *
218 * @return interpolated orbit
219 */
220 protected Orbit interpolateOrbit(final AbsoluteDate interpolationDate,
221 final List<TimeStampedPair<Orbit, StateCovariance>> neighborList) {
222
223 // Build orbit list from uncertain orbits
224 final List<Orbit> orbits = buildOrbitList(neighborList);
225
226 return orbitInterpolator.interpolate(interpolationDate, orbits);
227 }
228
229 /**
230 * Compute the interpolated covariance expressed in the interpolated orbit frame.
231 *
232 * @param uncertainStates list of orbits and associated covariances
233 * @param interpolatedOrbit interpolated orbit
234 *
235 * @return interpolated covariance expressed in the interpolated orbit frame
236 */
237 protected abstract StateCovariance computeInterpolatedCovarianceInOrbitFrame(
238 List<TimeStampedPair<Orbit, StateCovariance>> uncertainStates,
239 Orbit interpolatedOrbit);
240
241 /**
242 * Express covariance in output configuration defined at this instance construction.
243 *
244 * @param interpolatedOrbit interpolated orbit
245 * @param covarianceInOrbitFrame covariance expressed in interpolated orbit frame
246 *
247 * @return covariance in desired output configuration
248 */
249 protected TimeStampedPair<Orbit, StateCovariance> expressCovarianceInDesiredOutput(final Orbit interpolatedOrbit,
250 final StateCovariance covarianceInOrbitFrame) {
251
252 final StateCovariance covarianceOutput;
253
254 // Output frame is defined
255 if (outLOF == null) {
256
257 // Output frame is pseudo inertial
258 if (outFrame.isPseudoInertial()) {
259 final StateCovariance covarianceInOutputFrame =
260 covarianceInOrbitFrame.changeCovarianceFrame(interpolatedOrbit, outFrame);
261
262 covarianceOutput =
263 covarianceInOutputFrame.changeCovarianceType(interpolatedOrbit, outOrbitType, outPositionAngleType);
264 }
265 // Output frame is not pseudo inertial
266 else {
267 covarianceOutput = covarianceInOrbitFrame.changeCovarianceFrame(interpolatedOrbit, outFrame);
268 }
269
270 }
271 // Output local orbital frame is defined
272 else {
273 covarianceOutput = covarianceInOrbitFrame.changeCovarianceFrame(interpolatedOrbit, outLOF);
274 }
275
276 return new TimeStampedPair<>(interpolatedOrbit, covarianceOutput);
277 }
278
279 /**
280 * Build an orbit list from cached samples.
281 *
282 * @param neighborList neighbor list
283 *
284 * @return orbit list
285 */
286 private List<Orbit> buildOrbitList(final List<TimeStampedPair<Orbit, StateCovariance>> neighborList) {
287
288 // Get samples stream
289 final Stream<TimeStampedPair<Orbit, StateCovariance>> uncertainStateStream = neighborList.stream();
290
291 // Map to orbit
292 final Stream<Orbit> orbitStream = uncertainStateStream.map(TimeStampedPair::getFirst);
293
294 // Convert to list
295 return orbitStream.collect(Collectors.toList());
296 }
297 }