TimeStampedAngularCoordinatesHermiteInterpolator.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.analysis.interpolation.HermiteInterpolator;
import org.hipparchus.geometry.euclidean.threed.Rotation;
import org.hipparchus.geometry.euclidean.threed.RotationConvention;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathArrays;
import org.orekit.errors.OrekitInternalError;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.AbstractTimeInterpolator;
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).
*
* @author Vincent Cucchietti
* @author Luc Maisonobe
* @see HermiteInterpolator
* @see TimeStampedAngularCoordinates
*/
public class TimeStampedAngularCoordinatesHermiteInterpolator
extends AbstractTimeInterpolator<TimeStampedAngularCoordinates> {
/** 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 TimeStampedAngularCoordinatesHermiteInterpolator() {
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 TimeStampedAngularCoordinatesHermiteInterpolator(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 TimeStampedAngularCoordinatesHermiteInterpolator(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 TimeStampedAngularCoordinatesHermiteInterpolator(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 TimeStampedAngularCoordinates interpolate(final InterpolationData interpolationData) {
// Get date
final AbsoluteDate date = interpolationData.getInterpolationDate();
// Get sample
final List<TimeStampedAngularCoordinates> 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 Vector3D meanRate;
Vector3D sum = Vector3D.ZERO;
if (filter != AngularDerivativesFilter.USE_R) {
for (final TimeStampedAngularCoordinates datedAC : sample) {
sum = sum.add(datedAC.getRotationRate());
}
meanRate = new Vector3D(1.0 / sample.size(), sum);
}
else {
TimeStampedAngularCoordinates previous = null;
for (final TimeStampedAngularCoordinates datedAC : sample) {
if (previous != null) {
sum = sum.add(TimeStampedAngularCoordinates.estimateRate(previous.getRotation(), datedAC.getRotation(),
datedAC.getDate()
.durationFrom(previous.getDate())));
}
previous = datedAC;
}
meanRate = new Vector3D(1.0 / (sample.size() - 1), sum);
}
TimeStampedAngularCoordinates offset =
new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.ZERO);
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 HermiteInterpolator interpolator = new HermiteInterpolator();
// add sample points
double sign = 1.0;
Rotation previous = Rotation.IDENTITY;
for (final TimeStampedAngularCoordinates ac : sample) {
// remove linear offset from the current coordinates
final double dt = ac.getDate().durationFrom(date);
final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt));
// make sure all interpolated points will be on the same branch
final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(),
fixed.getRotation().getQ1(), previous.getQ1(),
fixed.getRotation().getQ2(), previous.getQ2(),
fixed.getRotation().getQ3(), previous.getQ3());
sign = FastMath.copySign(1.0, dot * sign);
previous = fixed.getRotation();
// check modified Rodrigues vector singularity
if (fixed.getRotation().getQ0() * 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 double[][] 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 AngularCoordinates(new Rotation(Vector3D.PLUS_I,
epsilon,
RotationConvention.VECTOR_OPERATOR),
Vector3D.ZERO, Vector3D.ZERO));
}
else {
// interpolation succeeded with the current offset
final double[][] p = interpolator.derivatives(0.0, 2);
final AngularCoordinates ac = AngularCoordinates.createFromModifiedRodrigues(p);
return new TimeStampedAngularCoordinates(offset.getDate(),
ac.getRotation(),
ac.getRotationRate(),
ac.getRotationAcceleration()).addOffset(offset);
}
}
// this should never happen
throw new OrekitInternalError(null);
}
}