TimeStampedAngularCoordinates.java

  1. /* Copyright 2002-2018 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.utils;

  19. import java.util.Collection;

  21. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  22. import org.hipparchus.analysis.interpolation.HermiteInterpolator;
  23. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  24. import org.hipparchus.geometry.euclidean.threed.Rotation;
  25. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  26. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  27. import org.hipparchus.util.FastMath;
  28. import org.hipparchus.util.MathArrays;
  29. import org.orekit.errors.OrekitException;
  30. import org.orekit.errors.OrekitInternalError;
  31. import org.orekit.errors.OrekitMessages;
  32. import org.orekit.time.AbsoluteDate;
  33. import org.orekit.time.TimeStamped;

  35. /** {@link TimeStamped time-stamped} version of {@link AngularCoordinates}.
  36.  * <p>Instances of this class are guaranteed to be immutable.</p>
  37.  * @author Luc Maisonobe
  38.  * @since 7.0
  39.  */
  40. public class TimeStampedAngularCoordinates extends AngularCoordinates implements TimeStamped {

  42.     /** Serializable UID. */
  43.     private static final long serialVersionUID = 20140723L;

  45.     /** The date. */
  46.     private final AbsoluteDate date;

  48.     /** Builds a rotation/rotation rate pair.
  49.      * @param date coordinates date
  50.      * @param rotation rotation
  51.      * @param rotationRate rotation rate Ω (rad/s)
  52.      * @param rotationAcceleration rotation acceleration dΩ/dt (rad²/s²)
  53.      */
  54.     public TimeStampedAngularCoordinates(final AbsoluteDate date,
  55.                                          final Rotation rotation,
  56.                                          final Vector3D rotationRate,
  57.                                          final Vector3D rotationAcceleration) {
  58.         super(rotation, rotationRate, rotationAcceleration);
  59.         this.date = date;
  60.     }

  62.     /** Build the rotation that transforms a pair of pv coordinates into another pair.

  64.      * <p><em>WARNING</em>! This method requires much more stringent assumptions on
  65.      * its parameters than the similar {@link Rotation#Rotation(Vector3D, Vector3D,
  66.      * Vector3D, Vector3D) constructor} from the {@link Rotation Rotation} class.
  67.      * As far as the Rotation constructor is concerned, the {@code v₂} vector from
  68.      * the second pair can be slightly misaligned. The Rotation constructor will
  69.      * compensate for this misalignment and create a rotation that ensure {@code
  70.      * v₁ = r(u₁)} and {@code v₂ ∈ plane (r(u₁), r(u₂))}. <em>THIS IS NOT
  71.      * TRUE ANYMORE IN THIS CLASS</em>! As derivatives are involved and must be
  72.      * preserved, this constructor works <em>only</em> if the two pairs are fully
  73.      * consistent, i.e. if a rotation exists that fulfill all the requirements: {@code
  74.      * v₁ = r(u₁)}, {@code v₂ = r(u₂)}, {@code dv₁/dt = dr(u₁)/dt}, {@code dv₂/dt
  75.      * = dr(u₂)/dt}, {@code d²v₁/dt² = d²r(u₁)/dt²}, {@code d²v₂/dt² = d²r(u₂)/dt²}.</p>

  77.      * @param date coordinates date
  78.      * @param u1 first vector of the origin pair
  79.      * @param u2 second vector of the origin pair
  80.      * @param v1 desired image of u1 by the rotation
  81.      * @param v2 desired image of u2 by the rotation
  82.      * @param tolerance relative tolerance factor used to check singularities
  83.      * @exception OrekitException if the vectors components cannot be converted to
  84.      * {@link DerivativeStructure} with proper order
  85.      */
  86.     public TimeStampedAngularCoordinates(final AbsoluteDate date,
  87.                                          final PVCoordinates u1, final PVCoordinates u2,
  88.                                          final PVCoordinates v1, final PVCoordinates v2,
  89.                                          final double tolerance)
  90.         throws OrekitException {
  91.         super(u1, u2, v1, v2, tolerance);
  92.         this.date = date;
  93.     }

  95.     /** Build one of the rotations that transform one pv coordinates into another one.

  97.      * <p>Except for a possible scale factor, if the instance were
  98.      * applied to the vector u it will produce the vector v. There is an
  99.      * infinite number of such rotations, this constructor choose the
  100.      * one with the smallest associated angle (i.e. the one whose axis
  101.      * is orthogonal to the (u, v) plane). If u and v are collinear, an
  102.      * arbitrary rotation axis is chosen.</p>

  104.      * @param date coordinates date
  105.      * @param u origin vector
  106.      * @param v desired image of u by the rotation
  107.      * @exception OrekitException if the vectors components cannot be converted to
  108.      * {@link DerivativeStructure} with proper order
  109.      */
  110.     public TimeStampedAngularCoordinates(final AbsoluteDate date,
  111.                                          final PVCoordinates u, final PVCoordinates v)
  112.         throws OrekitException {
  113.         super(u, v);
  114.         this.date = date;
  115.     }

  117.     /** Builds a TimeStampedAngularCoordinates from  a {@link FieldRotation}&lt;{@link DerivativeStructure}&gt;.
  118.      * <p>
  119.      * The rotation components must have time as their only derivation parameter and
  120.      * have consistent derivation orders.
  121.      * </p>
  122.      * @param date coordinates date
  123.      * @param r rotation with time-derivatives embedded within the coordinates
  124.      */
  125.     public TimeStampedAngularCoordinates(final AbsoluteDate date,
  126.                                          final FieldRotation<DerivativeStructure> r) {
  127.         super(r);
  128.         this.date = date;
  129.     }

  131.     /** {@inheritDoc} */
  132.     public AbsoluteDate getDate() {
  133.         return date;
  134.     }

  136.     /** Revert a rotation/rotation rate pair.
  137.      * Build a pair which reverse the effect of another pair.
  138.      * @return a new pair whose effect is the reverse of the effect
  139.      * of the instance
  140.      */
  141.     public TimeStampedAngularCoordinates revert() {
  142.         return new TimeStampedAngularCoordinates(date,
  143.                                                  getRotation().revert(),
  144.                                                  getRotation().applyInverseTo(getRotationRate().negate()),
  145.                                                  getRotation().applyInverseTo(getRotationAcceleration().negate()));
  146.     }

  148.     /** Get a time-shifted state.
  149.      * <p>
  150.      * The state can be slightly shifted to close dates. This shift is based on
  151.      * a simple linear model. It is <em>not</em> intended as a replacement for
  152.      * proper attitude propagation but should be sufficient for either small
  153.      * time shifts or coarse accuracy.
  154.      * </p>
  155.      * @param dt time shift in seconds
  156.      * @return a new state, shifted with respect to the instance (which is immutable)
  157.      */
  158.     public TimeStampedAngularCoordinates shiftedBy(final double dt) {
  159.         final AngularCoordinates sac = super.shiftedBy(dt);
  160.         return new TimeStampedAngularCoordinates(date.shiftedBy(dt),
  161.                                                  sac.getRotation(), sac.getRotationRate(), sac.getRotationAcceleration());

  163.     }

  165.     /** Add an offset from the instance.
  166.      * <p>
  167.      * We consider here that the offset rotation is applied first and the
  168.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  169.      * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
  170.      * b.addOffset(a)} lead to <em>different</em> results in most cases.
  171.      * </p>
  172.      * <p>
  173.      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
  174.      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
  175.      * so that round trip applications are possible. This means that both {@code
  176.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  177.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  178.      * </p>
  179.      * @param offset offset to subtract
  180.      * @return new instance, with offset subtracted
  181.      * @see #subtractOffset(AngularCoordinates)
  182.      */
  183.     @Override
  184.     public TimeStampedAngularCoordinates addOffset(final AngularCoordinates offset) {
  185.         final Vector3D rOmega    = getRotation().applyTo(offset.getRotationRate());
  186.         final Vector3D rOmegaDot = getRotation().applyTo(offset.getRotationAcceleration());
  187.         return new TimeStampedAngularCoordinates(date,
  188.                                                  getRotation().compose(offset.getRotation(), RotationConvention.VECTOR_OPERATOR),
  189.                                                  getRotationRate().add(rOmega),
  190.                                                  new Vector3D( 1.0, getRotationAcceleration(),
  191.                                                                1.0, rOmegaDot,
  192.                                                               -1.0, Vector3D.crossProduct(getRotationRate(), rOmega)));
  193.     }

  195.     /** Subtract an offset from the instance.
  196.      * <p>
  197.      * We consider here that the offset rotation is applied first and the
  198.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  199.      * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
  200.      * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
  201.      * </p>
  202.      * <p>
  203.      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
  204.      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
  205.      * so that round trip applications are possible. This means that both {@code
  206.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  207.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  208.      * </p>
  209.      * @param offset offset to subtract
  210.      * @return new instance, with offset subtracted
  211.      * @see #addOffset(AngularCoordinates)
  212.      */
  213.     @Override
  214.     public TimeStampedAngularCoordinates subtractOffset(final AngularCoordinates offset) {
  215.         return addOffset(offset.revert());
  216.     }

  218.     /** Interpolate angular coordinates.
  219.      * <p>
  220.      * The interpolated instance is created by polynomial Hermite interpolation
  221.      * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation.
  222.      * </p>
  223.      * <p>
  224.      * This method is based on Sergei Tanygin's paper <a
  225.      * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude
  226.      * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
  227.      * vector as described in Malcolm D. Shuster's paper <a
  228.      * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
  229.      * Survey of Attitude Representations</a>. This change avoids the singularity at π.
  230.      * There is still a singularity at 2π, which is handled by slightly offsetting all rotations
  231.      * when this singularity is detected. Another change is that the mean linear motion
  232.      * is first removed before interpolation and added back after interpolation. This allows
  233.      * to use interpolation even when the sample covers much more than one turn and even
  234.      * when sample points are separated by more than one turn.
  235.      * </p>
  236.      * <p>
  237.      * Note that even if first and second time derivatives (rotation rates and acceleration)
  238.      * from sample can be ignored, the interpolated instance always includes
  239.      * interpolated derivatives. This feature can be used explicitly to
  240.      * compute these derivatives when it would be too complex to compute them
  241.      * from an analytical formula: just compute a few sample points from the
  242.      * explicit formula and set the derivatives to zero in these sample points,
  243.      * then use interpolation to add derivatives consistent with the rotations.
  244.      * </p>
  245.      * @param date interpolation date
  246.      * @param filter filter for derivatives from the sample to use in interpolation
  247.      * @param sample sample points on which interpolation should be done
  248.      * @return a new position-velocity, interpolated at specified date
  249.      * @exception OrekitException if the number of point is too small for interpolating
  250.      */
  251.     public static TimeStampedAngularCoordinates interpolate(final AbsoluteDate date,
  252.                                                             final AngularDerivativesFilter filter,
  253.                                                             final Collection<TimeStampedAngularCoordinates> sample)
  254.         throws OrekitException {

  256.         // set up safety elements for 2π singularity avoidance
  257.         final double epsilon   = 2 * FastMath.PI / sample.size();
  258.         final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));

  260.         // set up a linear model canceling mean rotation rate
  261.         final Vector3D meanRate;
  262.         if (filter != AngularDerivativesFilter.USE_R) {
  263.             Vector3D sum = Vector3D.ZERO;
  264.             for (final TimeStampedAngularCoordinates datedAC : sample) {
  265.                 sum = sum.add(datedAC.getRotationRate());
  266.             }
  267.             meanRate = new Vector3D(1.0 / sample.size(), sum);
  268.         } else {
  269.             if (sample.size() < 2) {
  270.                 throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION,
  271.                                           sample.size());
  272.             }
  273.             Vector3D sum = Vector3D.ZERO;
  274.             TimeStampedAngularCoordinates previous = null;
  275.             for (final TimeStampedAngularCoordinates datedAC : sample) {
  276.                 if (previous != null) {
  277.                     sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(),
  278.                                                datedAC.date.durationFrom(previous.date)));
  279.                 }
  280.                 previous = datedAC;
  281.             }
  282.             meanRate = new Vector3D(1.0 / (sample.size() - 1), sum);
  283.         }
  284.         TimeStampedAngularCoordinates offset =
  285.             new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.ZERO);

  287.         boolean restart = true;
  288.         for (int i = 0; restart && i < sample.size() + 2; ++i) {

  290.             // offset adaptation parameters
  291.             restart = false;

  293.             // set up an interpolator taking derivatives into account
  294.             final HermiteInterpolator interpolator = new HermiteInterpolator();

  296.             // add sample points
  297.             double sign = +1.0;
  298.             Rotation previous = Rotation.IDENTITY;

  300.             for (final TimeStampedAngularCoordinates ac : sample) {

  302.                 // remove linear offset from the current coordinates
  303.                 final double dt = ac.date.durationFrom(date);
  304.                 final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt));

  306.                 // make sure all interpolated points will be on the same branch
  307.                 final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(),
  308.                                                                 fixed.getRotation().getQ1(), previous.getQ1(),
  309.                                                                 fixed.getRotation().getQ2(), previous.getQ2(),
  310.                                                                 fixed.getRotation().getQ3(), previous.getQ3());
  311.                 sign = FastMath.copySign(1.0, dot * sign);
  312.                 previous = fixed.getRotation();

  314.                 // check modified Rodrigues vector singularity
  315.                 if (fixed.getRotation().getQ0() * sign < threshold) {
  316.                     // the sample point is close to a modified Rodrigues vector singularity
  317.                     // we need to change the linear offset model to avoid this
  318.                     restart = true;
  319.                     break;
  320.                 }

  322.                 final double[][] rodrigues = fixed.getModifiedRodrigues(sign);
  323.                 switch (filter) {
  324.                     case USE_RRA:
  325.                         // populate sample with rotation, rotation rate and acceleration data
  326.                         interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
  327.                         break;
  328.                     case USE_RR:
  329.                         // populate sample with rotation and rotation rate data
  330.                         interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
  331.                         break;
  332.                     case USE_R:
  333.                         // populate sample with rotation data only
  334.                         interpolator.addSamplePoint(dt, rodrigues[0]);
  335.                         break;
  336.                     default :
  337.                         // this should never happen
  338.                         throw new OrekitInternalError(null);
  339.                 }
  340.             }

  342.             if (restart) {
  343.                 // interpolation failed, some intermediate rotation was too close to 2π
  344.                 // we need to offset all rotations to avoid the singularity
  345.                 offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I,
  346.                                                                               epsilon,
  347.                                                                               RotationConvention.VECTOR_OPERATOR),
  348.                                                                  Vector3D.ZERO, Vector3D.ZERO));
  349.             } else {
  350.                 // interpolation succeeded with the current offset
  351.                 final double[][] p = interpolator.derivatives(0.0, 2);
  352.                 final AngularCoordinates ac = createFromModifiedRodrigues(p);
  353.                 return new TimeStampedAngularCoordinates(offset.getDate(),
  354.                                                          ac.getRotation(),
  355.                                                          ac.getRotationRate(),
  356.                                                          ac.getRotationAcceleration()).addOffset(offset);
  357.             }

  359.         }

  361.         // this should never happen
  362.         throw new OrekitInternalError(null);

  364.     }

  366. }
Created with JaCoCo 0.7.9.201702052155