1 /* Copyright 2002-2023 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.utils;
18
19 import org.hipparchus.analysis.interpolation.HermiteInterpolator;
20 import org.hipparchus.geometry.euclidean.threed.Rotation;
21 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.util.FastMath;
24 import org.hipparchus.util.MathArrays;
25 import org.orekit.errors.OrekitInternalError;
26 import org.orekit.time.AbsoluteDate;
27 import org.orekit.time.AbstractTimeInterpolator;
28
29 import java.util.List;
30
31 /**
32 * Class using Hermite interpolator to interpolate time stamped angular coordinates.
33 * <p>
34 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation points
35 * (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
36 * and numerical problems (including NaN appearing).
37 *
38 * @author Vincent Cucchietti
39 * @author Luc Maisonobe
40 * @see HermiteInterpolator
41 * @see TimeStampedAngularCoordinates
42 */
43 public class TimeStampedAngularCoordinatesHermiteInterpolator
44 extends AbstractTimeInterpolator<TimeStampedAngularCoordinates> {
45
46 /** Filter for derivatives from the sample to use in interpolation. */
47 private final AngularDerivativesFilter filter;
48
49 /**
50 * Constructor with :
51 * <ul>
52 * <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
53 * <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
54 * <li>Use of angular and first time derivative for attitude interpolation</li>
55 * </ul>
56 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
57 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
58 * phenomenon</a> and numerical problems (including NaN appearing).
59 */
60 public TimeStampedAngularCoordinatesHermiteInterpolator() {
61 this(DEFAULT_INTERPOLATION_POINTS);
62 }
63
64 /**
65 * /** Constructor with :
66 * <ul>
67 * <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
68 * <li>Use of angular and first time derivative for attitude interpolation</li>
69 * </ul>
70 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
71 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
72 * phenomenon</a> and numerical problems (including NaN appearing).
73 *
74 * @param interpolationPoints number of interpolation points
75 */
76 public TimeStampedAngularCoordinatesHermiteInterpolator(final int interpolationPoints) {
77 this(interpolationPoints, AngularDerivativesFilter.USE_RR);
78 }
79
80 /**
81 * Constructor with :
82 * <ul>
83 * <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
84 * </ul>
85 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
86 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
87 * phenomenon</a> and numerical problems (including NaN appearing).
88 *
89 * @param interpolationPoints number of interpolation points
90 * @param filter filter for derivatives from the sample to use in interpolation
91 */
92 public TimeStampedAngularCoordinatesHermiteInterpolator(final int interpolationPoints,
93 final AngularDerivativesFilter filter) {
94 this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, filter);
95 }
96
97 /**
98 * Constructor.
99 * <p>
100 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
101 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
102 * phenomenon</a> and numerical problems (including NaN appearing).
103 *
104 * @param interpolationPoints number of interpolation points
105 * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
106 * @param filter filter for derivatives from the sample to use in interpolation
107 */
108 public TimeStampedAngularCoordinatesHermiteInterpolator(final int interpolationPoints,
109 final double extrapolationThreshold,
110 final AngularDerivativesFilter filter) {
111 super(interpolationPoints, extrapolationThreshold);
112 this.filter = filter;
113 }
114
115 /** Get filter for derivatives from the sample to use in interpolation.
116 * @return filter for derivatives from the sample to use in interpolation
117 */
118 public AngularDerivativesFilter getFilter() {
119 return filter;
120 }
121
122 /**
123 * {@inheritDoc}
124 * <p>
125 * The interpolated instance is created by polynomial Hermite interpolation on Rodrigues vector ensuring rotation rate
126 * remains the exact derivative of rotation.
127 * <p>
128 * This method is based on Sergei Tanygin's paper <a
129 * href="http://www.agi.com/resources/white-papers/attitude-interpolation">Attitude Interpolation</a>, changing the norm
130 * of the vector to match the modified Rodrigues vector as described in Malcolm D. Shuster's paper <a
131 * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
132 * Survey of Attitude Representations</a>. This change avoids the singularity at π. There is still a singularity at 2π,
133 * which is handled by slightly offsetting all rotations when this singularity is detected. Another change is that the
134 * mean linear motion is first removed before interpolation and added back after interpolation. This allows to use
135 * interpolation even when the sample covers much more than one turn and even when sample points are separated by more
136 * than one turn.
137 * </p>
138 * <p>
139 * Note that even if first and second time derivatives (rotation rates and acceleration) from sample can be ignored, the
140 * interpolated instance always includes interpolated derivatives. This feature can be used explicitly to compute these
141 * derivatives when it would be too complex to compute them from an analytical formula: just compute a few sample points
142 * from the explicit formula and set the derivatives to zero in these sample points, then use interpolation to add
143 * derivatives consistent with the rotations.
144 */
145 @Override
146 protected TimeStampedAngularCoordinates interpolate(final InterpolationData interpolationData) {
147
148 // Get date
149 final AbsoluteDate date = interpolationData.getInterpolationDate();
150
151 // Get sample
152 final List<TimeStampedAngularCoordinates> sample = interpolationData.getNeighborList();
153
154 // set up safety elements for 2π singularity avoidance
155 final double epsilon = 2 * FastMath.PI / sample.size();
156 final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));
157
158 // set up a linear model canceling mean rotation rate
159 final Vector3D meanRate;
160 Vector3D sum = Vector3D.ZERO;
161 if (filter != AngularDerivativesFilter.USE_R) {
162 for (final TimeStampedAngularCoordinates datedAC : sample) {
163 sum = sum.add(datedAC.getRotationRate());
164 }
165 meanRate = new Vector3D(1.0 / sample.size(), sum);
166 }
167 else {
168 TimeStampedAngularCoordinates previous = null;
169 for (final TimeStampedAngularCoordinates datedAC : sample) {
170 if (previous != null) {
171 sum = sum.add(TimeStampedAngularCoordinates.estimateRate(previous.getRotation(), datedAC.getRotation(),
172 datedAC.getDate()
173 .durationFrom(previous.getDate())));
174 }
175 previous = datedAC;
176 }
177 meanRate = new Vector3D(1.0 / (sample.size() - 1), sum);
178 }
179 TimeStampedAngularCoordinates offset =
180 new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.ZERO);
181
182 boolean restart = true;
183 for (int i = 0; restart && i < sample.size() + 2; ++i) {
184
185 // offset adaptation parameters
186 restart = false;
187
188 // set up an interpolator taking derivatives into account
189 final HermiteInterpolator interpolator = new HermiteInterpolator();
190
191 // add sample points
192 double sign = 1.0;
193 Rotation previous = Rotation.IDENTITY;
194
195 for (final TimeStampedAngularCoordinates ac : sample) {
196
197 // remove linear offset from the current coordinates
198 final double dt = ac.getDate().durationFrom(date);
199 final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt));
200
201 // make sure all interpolated points will be on the same branch
202 final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(),
203 fixed.getRotation().getQ1(), previous.getQ1(),
204 fixed.getRotation().getQ2(), previous.getQ2(),
205 fixed.getRotation().getQ3(), previous.getQ3());
206 sign = FastMath.copySign(1.0, dot * sign);
207 previous = fixed.getRotation();
208
209 // check modified Rodrigues vector singularity
210 if (fixed.getRotation().getQ0() * sign < threshold) {
211 // the sample point is close to a modified Rodrigues vector singularity
212 // we need to change the linear offset model to avoid this
213 restart = true;
214 break;
215 }
216
217 final double[][] rodrigues = fixed.getModifiedRodrigues(sign);
218 switch (filter) {
219 case USE_RRA:
220 // populate sample with rotation, rotation rate and acceleration data
221 interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
222 break;
223 case USE_RR:
224 // populate sample with rotation and rotation rate data
225 interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
226 break;
227 case USE_R:
228 // populate sample with rotation data only
229 interpolator.addSamplePoint(dt, rodrigues[0]);
230 break;
231 default:
232 // this should never happen
233 throw new OrekitInternalError(null);
234 }
235 }
236
237 if (restart) {
238 // interpolation failed, some intermediate rotation was too close to 2π
239 // we need to offset all rotations to avoid the singularity
240 offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I,
241 epsilon,
242 RotationConvention.VECTOR_OPERATOR),
243 Vector3D.ZERO, Vector3D.ZERO));
244 }
245 else {
246 // interpolation succeeded with the current offset
247 final double[][] p = interpolator.derivatives(0.0, 2);
248 final AngularCoordinates ac = AngularCoordinates.createFromModifiedRodrigues(p);
249 return new TimeStampedAngularCoordinates(offset.getDate(),
250 ac.getRotation(),
251 ac.getRotationRate(),
252 ac.getRotationAcceleration()).addOffset(offset);
253 }
254
255 }
256
257 // this should never happen
258 throw new OrekitInternalError(null);
259 }
260 }