GNSSAttitudeContext.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.analysis.UnivariateFunction;
  20. import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
  21. import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
  22. import org.hipparchus.analysis.solvers.UnivariateSolverUtils;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  25. import org.hipparchus.util.FastMath;
  26. import org.hipparchus.util.FieldSinCos;
  27. import org.hipparchus.util.SinCos;
  28. import org.orekit.frames.Frame;
  29. import org.orekit.frames.LOFType;
  30. import org.orekit.frames.Transform;
  31. import org.orekit.time.AbsoluteDate;
  32. import org.orekit.time.FieldAbsoluteDate;
  33. import org.orekit.time.TimeStamped;
  34. import org.orekit.utils.ExtendedPVCoordinatesProvider;
  35. import org.orekit.utils.FieldPVCoordinates;
  36. import org.orekit.utils.PVCoordinates;
  37. import org.orekit.utils.PVCoordinatesProvider;
  38. import org.orekit.utils.TimeStampedAngularCoordinates;
  39. import org.orekit.utils.TimeStampedPVCoordinates;

  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.PVCoordinatesProvider,
  47.  * org.orekit.time.AbsoluteDate, org.orekit.frames.Frame) getAttitude}.
  48.  * </p>
  49.  *
  50.  * @author Luc Maisonobe
  51.  * @since 9.2
  52.  */
  53. class GNSSAttitudeContext implements TimeStamped {

  55.     /** Constant Y axis. */
  56.     private static final PVCoordinates PLUS_Y_PV =
  57.             new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);

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

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

  66.     /** Current date. */
  67.     private final AbsoluteDate date;

  69.     /** Provider for Sun position. */
  70.     private final ExtendedPVCoordinatesProvider sun;

  72.     /** Provider for spacecraft position. */
  73.     private final PVCoordinatesProvider pvProv;

  75.     /** Spacecraft position at central date.
  76.      * @since 12.0
  77.      */
  78.     private final TimeStampedPVCoordinates svPV;

  80.     /** Sun position at central date.
  81.      * @since 12.0
  82.      */
  83.     private final TimeStampedPVCoordinates sunPV;

  85.     /** Inertial frame where velocity are computed. */
  86.     private final Frame inertialFrame;

  88.     /** Cosine of the angle between spacecraft and Sun direction. */
  89.     private final double svbCos;

  91.     /** Morning/Evening half orbit indicator. */
  92.     private final boolean morning;

  94.     /** Relative orbit angle to turn center.
  95.      * @since 12.0
  96.      */
  97.     private UnivariateDerivative2 delta;

  99.     /** Sun elevation at center.
  100.      * @since 12.0
  101.      */
  102.     private UnivariateDerivative2 beta;

  104.     /** Spacecraft angular velocity. */
  105.     private double muRate;

  107.     /** Limit cosine for the midnight turn. */
  108.     private double cNight;

  110.     /** Limit cosine for the noon turn. */
  111.     private double cNoon;

  113.     /** Turn time data. */
  114.     private TurnSpan turnSpan;

  116.     /** Simple constructor.
  117.      * @param date current date
  118.      * @param sun provider for Sun position
  119.      * @param pvProv provider for spacecraft position
  120.      * @param inertialFrame inertial frame where velocity are computed
  121.      * @param turnSpan turn time data, if a turn has already been identified in the date neighborhood,
  122.      * null otherwise
  123.      */
  124.     GNSSAttitudeContext(final AbsoluteDate date,
  125.                         final ExtendedPVCoordinatesProvider sun, final PVCoordinatesProvider pvProv,
  126.                         final Frame inertialFrame, final TurnSpan turnSpan) {

  128.         this.date          = date;
  129.         this.sun           = sun;
  130.         this.pvProv        = pvProv;
  131.         this.inertialFrame = inertialFrame;
  132.         this.sunPV         = sun.getPVCoordinates(date, inertialFrame);
  133.         this.svPV          = pvProv.getPVCoordinates(date, inertialFrame);
  134.         this.morning       = Vector3D.dotProduct(svPV.getVelocity(), sunPV.getPosition()) >= 0.0;
  135.         this.muRate        = svPV.getAngularVelocity().getNorm();
  136.         this.turnSpan      = turnSpan;

  138.         final FieldPVCoordinates<UnivariateDerivative2> sunPVD2 = sunPV.toUnivariateDerivative2PV();
  139.         final FieldPVCoordinates<UnivariateDerivative2> svPVD2  = svPV.toUnivariateDerivative2PV();
  140.         final UnivariateDerivative2 svbCosD2  = FieldVector3D.dotProduct(sunPVD2.getPosition(), svPVD2.getPosition()).
  141.                                                 divide(sunPVD2.getPosition().getNorm().multiply(svPVD2.getPosition().getNorm()));
  142.         svbCos = svbCosD2.getValue();

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

  146.         final UnivariateDerivative2 absDelta;
  147.         if (svbCos <= 0) {
  148.             // night side
  149.             absDelta = FastMath.acos(svbCosD2.negate().divide(FastMath.cos(beta)));
  150.         } else {
  151.             // Sun side
  152.             absDelta = FastMath.acos(svbCosD2.divide(FastMath.cos(beta)));
  153.         }
  154.         delta = FastMath.copySign(absDelta, -absDelta.getPartialDerivative(1));

  156.     }

  158.     /** Compute nominal yaw steering.
  159.      * @param d computation date
  160.      * @return nominal yaw steering
  161.      */
  162.     public TimeStampedAngularCoordinates nominalYaw(final AbsoluteDate d) {
  163.         final PVCoordinates pv = pvProv.getPVCoordinates(d, inertialFrame);
  164.         return new TimeStampedAngularCoordinates(d,
  165.                                                  pv.normalize(),
  166.                                                  PVCoordinates.crossProduct(sun.getPVCoordinates(d, inertialFrame), pv).normalize(),
  167.                                                  MINUS_Z_PV,
  168.                                                  PLUS_Y_PV,
  169.                                                  1.0e-9);
  170.     }

  172.     /** Compute Sun elevation.
  173.      * @param d computation date
  174.      * @return Sun elevation
  175.      */
  176.     public double beta(final AbsoluteDate d) {
  177.         final TimeStampedPVCoordinates pv = pvProv.getPVCoordinates(d, inertialFrame);
  178.         return 0.5 * FastMath.PI - Vector3D.angle(sun.getPosition(d, inertialFrame), pv.getMomentum());
  179.     }

  181.     /** Compute Sun elevation.
  182.      * @return Sun elevation
  183.      */
  184.     public UnivariateDerivative2 betaD2() {
  185.         return beta;
  186.     }

  188.     /** {@inheritDoc} */
  189.     @Override
  190.     public AbsoluteDate getDate() {
  191.         return date;
  192.     }

  194.     /** Get the turn span.
  195.      * @return turn span, may be null if context is outside of turn
  196.      */
  197.     public TurnSpan getTurnSpan() {
  198.         return turnSpan;
  199.     }

  201.     /** Get the cosine of the angle between spacecraft and Sun direction.
  202.      * @return cosine of the angle between spacecraft and Sun direction.
  203.      */
  204.     public double getSVBcos() {
  205.         return svbCos;
  206.     }

  208.     /** Get a Sun elevation angle that does not change sign within the turn.
  209.      * <p>
  210.      * This method either returns the current beta or replaces it with the
  211.      * value at turn start, so the sign remains constant throughout the
  212.      * turn. As of 9.2, it is used for GPS, Glonass and Galileo.
  213.      * </p>
  214.      * @return secured Sun elevation angle
  215.      * @see #beta(AbsoluteDate)
  216.      * @see #betaDS(FieldAbsoluteDate)
  217.      */
  218.     public double getSecuredBeta() {
  219.         return FastMath.abs(beta.getValue()) < BETA_SIGN_CHANGE_PROTECTION ?
  220.                beta(turnSpan.getTurnStartDate()) :
  221.                beta.getValue();
  222.     }

  224.     /** Check if a linear yaw model is still active or if we already reached target yaw.
  225.      * @param linearPhi value of the linear yaw model
  226.      * @param phiDot slope of the linear yaw model
  227.      * @return true if linear model is still active
  228.      */
  229.     public boolean linearModelStillActive(final double linearPhi, final double phiDot) {
  230.         final double dt0 = turnSpan.getTurnEndDate().durationFrom(date);
  231.         final UnivariateFunction yawReached = dt -> {
  232.             final AbsoluteDate  t       = date.shiftedBy(dt);
  233.             final Vector3D      pSun    = sun.getPosition(t, inertialFrame);
  234.             final PVCoordinates pv      = pvProv.getPVCoordinates(t, inertialFrame);
  235.             final Vector3D      pSat    = pv.getPosition();
  236.             final Vector3D      targetX = Vector3D.crossProduct(pSat, Vector3D.crossProduct(pSun, pSat)).normalize();

  238.             final double        phi         = linearPhi + dt * phiDot;
  239.             final SinCos        sc          = FastMath.sinCos(phi);
  240.             final Vector3D      pU          = pv.getPosition().normalize();
  241.             final Vector3D      mU          = pv.getMomentum().normalize();
  242.             final Vector3D      omega       = new Vector3D(-phiDot, pU);
  243.             final Vector3D      currentX    = new Vector3D(-sc.sin(), mU, -sc.cos(), Vector3D.crossProduct(pU, mU));
  244.             final Vector3D      currentXDot = Vector3D.crossProduct(omega, currentX);

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

  262.         return dt > 0.0;

  264.     }

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

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

  284.     }

  286.     /** Get the relative orbit angle to turn center.
  287.      * @return relative orbit angle to turn center
  288.      */
  289.     public UnivariateDerivative2 getDeltaDS() {
  290.         return delta;
  291.     }

  293.     /** Get the orbit angle since solar midnight.
  294.      * @return orbit angle since solar midnight
  295.      */
  296.     public double getOrbitAngleSinceMidnight() {
  297.         final double absAngle = inOrbitPlaneAbsoluteAngle(FastMath.PI - FastMath.acos(svbCos));
  298.         return morning ? absAngle : -absAngle;
  299.     }

  301.     /** Check if spacecraft is in the half orbit closest to Sun.
  302.      * @return true if spacecraft is in the half orbit closest to Sun
  303.      */
  304.     public boolean inSunSide() {
  305.         return svbCos > 0;
  306.     }

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

  319.     /** Get yaw at turn end.
  320.      * @param sunBeta Sun elevation above orbital plane
  321.      * (it <em>may</em> be different from {@link #getBeta()} in
  322.      * some special cases)
  323.      * @return yaw at turn end
  324.      */
  325.     public double getYawEnd(final double sunBeta) {
  326.         final double halfSpan = 0.5 * turnSpan.getTurnDuration() * muRate;
  327.         return computePhi(sunBeta, FastMath.copySign(halfSpan, -svbCos));
  328.     }

  330.     /** Compute yaw rate.
  331.      * @param sunBeta Sun elevation above orbital plane
  332.      * (it <em>may</em> be different from {@link #getBeta()} in
  333.      * some special cases)
  334.      * @return yaw rate
  335.      */
  336.     public double yawRate(final double sunBeta) {
  337.         return (getYawEnd(sunBeta) - getYawStart(sunBeta)) / turnSpan.getTurnDuration();
  338.     }

  340.     /** Get the spacecraft angular velocity.
  341.      * @return spacecraft angular velocity
  342.      */
  343.     public double getMuRate() {
  344.         return muRate;
  345.     }

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

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

  373.     /** Set turn half span and compute corresponding turn time range.
  374.      * @param halfSpan half span of the turn region, as an angle in orbit plane
  375.      * @param endMargin margin in seconds after turn end
  376.      */
  377.     public void setHalfSpan(final double halfSpan, final double endMargin) {

  379.         final AbsoluteDate start = date.shiftedBy((delta.getValue() - halfSpan) / muRate);
  380.         final AbsoluteDate end   = date.shiftedBy((delta.getValue() + halfSpan) / muRate);
  381.         final AbsoluteDate estimationDate = getDate();

  383.         if (turnSpan == null) {
  384.             turnSpan = new TurnSpan(start, end, estimationDate, endMargin);
  385.         } else {
  386.             turnSpan.updateStart(start, estimationDate);
  387.             turnSpan.updateEnd(end, estimationDate);
  388.         }
  389.     }

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

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

  405.     /** Generate an attitude with turn-corrected yaw.
  406.      * @param yaw yaw value to apply
  407.      * @param yawDot yaw first time derivative
  408.      * @return attitude with specified yaw
  409.      */
  410.     public TimeStampedAngularCoordinates turnCorrectedAttitude(final double yaw, final double yawDot) {
  411.         return turnCorrectedAttitude(new UnivariateDerivative2(yaw, yawDot, 0.0));
  412.     }

  414.     /** Generate an attitude with turn-corrected yaw.
  415.      * @param yaw yaw value to apply
  416.      * @return attitude with specified yaw
  417.      */
  418.     public TimeStampedAngularCoordinates turnCorrectedAttitude(final UnivariateDerivative2 yaw) {

  420.         // Earth pointing (Z aligned with position) with linear yaw (momentum with known cos/sin in the X/Y plane)
  421.         final Vector3D      p             = svPV.getPosition();
  422.         final Vector3D      v             = svPV.getVelocity();
  423.         final Vector3D      a             = svPV.getAcceleration();
  424.         final double        r2            = p.getNormSq();
  425.         final double        r             = FastMath.sqrt(r2);
  426.         final Vector3D      keplerianJerk = new Vector3D(-3 * Vector3D.dotProduct(p, v) / r2, a, -a.getNorm() / r, v);
  427.         final PVCoordinates velocity      = new PVCoordinates(v, a, keplerianJerk);
  428.         final PVCoordinates momentum      = PVCoordinates.crossProduct(svPV, velocity);

  430.         final FieldSinCos<UnivariateDerivative2> sc = FastMath.sinCos(yaw);
  431.         final UnivariateDerivative2 c = sc.cos().negate();
  432.         final UnivariateDerivative2 s = sc.sin().negate();
  433.         final Vector3D m0 = new Vector3D(s.getValue(),              c.getValue(),              0.0);
  434.         final Vector3D m1 = new Vector3D(s.getPartialDerivative(1), c.getPartialDerivative(1), 0.0);
  435.         final Vector3D m2 = new Vector3D(s.getPartialDerivative(2), c.getPartialDerivative(2), 0.0);
  436.         return new TimeStampedAngularCoordinates(date,
  437.                                                  svPV.normalize(), momentum.normalize(),
  438.                                                  MINUS_Z_PV, new PVCoordinates(m0, m1, m2),
  439.                                                  1.0e-9);

  441.     }

  443.     /** Compute Orbit Normal (ON) yaw.
  444.      * @return Orbit Normal yaw, using inertial frame as reference
  445.      */
  446.     public TimeStampedAngularCoordinates orbitNormalYaw() {
  447.         final Transform t = LOFType.LVLH_CCSDS.transformFromInertial(date, pvProv.getPVCoordinates(date, inertialFrame));
  448.         return new TimeStampedAngularCoordinates(date,
  449.                                                  t.getRotation(),
  450.                                                  t.getRotationRate(),
  451.                                                  t.getRotationAcceleration());
  452.     }

  454. }
Created with JaCoCo 0.8.11.202310140853