SpacecraftToObservedBody.java

  1. /* Copyright 2013-2022 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.rugged.utils;

  19. import java.io.Serializable;
  20. import java.util.ArrayList;
  21. import java.util.List;
  22. import java.util.stream.Collectors;

  24. import org.hipparchus.util.FastMath;
  25. import org.orekit.frames.Frame;
  26. import org.orekit.frames.Transform;
  27. import org.orekit.rugged.errors.DumpManager;
  28. import org.orekit.rugged.errors.RuggedException;
  29. import org.orekit.rugged.errors.RuggedMessages;
  30. import org.orekit.time.AbsoluteDate;
  31. import org.orekit.utils.AngularDerivativesFilter;
  32. import org.orekit.utils.CartesianDerivativesFilter;
  33. import org.orekit.utils.ImmutableTimeStampedCache;
  34. import org.orekit.utils.TimeStampedAngularCoordinates;
  35. import org.orekit.utils.TimeStampedCache;
  36. import org.orekit.utils.TimeStampedPVCoordinates;

  38. /** Provider for observation transforms.
  39.  * @author Luc Maisonobe
  40.  * @author Guylaine Prat
  41.  */
  42. public class SpacecraftToObservedBody implements Serializable {

  44.     /** Serializable UID. */
  45.     private static final long serialVersionUID = 20140909L;

  47.     /** Inertial frame. */
  48.     private final Frame inertialFrame;

  50.     /** Body frame. */
  51.     private final Frame bodyFrame;

  53.     /** Start of search time span. */
  54.     private final AbsoluteDate minDate;

  56.     /** End of search time span. */
  57.     private final AbsoluteDate maxDate;

  59.     /** Step to use for inertial frame to body frame transforms cache computations. */
  60.     private final double tStep;

  62.     /** Tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting. */
  63.     private final double overshootTolerance;

  65.     /** Transforms sample from observed body frame to inertial frame. */
  66.     private final List<Transform> bodyToInertial;

  68.     /** Transforms sample from inertial frame to observed body frame. */
  69.     private final List<Transform> inertialToBody;

  71.     /** Transforms sample from spacecraft frame to inertial frame. */
  72.     private final List<Transform> scToInertial;

  74.     /** Simple constructor.
  75.      * @param inertialFrame inertial frame
  76.      * @param bodyFrame observed body frame
  77.      * @param minDate start of search time span
  78.      * @param maxDate end of search time span
  79.      * @param tStep step to use for inertial frame to body frame transforms cache computations
  80.      * @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
  81.      * slightly the position, velocity and quaternions ephemerides
  82.      * @param positionsVelocities satellite position and velocity
  83.      * @param pvInterpolationNumber number of points to use for position/velocity interpolation
  84.      * @param pvFilter filter for derivatives from the sample to use in position/velocity interpolation
  85.      * @param quaternions satellite quaternions
  86.      * @param aInterpolationNumber number of points to use for attitude interpolation
  87.      * @param aFilter filter for derivatives from the sample to use in attitude interpolation
  88.      */
  89.     public SpacecraftToObservedBody(final Frame inertialFrame, final Frame bodyFrame,
  90.                                     final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
  91.                                     final double overshootTolerance,
  92.                                     final List<TimeStampedPVCoordinates> positionsVelocities, final int pvInterpolationNumber,
  93.                                     final CartesianDerivativesFilter pvFilter,
  94.                                     final List<TimeStampedAngularCoordinates> quaternions, final int aInterpolationNumber,
  95.                                     final AngularDerivativesFilter aFilter) {

  97.         this.inertialFrame      = inertialFrame;
  98.         this.bodyFrame          = bodyFrame;
  99.         this.minDate            = minDate;
  100.         this.maxDate            = maxDate;
  101.         this.overshootTolerance = overshootTolerance;

  103.         // safety checks
  104.         final AbsoluteDate minPVDate = positionsVelocities.get(0).getDate();
  105.         final AbsoluteDate maxPVDate = positionsVelocities.get(positionsVelocities.size() - 1).getDate();
  106.         if (minPVDate.durationFrom(minDate) > overshootTolerance) {
  107.             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minPVDate, maxPVDate);
  108.         }
  109.         if (maxDate.durationFrom(maxPVDate) > overshootTolerance) {
  110.             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minPVDate, maxPVDate);
  111.         }

  113.         final AbsoluteDate minQDate  = quaternions.get(0).getDate();
  114.         final AbsoluteDate maxQDate  = quaternions.get(quaternions.size() - 1).getDate();
  115.         if (minQDate.durationFrom(minDate) > overshootTolerance) {
  116.             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minQDate, maxQDate);
  117.         }
  118.         if (maxDate.durationFrom(maxQDate) > overshootTolerance) {
  119.             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minQDate, maxQDate);
  120.         }

  122.         // set up the cache for position-velocities
  123.         final TimeStampedCache<TimeStampedPVCoordinates> pvCache =
  124.                 new ImmutableTimeStampedCache<>(pvInterpolationNumber, positionsVelocities);

  126.         // set up the cache for attitudes
  127.         final TimeStampedCache<TimeStampedAngularCoordinates> aCache =
  128.                 new ImmutableTimeStampedCache<>(aInterpolationNumber, quaternions);

  130.         final int n = (int) FastMath.ceil(maxDate.durationFrom(minDate) / tStep);
  131.         this.tStep          = tStep;
  132.         this.bodyToInertial = new ArrayList<>(n);
  133.         this.inertialToBody = new ArrayList<>(n);
  134.         this.scToInertial   = new ArrayList<>(n);
  135.         for (AbsoluteDate date = minDate; bodyToInertial.size() < n; date = date.shiftedBy(tStep)) {

  137.             // interpolate position-velocity, allowing slight extrapolation near the boundaries
  138.             final AbsoluteDate pvInterpolationDate;
  139.             if (date.compareTo(pvCache.getEarliest().getDate()) < 0) {
  140.                 pvInterpolationDate = pvCache.getEarliest().getDate();
  141.             } else if (date.compareTo(pvCache.getLatest().getDate()) > 0) {
  142.                 pvInterpolationDate = pvCache.getLatest().getDate();
  143.             } else {
  144.                 pvInterpolationDate = date;
  145.             }
  146.             final TimeStampedPVCoordinates interpolatedPV =
  147.                     TimeStampedPVCoordinates.interpolate(pvInterpolationDate, pvFilter,
  148.                             pvCache.getNeighbors(pvInterpolationDate));
  149.             final TimeStampedPVCoordinates pv = interpolatedPV.shiftedBy(date.durationFrom(pvInterpolationDate));

  151.             // interpolate attitude, allowing slight extrapolation near the boundaries
  152.             final AbsoluteDate aInterpolationDate;
  153.             if (date.compareTo(aCache.getEarliest().getDate()) < 0) {
  154.                 aInterpolationDate = aCache.getEarliest().getDate();
  155.             } else if (date.compareTo(aCache.getLatest().getDate()) > 0) {
  156.                 aInterpolationDate = aCache.getLatest().getDate();
  157.             } else {
  158.                 aInterpolationDate = date;
  159.             }
  160.             final TimeStampedAngularCoordinates interpolatedQuaternion =
  161.                     TimeStampedAngularCoordinates.interpolate(aInterpolationDate, aFilter,
  162.                             aCache.getNeighbors(aInterpolationDate).collect(Collectors.toList()));
  163.             final TimeStampedAngularCoordinates quaternion = interpolatedQuaternion.shiftedBy(date.durationFrom(aInterpolationDate));

  165.             // store transform from spacecraft frame to inertial frame
  166.             scToInertial.add(new Transform(date,
  167.                     new Transform(date, quaternion.revert()),
  168.                     new Transform(date, pv)));

  170.             // store transform from body frame to inertial frame
  171.             final Transform b2i = bodyFrame.getTransformTo(inertialFrame, date);
  172.             bodyToInertial.add(b2i);
  173.             inertialToBody.add(b2i.getInverse());

  175.         }
  176.     }

  178.     /** Simple constructor.
  179.      * @param inertialFrame inertial frame
  180.      * @param bodyFrame observed body frame
  181.      * @param minDate start of search time span
  182.      * @param maxDate end of search time span
  183.      * @param tStep step to use for inertial frame to body frame transforms cache computations
  184.      * @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
  185.      * slightly the position, velocity and quaternions ephemerides
  186.      * @param bodyToInertial transforms sample from observed body frame to inertial frame
  187.      * @param scToInertial transforms sample from spacecraft frame to inertial frame
  188.      */
  189.     public SpacecraftToObservedBody(final Frame inertialFrame, final Frame bodyFrame,
  190.                                     final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
  191.                                     final double overshootTolerance,
  192.                                     final List<Transform> bodyToInertial, final List<Transform> scToInertial) {

  194.         this.inertialFrame      = inertialFrame;
  195.         this.bodyFrame          = bodyFrame;
  196.         this.minDate            = minDate;
  197.         this.maxDate            = maxDate;
  198.         this.tStep              = tStep;
  199.         this.overshootTolerance = overshootTolerance;
  200.         this.bodyToInertial     = bodyToInertial;
  201.         this.scToInertial       = scToInertial;

  203.         this.inertialToBody = new ArrayList<>(bodyToInertial.size());
  204.         for (final Transform b2i : bodyToInertial) {
  205.             inertialToBody.add(b2i.getInverse());
  206.         }

  208.     }

  210.     /** Get the inertial frame.
  211.      * @return inertial frame
  212.      */
  213.     public Frame getInertialFrame() {
  214.         return inertialFrame;
  215.     }

  217.     /** Get the body frame.
  218.      * @return body frame
  219.      */
  220.     public Frame getBodyFrame() {
  221.         return bodyFrame;
  222.     }

  224.     /** Get the start of search time span.
  225.      * @return start of search time span
  226.      */
  227.     public AbsoluteDate getMinDate() {
  228.         return minDate;
  229.     }

  231.     /** Get the end of search time span.
  232.      * @return end of search time span
  233.      */
  234.     public AbsoluteDate getMaxDate() {
  235.         return maxDate;
  236.     }

  238.     /** Get the step to use for inertial frame to body frame transforms cache computations.
  239.      * @return step to use for inertial frame to body frame transforms cache computations
  240.      */
  241.     public double getTStep() {
  242.         return tStep;
  243.     }

  245.     /** Get the tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting.
  246.      * @return tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting
  247.      */
  248.     public double getOvershootTolerance() {
  249.         return overshootTolerance;
  250.     }

  252.     /** Get transform from spacecraft to inertial frame.
  253.      * @param date date of the transform
  254.      * @return transform from spacecraft to inertial frame
  255.      */
  256.     public Transform getScToInertial(final AbsoluteDate date) {
  257.         return interpolate(date, scToInertial);
  258.     }

  260.     /** Get transform from inertial frame to observed body frame.
  261.      * @param date date of the transform
  262.      * @return transform from inertial frame to observed body frame
  263.      */
  264.     public Transform getInertialToBody(final AbsoluteDate date) {
  265.         return interpolate(date, inertialToBody);
  266.     }

  268.     /** Get transform from observed body frame to inertial frame.
  269.      * @param date date of the transform
  270.      * @return transform from observed body frame to inertial frame
  271.      */
  272.     public Transform getBodyToInertial(final AbsoluteDate date) {
  273.         return interpolate(date, bodyToInertial);
  274.     }

  276.     /** Interpolate transform.
  277.      * @param date date of the transform
  278.      * @param list transforms list to interpolate from
  279.      * @return interpolated transform
  280.      */
  281.     private Transform interpolate(final AbsoluteDate date, final List<Transform> list) {

  283.         // check date range
  284.         if (!isInRange(date)) {
  285.             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, date, minDate, maxDate);
  286.         }

  288.         final double    s     = date.durationFrom(list.get(0).getDate()) / tStep;
  289.         final int       index = FastMath.max(0, FastMath.min(list.size() - 1, (int) FastMath.rint(s)));

  291.         DumpManager.dumpTransform(this, index, bodyToInertial.get(index), scToInertial.get(index));

  293.         final Transform close = list.get(index);
  294.         return close.shiftedBy(date.durationFrom(close.getDate()));

  296.     }

  298.     /** Check if a date is in the supported range.
  299.      * @param date date to check
  300.      * @return true if date is in the supported range
  301.      */
  302.     public boolean isInRange(final AbsoluteDate date) {
  303.         return minDate.durationFrom(date) <= overshootTolerance &&
  304.                date.durationFrom(maxDate) <= overshootTolerance;
  305.     }

  307. }
Created with JaCoCo 0.8.7.202105040129