GNSSFieldAttitudeContext.java

  1. /* Copyright 2002-2020 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.gnss.attitude;

  18. import org.hipparchus.Field;
  19. import org.hipparchus.RealFieldElement;
  20. import org.hipparchus.analysis.UnivariateFunction;
  21. import org.hipparchus.analysis.differentiation.FDSFactory;
  22. import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
  23. import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
  24. import org.hipparchus.analysis.solvers.UnivariateSolverUtils;
  25. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  26. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  27. import org.hipparchus.util.FastMath;
  28. import org.hipparchus.util.SinCos;
  29. import org.orekit.frames.FieldTransform;
  30. import org.orekit.frames.Frame;
  31. import org.orekit.frames.LOFType;
  32. import org.orekit.time.AbsoluteDate;
  33. import org.orekit.time.FieldAbsoluteDate;
  34. import org.orekit.time.FieldTimeStamped;
  35. import org.orekit.utils.ExtendedPVCoordinatesProvider;
  36. import org.orekit.utils.FieldPVCoordinates;
  37. import org.orekit.utils.FieldPVCoordinatesProvider;
  38. import org.orekit.utils.PVCoordinates;
  39. import org.orekit.utils.TimeStampedFieldAngularCoordinates;
  40. import org.orekit.utils.TimeStampedFieldPVCoordinates;

  41. /**
  42.  * Boilerplate computations for GNSS attitude.
  43.  *
  44.  * <p>
  45.  * This class is intended to hold throw-away data pertaining to <em>one</em> call
  46.  * to {@link GNSSAttitudeProvider#getAttitude(org.orekit.utils.FieldPVCoordinatesProvider,
  47.  * org.orekit.time.FieldAbsoluteDate, org.orekit.frames.Frame)) getAttitude}. It allows
  48.  * the various {@link GNSSAttitudeProvider} implementations to be immutable as they
  49.  * do not store any state, and hence to be thread-safe and reentrant.
  50.  * </p>
  51.  *
  52.  * @author Luc Maisonobe
  53.  * @since 9.2
  54.  */
  55. class GNSSFieldAttitudeContext<T extends RealFieldElement<T>> implements FieldTimeStamped<T> {

  56.     /** Derivation order. */
  57.     private static final int ORDER = 2;

  58.     /** Constant Y axis. */
  59.     private static final PVCoordinates PLUS_Y_PV =
  60.             new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);

  61.     /** Constant Z axis. */
  62.     private static final PVCoordinates MINUS_Z_PV =
  63.             new PVCoordinates(Vector3D.MINUS_K, Vector3D.ZERO, Vector3D.ZERO);

  64.     /** Limit value below which we shoud use replace beta by betaIni. */
  65.     private static final double BETA_SIGN_CHANGE_PROTECTION = FastMath.toRadians(0.07);

  66.     /** Time derivation factory. */
  67.     private final FDSFactory<T> factory;

  68.     /** Constant Y axis. */
  69.     private final FieldPVCoordinates<T> plusY;

  70.     /** Constant Z axis. */
  71.     private final FieldPVCoordinates<T> minusZ;

  72.     /** Context date. */
  73.     private final AbsoluteDate dateDouble;

  74.     /** Current date. */
  75.     private final FieldAbsoluteDate<T> date;

  76.     /** Current date. */
  77.     private final FieldAbsoluteDate<FieldDerivativeStructure<T>> dateDS;

  78.     /** Provider for Sun position. */
  79.     private final ExtendedPVCoordinatesProvider sun;

  80.     /** Provider for spacecraft position. */
  81.     private final FieldPVCoordinatesProvider<T> pvProv;

  82.     /** Inertial frame where velocity are computed. */
  83.     private final Frame inertialFrame;

  84.     /** Cosine of the angle between spacecraft and Sun direction. */
  85.     private final FieldDerivativeStructure<T> svbCos;

  86.     /** Morning/Evening half orbit indicator. */
  87.     private final boolean morning;

  88.     /** Relative orbit angle to turn center. */
  89.     private final FieldDerivativeStructure<T> delta;

  90.     /** Spacecraft angular velocity. */
  91.     private T muRate;

  92.     /** Limit cosine for the midnight turn. */
  93.     private double cNight;

  94.     /** Limit cosine for the noon turn. */
  95.     private double cNoon;

  96.     /** Turn time data. */
  97.     private FieldTurnSpan<T> turnSpan;

  98.     /** Simple constructor.
  99.      * @param date current date
  100.      * @param sun provider for Sun position
  101.      * @param pvProv provider for spacecraft position
  102.      * @param inertialFrame inertial frame where velocity are computed
  103.      * @param turnSpan turn time data, if a turn has already been identified in the date neighborhood,
  104.      * null otherwise
  105.      */
  106.     GNSSFieldAttitudeContext(final FieldAbsoluteDate<T> date,
  107.                              final ExtendedPVCoordinatesProvider sun, final FieldPVCoordinatesProvider<T> pvProv,
  108.                              final Frame inertialFrame,  final FieldTurnSpan<T> turnSpan) {

  109.         final Field<T> field = date.getField();
  110.         factory  = new FDSFactory<>(field, 1, ORDER);
  111.         plusY    = new FieldPVCoordinates<>(field, PLUS_Y_PV);
  112.         minusZ   = new FieldPVCoordinates<>(field, MINUS_Z_PV);

  113.         this.dateDouble    = date.toAbsoluteDate();
  114.         this.date          = date;
  115.         this.dateDS        = addDerivatives(date);
  116.         this.sun           = sun;
  117.         this.pvProv        = pvProv;
  118.         this.inertialFrame = inertialFrame;
  119.         final FieldPVCoordinates<FieldDerivativeStructure<T>> sunPVDS = sun.getPVCoordinates(dateDS, inertialFrame);

  120.         final TimeStampedFieldPVCoordinates<T> svPV = pvProv.getPVCoordinates(date, inertialFrame);
  121.         final FieldPVCoordinates<FieldDerivativeStructure<T>> svPVDS  =
  122.                         svPV.toDerivativeStructurePV(factory.getCompiler().getOrder());
  123.         this.svbCos  = FieldVector3D.dotProduct(sunPVDS.getPosition(), svPVDS.getPosition()).
  124.                        divide(sunPVDS.getPosition().getNorm().multiply(svPVDS.getPosition().getNorm()));
  125.         this.morning = Vector3D.dotProduct(svPV.getVelocity().toVector3D(), sunPVDS.getPosition().toVector3D()) >= 0.0;

  126.         this.muRate = svPV.getAngularVelocity().getNorm();

  127.         this.turnSpan = turnSpan;

  128.         final FieldDerivativeStructure<T> absDelta;
  129.         if (svbCos.getValue().getReal() <= 0) {
  130.             // in eclipse turn mode
  131.             absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos().negate().add(FastMath.PI));
  132.         } else {
  133.             // in noon turn mode
  134.             absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos());
  135.         }
  136.         delta = absDelta.copySign(absDelta.getPartialDerivative(1).negate());

  137.     }

  138.     /** Convert a date, removing derivatives.
  139.      * @param d date to convert
  140.      * @return date without derivatives
  141.      */
  142.     private FieldAbsoluteDate<T> removeDerivatives(final FieldAbsoluteDate<FieldDerivativeStructure<T>> d) {
  143.         final AbsoluteDate                dd     = d.toAbsoluteDate();
  144.         final FieldDerivativeStructure<T> offset = d.durationFrom(dd);
  145.         return new FieldAbsoluteDate<>(date.getField(), dd).shiftedBy(offset.getValue());
  146.     }

  147.     /** Convert a date, adding derivatives.
  148.      * @param d date to convert
  149.      * @return date without derivatives
  150.      */
  151.     private FieldAbsoluteDate<FieldDerivativeStructure<T>> addDerivatives(final FieldAbsoluteDate<T> d) {
  152.         final AbsoluteDate dd     = d.toAbsoluteDate();
  153.         final T            offset = d.durationFrom(dd);
  154.         return new FieldAbsoluteDate<>(factory.getDerivativeField(), dd).shiftedBy(factory.variable(0, offset));
  155.     }

  156.     /** Compute nominal yaw steering.
  157.      * @param d computation date
  158.      * @return nominal yaw steering
  159.      */
  160.     public TimeStampedFieldAngularCoordinates<T> nominalYaw(final FieldAbsoluteDate<T> d) {
  161.         final TimeStampedFieldPVCoordinates<T> svPV = pvProv.getPVCoordinates(d, inertialFrame);
  162.         return new TimeStampedFieldAngularCoordinates<>(d,
  163.                                                         svPV.normalize(),
  164.                                                         sun.getPVCoordinates(d, inertialFrame).crossProduct(svPV).normalize(),
  165.                                                         minusZ,
  166.                                                         plusY,
  167.                                                         1.0e-9);
  168.     }

  169.     /** Compute Sun elevation.
  170.      * @param d computation date
  171.      * @return Sun elevation
  172.      */
  173.     public T beta(final FieldAbsoluteDate<T> d) {
  174.         final TimeStampedFieldPVCoordinates<T> svPV = pvProv.getPVCoordinates(d, inertialFrame);
  175.         return FieldVector3D.angle(sun.getPVCoordinates(d, inertialFrame).getPosition(), svPV.getMomentum()).
  176.                negate().
  177.                add(0.5 * FastMath.PI);
  178.     }

  179.     /** Compute Sun elevation.
  180.      * @param d computation date
  181.      * @return Sun elevation
  182.      */
  183.     private FieldDerivativeStructure<T> betaDS(final FieldAbsoluteDate<FieldDerivativeStructure<T>> d) {
  184.         final FieldPVCoordinates<FieldDerivativeStructure<T>> svPV =
  185.                         pvProv.getPVCoordinates(removeDerivatives(d), inertialFrame).
  186.                         toDerivativeStructurePV(d.getField().getZero().getOrder());
  187.         return FieldVector3D.angle(sun.getPVCoordinates(d, inertialFrame).getPosition(), svPV.getMomentum()).
  188.                negate().
  189.                add(0.5 * FastMath.PI);
  190.     }

  191.     /** Compute Sun elevation.
  192.      * @return Sun elevation
  193.      */
  194.     public FieldDerivativeStructure<T> betaDS() {
  195.         return betaDS(dateDS);
  196.     }

  197.     /** {@inheritDoc} */
  198.     @Override
  199.     public FieldAbsoluteDate<T> getDate() {
  200.         return date;
  201.     }

  202.     /** Get the turn span.
  203.      * @return turn span, may be null if context is outside of turn
  204.      */
  205.     public FieldTurnSpan<T> getTurnSpan() {
  206.         return turnSpan;
  207.     }

  208.     /** Get the cosine of the angle between spacecraft and Sun direction.
  209.      * @return cosine of the angle between spacecraft and Sun direction.
  210.      */
  211.     public T getSVBcos() {
  212.         return svbCos.getValue();
  213.     }

  214.     /** Get a Sun elevation angle that does not change sign within the turn.
  215.      * <p>
  216.      * This method either returns the current beta or replaces it with the
  217.      * value at turn start, so the sign remains constant throughout the
  218.      * turn. As of 9.2, it is used for GPS, Glonass and Galileo.
  219.      * </p>
  220.      * @return secured Sun elevation angle
  221.      * @see #beta(FieldAbsoluteDate)
  222.      * @see #betaDS(FieldAbsoluteDate)
  223.      */
  224.     public T getSecuredBeta() {
  225.         final T beta = beta(getDate());
  226.         return FastMath.abs(beta.getReal()) < BETA_SIGN_CHANGE_PROTECTION ?
  227.                beta(turnSpan.getTurnStartDate()) :
  228.                beta;
  229.     }

  230.     /** Check if a linear yaw model is still active or if we already reached target yaw.
  231.      * @param linearPhi value of the linear yaw model
  232.      * @param phiDot slope of the linear yaw model
  233.      * @return true if linear model is still active
  234.      */
  235.     public boolean linearModelStillActive(final T linearPhi, final T phiDot) {
  236.         final AbsoluteDate absDate = date.toAbsoluteDate();
  237.         final double dt0 = turnSpan.getTurnEndDate().durationFrom(date).getReal();
  238.         final UnivariateFunction yawReached = dt -> {
  239.             final AbsoluteDate  t       = absDate.shiftedBy(dt);
  240.             final Vector3D      pSun    = sun.getPVCoordinates(t, inertialFrame).getPosition();
  241.             final PVCoordinates pv      = pvProv.getPVCoordinates(date.shiftedBy(dt), inertialFrame).toPVCoordinates();
  242.             final Vector3D      pSat    = pv.getPosition();
  243.             final Vector3D      targetX = Vector3D.crossProduct(pSat, Vector3D.crossProduct(pSun, pSat)).normalize();

  244.             final double        phi         = linearPhi.getReal() + dt * phiDot.getReal();
  245.             final SinCos        sc          = FastMath.sinCos(phi);
  246.             final Vector3D      pU          = pv.getPosition().normalize();
  247.             final Vector3D      mU          = pv.getMomentum().normalize();
  248.             final Vector3D      omega       = new Vector3D(-phiDot.getReal(), pU);
  249.             final Vector3D      currentX    = new Vector3D(-sc.sin(), mU, -sc.cos(), Vector3D.crossProduct(pU, mU));
  250.             final Vector3D      currentXDot = Vector3D.crossProduct(omega, currentX);

  251.             return Vector3D.dotProduct(targetX, currentXDot);
  252.         };
  253.         final double fullTurn = 2 * FastMath.PI / FastMath.abs(phiDot.getReal());
  254.         final double dtMin    = FastMath.min(turnSpan.getTurnStartDate().durationFrom(date).getReal(), dt0 - 60.0);
  255.         final double dtMax    = FastMath.max(dtMin + fullTurn, dt0 + 60.0);
  256.         double[] bracket = UnivariateSolverUtils.bracket(yawReached, dt0,
  257.                                                          dtMin, dtMax, fullTurn / 100, 1.0, 100);
  258.         if (yawReached.value(bracket[0]) <= 0.0) {
  259.             // we have bracketed the wrong crossing
  260.             bracket = UnivariateSolverUtils.bracket(yawReached, 0.5 * (bracket[0] + bracket[1] + fullTurn),
  261.                                                     bracket[1], bracket[1] + fullTurn, fullTurn / 100, 1.0, 100);
  262.         }
  263.         final double dt = new BracketingNthOrderBrentSolver(1.0e-3, 5).
  264.                           solve(100, yawReached, bracket[0], bracket[1]);
  265.         turnSpan.updateEnd(date.shiftedBy(dt), absDate);

  266.         return dt > 0.0;

  267.     }

  268.     /** Set up the midnight/noon turn region.
  269.      * @param cosNight limit cosine for the midnight turn
  270.      * @param cosNoon limit cosine for the noon turn
  271.      * @return true if spacecraft is in the midnight/noon turn region
  272.      */
  273.     public boolean setUpTurnRegion(final double cosNight, final double cosNoon) {
  274.         this.cNight = cosNight;
  275.         this.cNoon  = cosNoon;

  276.         if (svbCos.getValue().getReal() < cNight || svbCos.getValue().getReal() > cNoon) {
  277.             // we are within turn triggering zone
  278.             return true;
  279.         } else {
  280.             // we are outside of turn triggering zone,
  281.             // but we may still be trying to recover nominal attitude at the end of a turn
  282.             return inTurnTimeRange();
  283.         }

  284.     }

  285.     /** Get the relative orbit angle to turn center.
  286.      * @return relative orbit angle to turn center
  287.      */
  288.     public FieldDerivativeStructure<T> getDeltaDS() {
  289.         return delta;
  290.     }

  291.     /** Get the orbit angle since solar midnight.
  292.      * @return orbit angle since solar midnight
  293.      */
  294.     public T getOrbitAngleSinceMidnight() {
  295.         final T absAngle = inOrbitPlaneAbsoluteAngle(FastMath.acos(svbCos.getValue()).negate().add(FastMath.PI));
  296.         return morning ? absAngle : absAngle.negate();
  297.     }

  298.     /** Check if spacecraft is in the half orbit closest to Sun.
  299.      * @return true if spacecraft is in the half orbit closest to Sun
  300.      */
  301.     public boolean inSunSide() {
  302.         return svbCos.getValue().getReal() > 0;
  303.     }

  304.     /** Get yaw at turn start.
  305.      * @param sunBeta Sun elevation above orbital plane
  306.      * (it <em>may</em> be different from {@link #getBeta()} in
  307.      * some special cases)
  308.      * @return yaw at turn start
  309.      */
  310.     public T getYawStart(final T sunBeta) {
  311.         final T halfSpan = turnSpan.getTurnDuration().multiply(muRate).multiply(0.5);
  312.         return computePhi(sunBeta, FastMath.copySign(halfSpan, svbCos.getValue()));
  313.     }

  314.     /** Get yaw at turn end.
  315.      * @param sunBeta Sun elevation above orbital plane
  316.      * (it <em>may</em> be different from {@link #getBeta()} in
  317.      * some special cases)
  318.      * @return yaw at turn end
  319.      */
  320.     public T getYawEnd(final T sunBeta) {
  321.         final T halfSpan = turnSpan.getTurnDuration().multiply(muRate).multiply(0.5);
  322.         return computePhi(sunBeta, FastMath.copySign(halfSpan, svbCos.getValue().negate()));
  323.     }

  324.     /** Compute yaw rate.
  325.      * @param sunBeta Sun elevation above orbital plane
  326.      * (it <em>may</em> be different from {@link #getBeta()} in
  327.      * some special cases)
  328.      * @return yaw rate
  329.      */
  330.     public T yawRate(final T sunBeta) {
  331.         return getYawEnd(sunBeta).subtract(getYawStart(sunBeta)).divide(turnSpan.getTurnDuration());
  332.     }

  333.     /** Get the spacecraft angular velocity.
  334.      * @return spacecraft angular velocity
  335.      */
  336.     public T getMuRate() {
  337.         return muRate;
  338.     }

  339.     /** Project a spacecraft/Sun angle into orbital plane.
  340.      * <p>
  341.      * This method is intended to find the limits of the noon and midnight
  342.      * turns in orbital plane. The return angle is signed, depending on the
  343.      * spacecraft being before or after turn middle point.
  344.      * </p>
  345.      * @param angle spacecraft/Sun angle (or spacecraft/opposite-of-Sun)
  346.      * @return angle projected into orbital plane, always positive
  347.      */
  348.     private FieldDerivativeStructure<T> inOrbitPlaneAbsoluteAngle(final FieldDerivativeStructure<T> angle) {
  349.         return FastMath.acos(FastMath.cos(angle).divide(FastMath.cos(beta(getDate()))));
  350.     }

  351.     /** Project a spacecraft/Sun angle into orbital plane.
  352.      * <p>
  353.      * This method is intended to find the limits of the noon and midnight
  354.      * turns in orbital plane. The return angle is always positive. The
  355.      * correct sign to apply depends on the spacecraft being before or
  356.      * after turn middle point.
  357.      * </p>
  358.      * @param angle spacecraft/Sun angle (or spacecraft/opposite-of-Sun)
  359.      * @return angle projected into orbital plane, always positive
  360.      */
  361.     public T inOrbitPlaneAbsoluteAngle(final T angle) {
  362.         return FastMath.acos(FastMath.cos(angle).divide(FastMath.cos(beta(getDate()))));
  363.     }

  364.     /** Compute yaw.
  365.      * @param sunBeta Sun elevation above orbital plane
  366.      * (it <em>may</em> be different from {@link #getBeta()} in
  367.      * some special cases)
  368.      * @param inOrbitPlaneAngle in orbit angle between spacecraft
  369.      * and Sun (or opposite of Sun) projection
  370.      * @return yaw angle
  371.      */
  372.     public T computePhi(final T sunBeta, final T inOrbitPlaneAngle) {
  373.         return FastMath.atan2(FastMath.tan(sunBeta).negate(), FastMath.sin(inOrbitPlaneAngle));
  374.     }

  375.     /** Set turn half span and compute corresponding turn time range.
  376.      * @param halfSpan half span of the turn region, as an angle in orbit plane
  377.      * @param endMargin margin in seconds after turn end
  378.      */
  379.     public void setHalfSpan(final T halfSpan, final double endMargin) {
  380.         final FieldAbsoluteDate<T> start = date.shiftedBy(delta.getValue().subtract(halfSpan).divide(muRate));
  381.         final FieldAbsoluteDate<T> end   = date.shiftedBy(delta.getValue().add(halfSpan).divide(muRate));
  382.         final AbsoluteDate estimationDate = getDate().toAbsoluteDate();
  383.         if (turnSpan == null) {
  384.             turnSpan = new FieldTurnSpan<>(start, end, estimationDate, endMargin);
  385.         } else {
  386.             turnSpan.updateStart(start, estimationDate);
  387.             turnSpan.updateEnd(end, estimationDate);
  388.         }
  389.     }

  390.     /** Check if context is within turn range.
  391.      * @return true if context is within range extended by end margin
  392.      */
  393.     public boolean inTurnTimeRange() {
  394.         return turnSpan != null && turnSpan.inTurnTimeRange(dateDouble);
  395.     }

  396.     /** Get elapsed time since turn start.
  397.      * @return elapsed time from turn start to current date
  398.      */
  399.     public T timeSinceTurnStart() {
  400.         return getDate().durationFrom(turnSpan.getTurnStartDate());
  401.     }

  402.     /** Generate an attitude with turn-corrected yaw.
  403.      * @param yaw yaw value to apply
  404.      * @param yawDot yaw first time derivative
  405.      * @return attitude with specified yaw
  406.      */
  407.     public TimeStampedFieldAngularCoordinates<T> turnCorrectedAttitude(final T yaw, final T yawDot) {
  408.         return turnCorrectedAttitude(factory.build(yaw, yawDot, yaw.getField().getZero()));
  409.     }

  410.     /** Generate an attitude with turn-corrected yaw.
  411.      * @param yaw yaw value to apply
  412.      * @return attitude with specified yaw
  413.      */
  414.     public TimeStampedFieldAngularCoordinates<T> turnCorrectedAttitude(final FieldDerivativeStructure<T> yaw) {

  415.         // Earth pointing (Z aligned with position) with linear yaw (momentum with known cos/sin in the X/Y plane)
  416.         final FieldPVCoordinates<T> svPV          = pvProv.getPVCoordinates(date, inertialFrame);
  417.         final FieldVector3D<T>      p             = svPV.getPosition();
  418.         final FieldVector3D<T>      v             = svPV.getVelocity();
  419.         final FieldVector3D<T>      a             = svPV.getAcceleration();
  420.         final T                     r2            = p.getNormSq();
  421.         final T                     r             = FastMath.sqrt(r2);
  422.         final FieldVector3D<T>      keplerianJerk = new FieldVector3D<>(FieldVector3D.dotProduct(p, v).multiply(-3).divide(r2), a,
  423.                                                                         a.getNorm().negate().divide(r), v);
  424.         final FieldPVCoordinates<T> velocity      = new FieldPVCoordinates<>(v, a, keplerianJerk);
  425.         final FieldPVCoordinates<T> momentum      = svPV.crossProduct(velocity);

  426.         final FieldDerivativeStructure<T> c = FastMath.cos(yaw).negate();
  427.         final FieldDerivativeStructure<T> s = FastMath.sin(yaw).negate();
  428.         final T                           z = yaw.getFactory().getValueField().getZero();
  429.         final FieldVector3D<T> m0 = new FieldVector3D<>(s.getValue(),              c.getValue(),              z);
  430.         final FieldVector3D<T> m1 = new FieldVector3D<>(s.getPartialDerivative(1), c.getPartialDerivative(1), z);
  431.         final FieldVector3D<T> m2 = new FieldVector3D<>(s.getPartialDerivative(2), c.getPartialDerivative(2), z);
  432.         return new TimeStampedFieldAngularCoordinates<>(date,
  433.                                                         svPV.normalize(), momentum.normalize(),
  434.                                                         minusZ, new FieldPVCoordinates<>(m0, m1, m2),
  435.                                                         1.0e-9);

  436.     }

  437.     /** Compute Orbit Normal (ON) yaw.
  438.      * @return Orbit Normal yaw, using inertial frame as reference
  439.      */
  440.     public TimeStampedFieldAngularCoordinates<T> orbitNormalYaw() {
  441.         final FieldTransform<T> t = LOFType.VVLH.transformFromInertial(date, pvProv.getPVCoordinates(date, inertialFrame));
  442.         return new TimeStampedFieldAngularCoordinates<>(date,
  443.                                                         t.getRotation(),
  444.                                                         t.getRotationRate(),
  445.                                                         t.getRotationAcceleration());
  446.     }

  447. }