SpacecraftToObservedBody.java
/* Copyright 2013-2022 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.rugged.utils;
import java.io.Serializable;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;
import org.hipparchus.util.FastMath;
import org.orekit.frames.Frame;
import org.orekit.frames.Transform;
import org.orekit.rugged.errors.DumpManager;
import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.errors.RuggedMessages;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.AngularDerivativesFilter;
import org.orekit.utils.CartesianDerivativesFilter;
import org.orekit.utils.ImmutableTimeStampedCache;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedCache;
import org.orekit.utils.TimeStampedPVCoordinates;
/** Provider for observation transforms.
* @author Luc Maisonobe
* @author Guylaine Prat
*/
public class SpacecraftToObservedBody implements Serializable {
/** Serializable UID. */
private static final long serialVersionUID = 20140909L;
/** Inertial frame. */
private final Frame inertialFrame;
/** Body frame. */
private final Frame bodyFrame;
/** Start of search time span. */
private final AbsoluteDate minDate;
/** End of search time span. */
private final AbsoluteDate maxDate;
/** Step to use for inertial frame to body frame transforms cache computations. */
private final double tStep;
/** Tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting. */
private final double overshootTolerance;
/** Transforms sample from observed body frame to inertial frame. */
private final List<Transform> bodyToInertial;
/** Transforms sample from inertial frame to observed body frame. */
private final List<Transform> inertialToBody;
/** Transforms sample from spacecraft frame to inertial frame. */
private final List<Transform> scToInertial;
/** Simple constructor.
* @param inertialFrame inertial frame
* @param bodyFrame observed body frame
* @param minDate start of search time span
* @param maxDate end of search time span
* @param tStep step to use for inertial frame to body frame transforms cache computations
* @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
* slightly the position, velocity and quaternions ephemerides
* @param positionsVelocities satellite position and velocity
* @param pvInterpolationNumber number of points to use for position/velocity interpolation
* @param pvFilter filter for derivatives from the sample to use in position/velocity interpolation
* @param quaternions satellite quaternions
* @param aInterpolationNumber number of points to use for attitude interpolation
* @param aFilter filter for derivatives from the sample to use in attitude interpolation
*/
public SpacecraftToObservedBody(final Frame inertialFrame, final Frame bodyFrame,
final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
final double overshootTolerance,
final List<TimeStampedPVCoordinates> positionsVelocities, final int pvInterpolationNumber,
final CartesianDerivativesFilter pvFilter,
final List<TimeStampedAngularCoordinates> quaternions, final int aInterpolationNumber,
final AngularDerivativesFilter aFilter) {
this.inertialFrame = inertialFrame;
this.bodyFrame = bodyFrame;
this.minDate = minDate;
this.maxDate = maxDate;
this.overshootTolerance = overshootTolerance;
// safety checks
final AbsoluteDate minPVDate = positionsVelocities.get(0).getDate();
final AbsoluteDate maxPVDate = positionsVelocities.get(positionsVelocities.size() - 1).getDate();
if (minPVDate.durationFrom(minDate) > overshootTolerance) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minPVDate, maxPVDate);
}
if (maxDate.durationFrom(maxPVDate) > overshootTolerance) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minPVDate, maxPVDate);
}
final AbsoluteDate minQDate = quaternions.get(0).getDate();
final AbsoluteDate maxQDate = quaternions.get(quaternions.size() - 1).getDate();
if (minQDate.durationFrom(minDate) > overshootTolerance) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minQDate, maxQDate);
}
if (maxDate.durationFrom(maxQDate) > overshootTolerance) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minQDate, maxQDate);
}
// set up the cache for position-velocities
final TimeStampedCache<TimeStampedPVCoordinates> pvCache =
new ImmutableTimeStampedCache<>(pvInterpolationNumber, positionsVelocities);
// set up the cache for attitudes
final TimeStampedCache<TimeStampedAngularCoordinates> aCache =
new ImmutableTimeStampedCache<>(aInterpolationNumber, quaternions);
final int n = (int) FastMath.ceil(maxDate.durationFrom(minDate) / tStep);
this.tStep = tStep;
this.bodyToInertial = new ArrayList<>(n);
this.inertialToBody = new ArrayList<>(n);
this.scToInertial = new ArrayList<>(n);
for (AbsoluteDate date = minDate; bodyToInertial.size() < n; date = date.shiftedBy(tStep)) {
// interpolate position-velocity, allowing slight extrapolation near the boundaries
final AbsoluteDate pvInterpolationDate;
if (date.compareTo(pvCache.getEarliest().getDate()) < 0) {
pvInterpolationDate = pvCache.getEarliest().getDate();
} else if (date.compareTo(pvCache.getLatest().getDate()) > 0) {
pvInterpolationDate = pvCache.getLatest().getDate();
} else {
pvInterpolationDate = date;
}
final TimeStampedPVCoordinates interpolatedPV =
TimeStampedPVCoordinates.interpolate(pvInterpolationDate, pvFilter,
pvCache.getNeighbors(pvInterpolationDate));
final TimeStampedPVCoordinates pv = interpolatedPV.shiftedBy(date.durationFrom(pvInterpolationDate));
// interpolate attitude, allowing slight extrapolation near the boundaries
final AbsoluteDate aInterpolationDate;
if (date.compareTo(aCache.getEarliest().getDate()) < 0) {
aInterpolationDate = aCache.getEarliest().getDate();
} else if (date.compareTo(aCache.getLatest().getDate()) > 0) {
aInterpolationDate = aCache.getLatest().getDate();
} else {
aInterpolationDate = date;
}
final TimeStampedAngularCoordinates interpolatedQuaternion =
TimeStampedAngularCoordinates.interpolate(aInterpolationDate, aFilter,
aCache.getNeighbors(aInterpolationDate).collect(Collectors.toList()));
final TimeStampedAngularCoordinates quaternion = interpolatedQuaternion.shiftedBy(date.durationFrom(aInterpolationDate));
// store transform from spacecraft frame to inertial frame
scToInertial.add(new Transform(date,
new Transform(date, quaternion.revert()),
new Transform(date, pv)));
// store transform from body frame to inertial frame
final Transform b2i = bodyFrame.getTransformTo(inertialFrame, date);
bodyToInertial.add(b2i);
inertialToBody.add(b2i.getInverse());
}
}
/** Simple constructor.
* @param inertialFrame inertial frame
* @param bodyFrame observed body frame
* @param minDate start of search time span
* @param maxDate end of search time span
* @param tStep step to use for inertial frame to body frame transforms cache computations
* @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
* slightly the position, velocity and quaternions ephemerides
* @param bodyToInertial transforms sample from observed body frame to inertial frame
* @param scToInertial transforms sample from spacecraft frame to inertial frame
*/
public SpacecraftToObservedBody(final Frame inertialFrame, final Frame bodyFrame,
final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
final double overshootTolerance,
final List<Transform> bodyToInertial, final List<Transform> scToInertial) {
this.inertialFrame = inertialFrame;
this.bodyFrame = bodyFrame;
this.minDate = minDate;
this.maxDate = maxDate;
this.tStep = tStep;
this.overshootTolerance = overshootTolerance;
this.bodyToInertial = bodyToInertial;
this.scToInertial = scToInertial;
this.inertialToBody = new ArrayList<>(bodyToInertial.size());
for (final Transform b2i : bodyToInertial) {
inertialToBody.add(b2i.getInverse());
}
}
/** Get the inertial frame.
* @return inertial frame
*/
public Frame getInertialFrame() {
return inertialFrame;
}
/** Get the body frame.
* @return body frame
*/
public Frame getBodyFrame() {
return bodyFrame;
}
/** Get the start of search time span.
* @return start of search time span
*/
public AbsoluteDate getMinDate() {
return minDate;
}
/** Get the end of search time span.
* @return end of search time span
*/
public AbsoluteDate getMaxDate() {
return maxDate;
}
/** Get the step to use for inertial frame to body frame transforms cache computations.
* @return step to use for inertial frame to body frame transforms cache computations
*/
public double getTStep() {
return tStep;
}
/** Get the tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting.
* @return tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting
*/
public double getOvershootTolerance() {
return overshootTolerance;
}
/** Get transform from spacecraft to inertial frame.
* @param date date of the transform
* @return transform from spacecraft to inertial frame
*/
public Transform getScToInertial(final AbsoluteDate date) {
return interpolate(date, scToInertial);
}
/** Get transform from inertial frame to observed body frame.
* @param date date of the transform
* @return transform from inertial frame to observed body frame
*/
public Transform getInertialToBody(final AbsoluteDate date) {
return interpolate(date, inertialToBody);
}
/** Get transform from observed body frame to inertial frame.
* @param date date of the transform
* @return transform from observed body frame to inertial frame
*/
public Transform getBodyToInertial(final AbsoluteDate date) {
return interpolate(date, bodyToInertial);
}
/** Interpolate transform.
* @param date date of the transform
* @param list transforms list to interpolate from
* @return interpolated transform
*/
private Transform interpolate(final AbsoluteDate date, final List<Transform> list) {
// check date range
if (!isInRange(date)) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, date, minDate, maxDate);
}
final double s = date.durationFrom(list.get(0).getDate()) / tStep;
final int index = FastMath.max(0, FastMath.min(list.size() - 1, (int) FastMath.rint(s)));
DumpManager.dumpTransform(this, index, bodyToInertial.get(index), scToInertial.get(index));
final Transform close = list.get(index);
return close.shiftedBy(date.durationFrom(close.getDate()));
}
/** Check if a date is in the supported range.
* @param date date to check
* @return true if date is in the supported range
*/
public boolean isInRange(final AbsoluteDate date) {
return minDate.durationFrom(date) <= overshootTolerance &&
date.durationFrom(maxDate) <= overshootTolerance;
}
}