SpacecraftToObservedBody.java

  1. /* Copyright 2013-2017 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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 org.hipparchus.util.FastMath;
  20. import java.io.Serializable;
  21. import java.util.ArrayList;
  22. import java.util.List;
  23. import java.util.stream.Collectors;

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

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

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

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

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

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

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

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

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

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

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

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

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

  103.             this.inertialFrame      = inertialFrame;
  104.             this.bodyFrame          = bodyFrame;
  105.             this.minDate            = minDate;
  106.             this.maxDate            = maxDate;
  107.             this.overshootTolerance = overshootTolerance;

  109.             // safety checks
  110.             final AbsoluteDate minPVDate = positionsVelocities.get(0).getDate();
  111.             final AbsoluteDate maxPVDate = positionsVelocities.get(positionsVelocities.size() - 1).getDate();
  112.             if (minPVDate.durationFrom(minDate) > overshootTolerance) {
  113.                 throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minPVDate, maxPVDate);
  114.             }
  115.             if (maxDate.durationFrom(maxPVDate) > overshootTolerance) {
  116.                 throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minPVDate, maxPVDate);
  117.             }

  119.             final AbsoluteDate minQDate  = quaternions.get(0).getDate();
  120.             final AbsoluteDate maxQDate  = quaternions.get(quaternions.size() - 1).getDate();
  121.             if (minQDate.durationFrom(minDate) > overshootTolerance) {
  122.                 throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minQDate, maxQDate);
  123.             }
  124.             if (maxDate.durationFrom(maxQDate) > overshootTolerance) {
  125.                 throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minQDate, maxQDate);
  126.             }

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

  132.             // set up the cache for attitudes
  133.             final TimeStampedCache<TimeStampedAngularCoordinates> aCache =
  134.                     new ImmutableTimeStampedCache<TimeStampedAngularCoordinates>(aInterpolationNumber, quaternions);

  136.             final int n = (int) FastMath.ceil(maxDate.durationFrom(minDate) / tStep);
  137.             this.tStep          = tStep;
  138.             this.bodyToInertial = new ArrayList<Transform>(n);
  139.             this.inertialToBody = new ArrayList<Transform>(n);
  140.             this.scToInertial   = new ArrayList<Transform>(n);
  141.             for (AbsoluteDate date = minDate; bodyToInertial.size() < n; date = date.shiftedBy(tStep)) {

  143.                 // interpolate position-velocity, allowing slight extrapolation near the boundaries
  144.                 final AbsoluteDate pvInterpolationDate;
  145.                 if (date.compareTo(pvCache.getEarliest().getDate()) < 0) {
  146.                     pvInterpolationDate = pvCache.getEarliest().getDate();
  147.                 } else if (date.compareTo(pvCache.getLatest().getDate()) > 0) {
  148.                     pvInterpolationDate = pvCache.getLatest().getDate();
  149.                 } else {
  150.                     pvInterpolationDate = date;
  151.                 }
  152.                 final TimeStampedPVCoordinates interpolatedPV =
  153.                         TimeStampedPVCoordinates.interpolate(pvInterpolationDate, pvFilter,
  154.                                                              pvCache.getNeighbors(pvInterpolationDate));
  155.                 final TimeStampedPVCoordinates pv = interpolatedPV.shiftedBy(date.durationFrom(pvInterpolationDate));

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

  171.                 // store transform from spacecraft frame to inertial frame
  172.                 scToInertial.add(new Transform(date,
  173.                                                new Transform(date, quaternion.revert()),
  174.                                                new Transform(date, pv)));

  176.                 // store transform from body frame to inertial frame
  177.                 final Transform b2i = bodyFrame.getTransformTo(inertialFrame, date);
  178.                 bodyToInertial.add(b2i);
  179.                 inertialToBody.add(b2i.getInverse());

  181.             }

  183.         } catch (OrekitException oe) {
  184.             throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
  185.         }
  186.     }

  188.     /** Simple constructor.
  189.      * @param inertialFrame inertial frame
  190.      * @param bodyFrame observed body frame
  191.      * @param minDate start of search time span
  192.      * @param maxDate end of search time span
  193.      * @param tStep step to use for inertial frame to body frame transforms cache computations
  194.      * @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
  195.      * slightly the position, velocity and quaternions ephemerides
  196.      * @param bodyToInertial transforms sample from observed body frame to inertial frame
  197.      * @param scToInertial transforms sample from spacecraft frame to inertial frame
  198.      */
  199.     public SpacecraftToObservedBody(final Frame inertialFrame, final Frame bodyFrame,
  200.                                     final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
  201.                                     final double overshootTolerance,
  202.                                     final List<Transform> bodyToInertial, final List<Transform> scToInertial) {

  204.         this.inertialFrame      = inertialFrame;
  205.         this.bodyFrame          = bodyFrame;
  206.         this.minDate            = minDate;
  207.         this.maxDate            = maxDate;
  208.         this.tStep              = tStep;
  209.         this.overshootTolerance = overshootTolerance;
  210.         this.bodyToInertial     = bodyToInertial;
  211.         this.scToInertial       = scToInertial;

  213.         this.inertialToBody = new ArrayList<Transform>(bodyToInertial.size());
  214.         for (final Transform b2i : bodyToInertial) {
  215.             inertialToBody.add(b2i.getInverse());
  216.         }

  218.     }

  220.     /** Get the inertial frame.
  221.      * @return inertial frame
  222.      */
  223.     public Frame getInertialFrame() {
  224.         return inertialFrame;
  225.     }

  227.     /** Get the body frame.
  228.      * @return body frame
  229.      */
  230.     public Frame getBodyFrame() {
  231.         return bodyFrame;
  232.     }

  234.     /** Get the start of search time span.
  235.      * @return start of search time span
  236.      */
  237.     public AbsoluteDate getMinDate() {
  238.         return minDate;
  239.     }

  241.     /** Get the end of search time span.
  242.      * @return end of search time span
  243.      */
  244.     public AbsoluteDate getMaxDate() {
  245.         return maxDate;
  246.     }

  248.     /** Get the step to use for inertial frame to body frame transforms cache computations.
  249.      * @return step to use for inertial frame to body frame transforms cache computations
  250.      */
  251.     public double getTStep() {
  252.         return tStep;
  253.     }

  255.     /** Get the tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting.
  256.      * @return tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting
  257.      */
  258.     public double getOvershootTolerance() {
  259.         return overshootTolerance;
  260.     }

  262.     /** Get transform from spacecraft to inertial frame.
  263.      * @param date date of the transform
  264.      * @return transform from spacecraft to inertial frame
  265.      * @exception RuggedException if frames cannot be computed at date
  266.      */
  267.     public Transform getScToInertial(final AbsoluteDate date)
  268.         throws RuggedException {
  269.         return interpolate(date, scToInertial);
  270.     }

  272.     /** Get transform from inertial frame to observed body frame.
  273.      * @param date date of the transform
  274.      * @return transform from inertial frame to observed body frame
  275.      * @exception RuggedException if frames cannot be computed at date
  276.      */
  277.     public Transform getInertialToBody(final AbsoluteDate date)
  278.         throws RuggedException {
  279.         return interpolate(date, inertialToBody);
  280.     }

  282.     /** Get transform from observed body frame to inertial frame.
  283.      * @param date date of the transform
  284.      * @return transform from observed body frame to inertial frame
  285.      * @exception RuggedException if frames cannot be computed at date
  286.      */
  287.     public Transform getBodyToInertial(final AbsoluteDate date)
  288.         throws RuggedException {
  289.         return interpolate(date, bodyToInertial);
  290.     }

  292.     /** Interpolate transform.
  293.      * @param date date of the transform
  294.      * @param list transforms list to interpolate from
  295.      * @return interpolated transform
  296.      * @exception RuggedException if frames cannot be computed at date
  297.      */
  298.     private Transform interpolate(final AbsoluteDate date, final List<Transform> list)
  299.         throws RuggedException {

  301.         // check date range
  302.         if (!isInRange(date)) {
  303.             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, date, minDate, maxDate);
  304.         }

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

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

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

  314.     }

  316.     /** Check if a date is in the supported range.
  317.      * @param date date to check
  318.      * @return true if date is in the supported range
  319.      */
  320.     public boolean isInRange(final AbsoluteDate date) {
  321.         return (minDate.durationFrom(date) <= overshootTolerance) &&
  322.                (date.durationFrom(maxDate) <= overshootTolerance);
  323.     }

  325. }
Created with JaCoCo 0.7.9.201702052155