GNSSFieldAttitudeContext.java

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

  19. import org.hipparchus.CalculusFieldElement;
  20. import org.hipparchus.Field;
  21. import org.hipparchus.analysis.UnivariateFunction;
  22. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
  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.FieldSinCos;
  29. import org.hipparchus.util.SinCos;
  30. import org.orekit.frames.FieldTransform;
  31. import org.orekit.frames.Frame;
  32. import org.orekit.frames.LOFType;
  33. import org.orekit.time.AbsoluteDate;
  34. import org.orekit.time.FieldAbsoluteDate;
  35. import org.orekit.time.FieldTimeStamped;
  36. import org.orekit.utils.ExtendedPVCoordinatesProvider;
  37. import org.orekit.utils.FieldPVCoordinates;
  38. import org.orekit.utils.FieldPVCoordinatesProvider;
  39. import org.orekit.utils.PVCoordinates;
  40. import org.orekit.utils.TimeStampedFieldAngularCoordinates;
  41. import org.orekit.utils.TimeStampedFieldPVCoordinates;

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

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

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

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

  70.     /** Constant Y axis. */
  71.     private final FieldPVCoordinates<T> plusY;

  73.     /** Constant Z axis. */
  74.     private final FieldPVCoordinates<T> minusZ;

  76.     /** Context date. */
  77.     private final AbsoluteDate dateDouble;

  79.     /** Current date. */
  80.     private final FieldAbsoluteDate<T> date;

  82.     /** Provider for Sun position. */
  83.     private final ExtendedPVCoordinatesProvider sun;

  85.     /** Provider for spacecraft position. */
  86.     private final FieldPVCoordinatesProvider<T> pvProv;

  88.     /** Spacecraft position at central date.
  89.      * @since 12.0
  90.      */
  91.     private final TimeStampedFieldPVCoordinates<T> svPV;

  93.     /** Sun position at central date.
  94.      * @since 12.0
  95.      */
  96.     private final TimeStampedFieldPVCoordinates<T> sunPV;

  98.     /** Inertial frame where velocity are computed. */
  99.     private final Frame inertialFrame;

  101.     /** Cosine of the angle between spacecraft and Sun direction. */
  102.     private final T svbCos;

  104.     /** Morning/Evening half orbit indicator. */
  105.     private final boolean morning;

  107.     /** Relative orbit angle to turn center. */
  108.     private final FieldUnivariateDerivative2<T> delta;

  110.     /** Sun elevation at center.
  111.      * @since 12.0
  112.      */
  113.     private final FieldUnivariateDerivative2<T> beta;

  115.     /** Spacecraft angular velocity. */
  116.     private T muRate;

  118.     /** Limit cosine for the midnight turn. */
  119.     private double cNight;

  121.     /** Limit cosine for the noon turn. */
  122.     private double cNoon;

  124.     /** Turn time data. */
  125.     private FieldTurnSpan<T> turnSpan;

  127.     /** Simple constructor.
  128.      * @param date current date
  129.      * @param sun provider for Sun position
  130.      * @param pvProv provider for spacecraft position
  131.      * @param inertialFrame inertial frame where velocity are computed
  132.      * @param turnSpan turn time data, if a turn has already been identified in the date neighborhood,
  133.      * null otherwise
  134.      */
  135.     GNSSFieldAttitudeContext(final FieldAbsoluteDate<T> date,
  136.                              final ExtendedPVCoordinatesProvider sun, final FieldPVCoordinatesProvider<T> pvProv,
  137.                              final Frame inertialFrame,  final FieldTurnSpan<T> turnSpan) {

  139.         final Field<T> field = date.getField();
  140.         plusY    = new FieldPVCoordinates<>(field, PLUS_Y_PV);
  141.         minusZ   = new FieldPVCoordinates<>(field, MINUS_Z_PV);

  143.         this.dateDouble    = date.toAbsoluteDate();
  144.         this.date          = date;
  145.         this.sun           = sun;
  146.         this.pvProv        = pvProv;
  147.         this.inertialFrame = inertialFrame;
  148.         this.sunPV         = sun.getPVCoordinates(date, inertialFrame);
  149.         this.svPV          = pvProv.getPVCoordinates(date, inertialFrame);
  150.         this.morning       = Vector3D.dotProduct(svPV.getVelocity().toVector3D(), sunPV.getPosition().toVector3D()) >= 0.0;
  151.         this.muRate        = svPV.getAngularVelocity().getNorm();
  152.         this.turnSpan      = turnSpan;

  154.         final FieldPVCoordinates<FieldUnivariateDerivative2<T>> sunPVD2 = sunPV.toUnivariateDerivative2PV();
  155.         final FieldPVCoordinates<FieldUnivariateDerivative2<T>> svPVD2  = svPV.toUnivariateDerivative2PV();
  156.         final FieldUnivariateDerivative2<T> svbCosD2  = FieldVector3D.dotProduct(sunPVD2.getPosition(), svPVD2.getPosition()).
  157.                                                         divide(sunPVD2.getPosition().getNorm().multiply(svPVD2.getPosition().getNorm()));
  158.         svbCos = svbCosD2.getValue();

  160.         beta  = FieldVector3D.angle(sunPVD2.getPosition(), svPVD2.getMomentum()).negate().add(0.5 * FastMath.PI);

  162.         final FieldUnivariateDerivative2<T> absDelta;
  163.         if (svbCos.getReal() <= 0) {
  164.             // night side
  165.             absDelta = FastMath.acos(svbCosD2.negate().divide(FastMath.cos(beta)));
  166.         } else {
  167.             // Sun side
  168.             absDelta = FastMath.acos(svbCosD2.divide(FastMath.cos(beta)));
  169.         }
  170.         delta = absDelta.copySign(absDelta.getPartialDerivative(1).negate());

  172.     }

  174.     /** Compute nominal yaw steering.
  175.      * @param d computation date
  176.      * @return nominal yaw steering
  177.      */
  178.     public TimeStampedFieldAngularCoordinates<T> nominalYaw(final FieldAbsoluteDate<T> d) {
  179.         final TimeStampedFieldPVCoordinates<T> pv = pvProv.getPVCoordinates(d, inertialFrame);
  180.         return new TimeStampedFieldAngularCoordinates<>(d,
  181.                                                         pv.normalize(),
  182.                                                         sun.getPVCoordinates(d, inertialFrame).crossProduct(pv).normalize(),
  183.                                                         minusZ,
  184.                                                         plusY,
  185.                                                         1.0e-9);
  186.     }

  188.     /** Compute Sun elevation.
  189.      * @param d computation date
  190.      * @return Sun elevation
  191.      */
  192.     public T beta(final FieldAbsoluteDate<T> d) {
  193.         final TimeStampedFieldPVCoordinates<T> pv = pvProv.getPVCoordinates(d, inertialFrame);
  194.         return FieldVector3D.angle(sun.getPosition(d, inertialFrame), pv.getMomentum()).
  195.                negate().
  196.                add(svPV.getPosition().getX().getPi().multiply(0.5));
  197.     }

  199.     /** Compute Sun elevation.
  200.      * @return Sun elevation
  201.      */
  202.     public FieldUnivariateDerivative2<T> betaD2() {
  203.         return beta;
  204.     }

  206.     /** {@inheritDoc} */
  207.     @Override
  208.     public FieldAbsoluteDate<T> getDate() {
  209.         return date;
  210.     }

  212.     /** Get the turn span.
  213.      * @return turn span, may be null if context is outside of turn
  214.      */
  215.     public FieldTurnSpan<T> getTurnSpan() {
  216.         return turnSpan;
  217.     }

  219.     /** Get the cosine of the angle between spacecraft and Sun direction.
  220.      * @return cosine of the angle between spacecraft and Sun direction.
  221.      */
  222.     public T getSVBcos() {
  223.         return svbCos;
  224.     }

  226.     /** Get a Sun elevation angle that does not change sign within the turn.
  227.      * <p>
  228.      * This method either returns the current beta or replaces it with the
  229.      * value at turn start, so the sign remains constant throughout the
  230.      * turn. As of 9.2, it is used for GPS, Glonass and Galileo.
  231.      * </p>
  232.      * @return secured Sun elevation angle
  233.      * @see #beta(FieldAbsoluteDate)
  234.      * @see #betaDS(FieldAbsoluteDate)
  235.      */
  236.     public T getSecuredBeta() {
  237.         return FastMath.abs(beta.getValue().getReal()) < BETA_SIGN_CHANGE_PROTECTION ?
  238.                beta(turnSpan.getTurnStartDate()) :
  239.                beta.getValue();
  240.     }

  242.     /** Check if a linear yaw model is still active or if we already reached target yaw.
  243.      * @param linearPhi value of the linear yaw model
  244.      * @param phiDot slope of the linear yaw model
  245.      * @return true if linear model is still active
  246.      */
  247.     public boolean linearModelStillActive(final T linearPhi, final T phiDot) {
  248.         final AbsoluteDate absDate = date.toAbsoluteDate();
  249.         final double dt0 = turnSpan.getTurnEndDate().durationFrom(date).getReal();
  250.         final UnivariateFunction yawReached = dt -> {
  251.             final AbsoluteDate  t       = absDate.shiftedBy(dt);
  252.             final Vector3D      pSun    = sun.getPosition(t, inertialFrame);
  253.             final PVCoordinates pv      = pvProv.getPVCoordinates(date.shiftedBy(dt), inertialFrame).toPVCoordinates();
  254.             final Vector3D      pSat    = pv.getPosition();
  255.             final Vector3D      targetX = Vector3D.crossProduct(pSat, Vector3D.crossProduct(pSun, pSat)).normalize();

  257.             final double        phi         = linearPhi.getReal() + dt * phiDot.getReal();
  258.             final SinCos        sc          = FastMath.sinCos(phi);
  259.             final Vector3D      pU          = pv.getPosition().normalize();
  260.             final Vector3D      mU          = pv.getMomentum().normalize();
  261.             final Vector3D      omega       = new Vector3D(-phiDot.getReal(), pU);
  262.             final Vector3D      currentX    = new Vector3D(-sc.sin(), mU, -sc.cos(), Vector3D.crossProduct(pU, mU));
  263.             final Vector3D      currentXDot = Vector3D.crossProduct(omega, currentX);

  265.             return Vector3D.dotProduct(targetX, currentXDot);
  266.         };
  267.         final double fullTurn = 2 * FastMath.PI / FastMath.abs(phiDot.getReal());
  268.         final double dtMin    = FastMath.min(turnSpan.getTurnStartDate().durationFrom(date).getReal(), dt0 - 60.0);
  269.         final double dtMax    = FastMath.max(dtMin + fullTurn, dt0 + 60.0);
  270.         double[] bracket = UnivariateSolverUtils.bracket(yawReached, dt0,
  271.                                                          dtMin, dtMax, fullTurn / 100, 1.0, 100);
  272.         if (yawReached.value(bracket[0]) <= 0.0) {
  273.             // we have bracketed the wrong crossing
  274.             bracket = UnivariateSolverUtils.bracket(yawReached, 0.5 * (bracket[0] + bracket[1] + fullTurn),
  275.                                                     bracket[1], bracket[1] + fullTurn, fullTurn / 100, 1.0, 100);
  276.         }
  277.         final double dt = new BracketingNthOrderBrentSolver(1.0e-3, 5).
  278.                           solve(100, yawReached, bracket[0], bracket[1]);
  279.         turnSpan.updateEnd(date.shiftedBy(dt), absDate);

  281.         return dt > 0.0;

  283.     }

  285.     /** Set up the midnight/noon turn region.
  286.      * @param cosNight limit cosine for the midnight turn
  287.      * @param cosNoon limit cosine for the noon turn
  288.      * @return true if spacecraft is in the midnight/noon turn region
  289.      */
  290.     public boolean setUpTurnRegion(final double cosNight, final double cosNoon) {
  291.         this.cNight = cosNight;
  292.         this.cNoon  = cosNoon;

  294.         if (svbCos.getReal() < cNight || svbCos.getReal() > cNoon) {
  295.             // we are within turn triggering zone
  296.             return true;
  297.         } else {
  298.             // we are outside of turn triggering zone,
  299.             // but we may still be trying to recover nominal attitude at the end of a turn
  300.             return inTurnTimeRange();
  301.         }

  303.     }

  305.     /** Get the relative orbit angle to turn center.
  306.      * @return relative orbit angle to turn center
  307.      */
  308.     public FieldUnivariateDerivative2<T> getDeltaDS() {
  309.         return delta;
  310.     }

  312.     /** Get the orbit angle since solar midnight.
  313.      * @return orbit angle since solar midnight
  314.      */
  315.     public T getOrbitAngleSinceMidnight() {
  316.         final T absAngle = inOrbitPlaneAbsoluteAngle(FastMath.acos(svbCos).negate().add(svbCos.getPi()));
  317.         return morning ? absAngle : absAngle.negate();
  318.     }

  320.     /** Check if spacecraft is in the half orbit closest to Sun.
  321.      * @return true if spacecraft is in the half orbit closest to Sun
  322.      */
  323.     public boolean inSunSide() {
  324.         return svbCos.getReal() > 0;
  325.     }

  327.     /** Get yaw at turn start.
  328.      * @param sunBeta Sun elevation above orbital plane
  329.      * (it <em>may</em> be different from {@link #getBeta()} in
  330.      * some special cases)
  331.      * @return yaw at turn start
  332.      */
  333.     public T getYawStart(final T sunBeta) {
  334.         final T halfSpan = turnSpan.getTurnDuration().multiply(muRate).multiply(0.5);
  335.         return computePhi(sunBeta, FastMath.copySign(halfSpan, svbCos));
  336.     }

  338.     /** Get yaw at turn end.
  339.      * @param sunBeta Sun elevation above orbital plane
  340.      * (it <em>may</em> be different from {@link #getBeta()} in
  341.      * some special cases)
  342.      * @return yaw at turn end
  343.      */
  344.     public T getYawEnd(final T sunBeta) {
  345.         final T halfSpan = turnSpan.getTurnDuration().multiply(muRate).multiply(0.5);
  346.         return computePhi(sunBeta, FastMath.copySign(halfSpan, svbCos.negate()));
  347.     }

  349.     /** Compute yaw rate.
  350.      * @param sunBeta Sun elevation above orbital plane
  351.      * (it <em>may</em> be different from {@link #getBeta()} in
  352.      * some special cases)
  353.      * @return yaw rate
  354.      */
  355.     public T yawRate(final T sunBeta) {
  356.         return getYawEnd(sunBeta).subtract(getYawStart(sunBeta)).divide(turnSpan.getTurnDuration());
  357.     }

  359.     /** Get the spacecraft angular velocity.
  360.      * @return spacecraft angular velocity
  361.      */
  362.     public T getMuRate() {
  363.         return muRate;
  364.     }

  366.     /** Project a spacecraft/Sun angle into orbital plane.
  367.      * <p>
  368.      * This method is intended to find the limits of the noon and midnight
  369.      * turns in orbital plane. The return angle is always positive. The
  370.      * correct sign to apply depends on the spacecraft being before or
  371.      * after turn middle point.
  372.      * </p>
  373.      * @param angle spacecraft/Sun angle (or spacecraft/opposite-of-Sun)
  374.      * @return angle projected into orbital plane, always positive
  375.      */
  376.     public T inOrbitPlaneAbsoluteAngle(final T angle) {
  377.         return FastMath.acos(FastMath.cos(angle).divide(FastMath.cos(beta(getDate()))));
  378.     }

  380.     /** Compute yaw.
  381.      * @param sunBeta Sun elevation above orbital plane
  382.      * (it <em>may</em> be different from {@link #getBeta()} in
  383.      * some special cases)
  384.      * @param inOrbitPlaneAngle in orbit angle between spacecraft
  385.      * and Sun (or opposite of Sun) projection
  386.      * @return yaw angle
  387.      */
  388.     public T computePhi(final T sunBeta, final T inOrbitPlaneAngle) {
  389.         return FastMath.atan2(FastMath.tan(sunBeta).negate(), FastMath.sin(inOrbitPlaneAngle));
  390.     }

  392.     /** Set turn half span and compute corresponding turn time range.
  393.      * @param halfSpan half span of the turn region, as an angle in orbit plane
  394.      * @param endMargin margin in seconds after turn end
  395.      */
  396.     public void setHalfSpan(final T halfSpan, final double endMargin) {
  397.         final FieldAbsoluteDate<T> start = date.shiftedBy(delta.getValue().subtract(halfSpan).divide(muRate));
  398.         final FieldAbsoluteDate<T> end   = date.shiftedBy(delta.getValue().add(halfSpan).divide(muRate));
  399.         final AbsoluteDate estimationDate = getDate().toAbsoluteDate();
  400.         if (turnSpan == null) {
  401.             turnSpan = new FieldTurnSpan<>(start, end, estimationDate, endMargin);
  402.         } else {
  403.             turnSpan.updateStart(start, estimationDate);
  404.             turnSpan.updateEnd(end, estimationDate);
  405.         }
  406.     }

  408.     /** Check if context is within turn range.
  409.      * @return true if context is within range extended by end margin
  410.      */
  411.     public boolean inTurnTimeRange() {
  412.         return turnSpan != null && turnSpan.inTurnTimeRange(dateDouble);
  413.     }

  415.     /** Get elapsed time since turn start.
  416.      * @return elapsed time from turn start to current date
  417.      */
  418.     public T timeSinceTurnStart() {
  419.         return getDate().durationFrom(turnSpan.getTurnStartDate());
  420.     }

  422.     /** Generate an attitude with turn-corrected yaw.
  423.      * @param yaw yaw value to apply
  424.      * @param yawDot yaw first time derivative
  425.      * @return attitude with specified yaw
  426.      */
  427.     public TimeStampedFieldAngularCoordinates<T> turnCorrectedAttitude(final T yaw, final T yawDot) {
  428.         return turnCorrectedAttitude(new FieldUnivariateDerivative2<>(yaw, yawDot, yaw.getField().getZero()));
  429.     }

  431.     /** Generate an attitude with turn-corrected yaw.
  432.      * @param yaw yaw value to apply
  433.      * @return attitude with specified yaw
  434.      */
  435.     public TimeStampedFieldAngularCoordinates<T> turnCorrectedAttitude(final FieldUnivariateDerivative2<T> yaw) {

  437.         // Earth pointing (Z aligned with position) with linear yaw (momentum with known cos/sin in the X/Y plane)
  438.         final FieldVector3D<T>      p             = svPV.getPosition();
  439.         final FieldVector3D<T>      v             = svPV.getVelocity();
  440.         final FieldVector3D<T>      a             = svPV.getAcceleration();
  441.         final T                     r2            = p.getNormSq();
  442.         final T                     r             = FastMath.sqrt(r2);
  443.         final FieldVector3D<T>      keplerianJerk = new FieldVector3D<>(FieldVector3D.dotProduct(p, v).multiply(-3).divide(r2), a,
  444.                                                                         a.getNorm().negate().divide(r), v);
  445.         final FieldPVCoordinates<T> velocity      = new FieldPVCoordinates<>(v, a, keplerianJerk);
  446.         final FieldPVCoordinates<T> momentum      = svPV.crossProduct(velocity);

  448.         final FieldSinCos<FieldUnivariateDerivative2<T>> sc = FastMath.sinCos(yaw);
  449.         final FieldUnivariateDerivative2<T> c = sc.cos().negate();
  450.         final FieldUnivariateDerivative2<T> s = sc.sin().negate();
  451.         final T                             z = yaw.getValueField().getZero();
  452.         final FieldVector3D<T> m0 = new FieldVector3D<>(s.getValue(),              c.getValue(),              z);
  453.         final FieldVector3D<T> m1 = new FieldVector3D<>(s.getPartialDerivative(1), c.getPartialDerivative(1), z);
  454.         final FieldVector3D<T> m2 = new FieldVector3D<>(s.getPartialDerivative(2), c.getPartialDerivative(2), z);
  455.         return new TimeStampedFieldAngularCoordinates<>(date,
  456.                                                         svPV.normalize(), momentum.normalize(),
  457.                                                         minusZ, new FieldPVCoordinates<>(m0, m1, m2),
  458.                                                         1.0e-9);

  460.     }

  462.     /** Compute Orbit Normal (ON) yaw.
  463.      * @return Orbit Normal yaw, using inertial frame as reference
  464.      */
  465.     public TimeStampedFieldAngularCoordinates<T> orbitNormalYaw() {
  466.         final FieldTransform<T> t = LOFType.LVLH_CCSDS.transformFromInertial(date, pvProv.getPVCoordinates(date, inertialFrame));
  467.         return new TimeStampedFieldAngularCoordinates<>(date,
  468.                                                         t.getRotation(),
  469.                                                         t.getRotationRate(),
  470.                                                         t.getRotationAcceleration());
  471.     }

  473. }
Created with JaCoCo 0.8.11.202310140853