TimeStampedFieldAngularCoordinatesHermiteInterpolator.java
/* Copyright 2002-2024 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.utils;
import org.hipparchus.CalculusFieldElement;
import org.hipparchus.Field;
import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
import org.hipparchus.geometry.euclidean.threed.FieldRotation;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.RotationConvention;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitInternalError;
import org.orekit.time.AbstractFieldTimeInterpolator;
import org.orekit.time.FieldAbsoluteDate;
import java.util.List;
/**
* Class using Hermite interpolator to interpolate time stamped angular coordinates.
* <p>
* As this implementation of interpolation is polynomial, it should be used only with small number of interpolation points
* (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
* and numerical problems (including NaN appearing).
*
* @param <KK> type of the field element
*
* @author Vincent Cucchietti
* @author Luc Maisonobe
* @see FieldHermiteInterpolator
* @see TimeStampedFieldAngularCoordinates
*/
public class TimeStampedFieldAngularCoordinatesHermiteInterpolator<KK extends CalculusFieldElement<KK>>
extends AbstractFieldTimeInterpolator<TimeStampedFieldAngularCoordinates<KK>, KK> {
/** Filter for derivatives from the sample to use in interpolation. */
private final AngularDerivativesFilter filter;
/**
* Constructor with :
* <ul>
* <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
* <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
* <li>Use of angular and first time derivative for attitude interpolation</li>
* </ul>
* As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
* points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
* phenomenon</a> and numerical problems (including NaN appearing).
*/
public TimeStampedFieldAngularCoordinatesHermiteInterpolator() {
this(DEFAULT_INTERPOLATION_POINTS);
}
/**
* /** Constructor with :
* <ul>
* <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
* <li>Use of angular and first time derivative for attitude interpolation</li>
* </ul>
* As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
* points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
* phenomenon</a> and numerical problems (including NaN appearing).
*
* @param interpolationPoints number of interpolation points
*/
public TimeStampedFieldAngularCoordinatesHermiteInterpolator(final int interpolationPoints) {
this(interpolationPoints, AngularDerivativesFilter.USE_RR);
}
/**
* Constructor with :
* <ul>
* <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
* </ul>
* As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
* points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
* phenomenon</a> and numerical problems (including NaN appearing).
*
* @param interpolationPoints number of interpolation points
* @param filter filter for derivatives from the sample to use in interpolation
*/
public TimeStampedFieldAngularCoordinatesHermiteInterpolator(final int interpolationPoints,
final AngularDerivativesFilter filter) {
this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, filter);
}
/**
* Constructor.
* <p>
* As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
* points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
* phenomenon</a> and numerical problems (including NaN appearing).
*
* @param interpolationPoints number of interpolation points
* @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
* @param filter filter for derivatives from the sample to use in interpolation
*/
public TimeStampedFieldAngularCoordinatesHermiteInterpolator(final int interpolationPoints,
final double extrapolationThreshold,
final AngularDerivativesFilter filter) {
super(interpolationPoints, extrapolationThreshold);
this.filter = filter;
}
/** Get filter for derivatives from the sample to use in interpolation.
* @return filter for derivatives from the sample to use in interpolation
*/
public AngularDerivativesFilter getFilter() {
return filter;
}
/**
* {@inheritDoc}
* <p>
* The interpolated instance is created by polynomial Hermite interpolation on Rodrigues vector ensuring rotation rate
* remains the exact derivative of rotation.
* <p>
* This method is based on Sergei Tanygin's paper <a
* href="http://www.agi.com/resources/white-papers/attitude-interpolation">Attitude Interpolation</a>, changing the norm
* of the vector to match the modified Rodrigues vector as described in Malcolm D. Shuster's paper <a
* href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
* Survey of Attitude Representations</a>. This change avoids the singularity at π. There is still a singularity at 2π,
* which is handled by slightly offsetting all rotations when this singularity is detected. Another change is that the
* mean linear motion is first removed before interpolation and added back after interpolation. This allows to use
* interpolation even when the sample covers much more than one turn and even when sample points are separated by more
* than one turn.
* </p>
* <p>
* Note that even if first and second time derivatives (rotation rates and acceleration) from sample can be ignored, the
* interpolated instance always includes interpolated derivatives. This feature can be used explicitly to compute these
* derivatives when it would be too complex to compute them from an analytical formula: just compute a few sample points
* from the explicit formula and set the derivatives to zero in these sample points, then use interpolation to add
* derivatives consistent with the rotations.
*/
@Override
protected TimeStampedFieldAngularCoordinates<KK> interpolate(final InterpolationData interpolationData) {
// Get interpolation date
final FieldAbsoluteDate<KK> interpolationDate = interpolationData.getInterpolationDate();
// Get sample
final List<TimeStampedFieldAngularCoordinates<KK>> sample = interpolationData.getNeighborList();
// set up safety elements for 2π singularity avoidance
final double epsilon = 2 * FastMath.PI / sample.size();
final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));
// set up a linear model canceling mean rotation rate
final Field<KK> field = interpolationData.getField();
final FieldVector3D<KK> meanRate;
FieldVector3D<KK> sum = FieldVector3D.getZero(field);
if (filter != AngularDerivativesFilter.USE_R) {
for (final TimeStampedFieldAngularCoordinates<KK> datedAC : sample) {
sum = sum.add(datedAC.getRotationRate());
}
meanRate = new FieldVector3D<>(1.0 / sample.size(), sum);
}
else {
TimeStampedFieldAngularCoordinates<KK> previous = null;
for (final TimeStampedFieldAngularCoordinates<KK> datedAC : sample) {
if (previous != null) {
sum = sum.add(TimeStampedFieldAngularCoordinates.estimateRate(previous.getRotation(),
datedAC.getRotation(),
datedAC.getDate()
.durationFrom(previous.getDate())));
}
previous = datedAC;
}
meanRate = new FieldVector3D<>(1.0 / (sample.size() - 1), sum);
}
TimeStampedFieldAngularCoordinates<KK> offset =
new TimeStampedFieldAngularCoordinates<>(interpolationDate, FieldRotation.getIdentity(field),
meanRate, FieldVector3D.getZero(field));
boolean restart = true;
for (int i = 0; restart && i < sample.size() + 2; ++i) {
// offset adaptation parameters
restart = false;
// set up an interpolator taking derivatives into account
final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();
// add sample points
final KK one = interpolationData.getOne();
double sign = 1.0;
FieldRotation<KK> previous = FieldRotation.getIdentity(field);
for (final TimeStampedFieldAngularCoordinates<KK> ac : sample) {
// remove linear offset from the current coordinates
final KK dt = ac.getDate().durationFrom(interpolationDate);
final TimeStampedFieldAngularCoordinates<KK> fixed = ac.subtractOffset(offset.shiftedBy(dt));
// make sure all interpolated points will be on the same branch
final double dot = one.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(),
fixed.getRotation().getQ1(), previous.getQ1(),
fixed.getRotation().getQ2(), previous.getQ2(),
fixed.getRotation().getQ3(), previous.getQ3()).getReal();
sign = FastMath.copySign(1.0, dot * sign);
previous = fixed.getRotation();
// check modified Rodrigues vector singularity
if (fixed.getRotation().getQ0().getReal() * sign < threshold) {
// the sample point is close to a modified Rodrigues vector singularity
// we need to change the linear offset model to avoid this
restart = true;
break;
}
final KK[][] rodrigues = fixed.getModifiedRodrigues(sign);
switch (filter) {
case USE_RRA:
// populate sample with rotation, rotation rate and acceleration data
interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
break;
case USE_RR:
// populate sample with rotation and rotation rate data
interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
break;
case USE_R:
// populate sample with rotation data only
interpolator.addSamplePoint(dt, rodrigues[0]);
break;
default:
// this should never happen
throw new OrekitInternalError(null);
}
}
if (restart) {
// interpolation failed, some intermediate rotation was too close to 2π
// we need to offset all rotations to avoid the singularity
offset = offset.addOffset(
new FieldAngularCoordinates<>(new FieldRotation<>(FieldVector3D.getPlusI(field),
one.newInstance(epsilon),
RotationConvention.VECTOR_OPERATOR),
FieldVector3D.getZero(field), FieldVector3D.getZero(field)));
} else {
// interpolation succeeded with the current offset
final KK zero = interpolationData.getZero();
final KK[][] p = interpolator.derivatives(zero, 2);
final FieldAngularCoordinates<KK> ac = FieldAngularCoordinates.createFromModifiedRodrigues(p);
return new TimeStampedFieldAngularCoordinates<>(offset.getDate(),
ac.getRotation(),
ac.getRotationRate(),
ac.getRotationAcceleration()).addOffset(offset);
}
}
// this should never happen
throw new OrekitInternalError(null);
}
}