GNSSFieldAttitudeContext.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.gnss.attitude;

  19. import org.hipparchus.Field;
  20. import org.hipparchus.RealFieldElement;
  21. import org.hipparchus.analysis.differentiation.FDSFactory;
  22. import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
  23. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  24. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  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.orekit.errors.OrekitException;
  29. import org.orekit.frames.FieldTransform;
  30. import org.orekit.frames.LOFType;
  31. import org.orekit.time.FieldAbsoluteDate;
  32. import org.orekit.time.FieldTimeStamped;
  33. import org.orekit.utils.FieldPVCoordinates;
  34. import org.orekit.utils.PVCoordinates;
  35. import org.orekit.utils.TimeStampedFieldAngularCoordinates;
  36. import org.orekit.utils.TimeStampedFieldPVCoordinates;

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

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

  59.     /** Constant Z axis. */
  60.     private static final PVCoordinates MINUS_Z =
  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.     /** Derivation order. */
  67.     private static final int ORDER = 2;

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

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

  75.     /** Spacecraft position-velocity in inertial frame. */
  76.     private final TimeStampedFieldPVCoordinates<T> svPV;

  78.     /** Spacecraft position-velocity in inertial frame. */
  79.     private final FieldPVCoordinates<FieldDerivativeStructure<T>> svPVDS;

  81.     /** Angle between Sun and orbital plane. */
  82.     private final FieldDerivativeStructure<T> beta;

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

  87.     /** Nominal yaw. */
  88.     private final TimeStampedFieldAngularCoordinates<T> nominalYaw;

  90.     /** Nominal yaw. */
  91.     private final FieldRotation<FieldDerivativeStructure<T>> nominalYawDS;

  93.     /** Spacecraft angular velocity. */
  94.     private T muRate;

  96.     /** Limit cosine for the midnight turn. */
  97.     private double cNight;

  99.     /** Limit cosine for the noon turn. */
  100.     private double cNoon;

  102.     /** Relative orbit angle to turn center. */
  103.     private FieldDerivativeStructure<T> delta;

  105.     /** Half span of the turn region, as an angle in orbit plane. */
  106.     private T halfSpan;

  108.     /** Turn start date. */
  109.     private FieldAbsoluteDate<T> turnStart;

  111.     /** Turn end date. */
  112.     private FieldAbsoluteDate<T> turnEnd;

  114.     /** Simple constructor.
  115.      * @param sunPV Sun position-velocity in inertial frame
  116.      * @param svPV spacecraft position-velocity in inertial frame
  117.      * @exception OrekitException if yaw cannot be corrected
  118.      */
  119.     GNSSFieldAttitudeContext(final TimeStampedFieldPVCoordinates<T> sunPV, final TimeStampedFieldPVCoordinates<T> svPV)
  120.         throws OrekitException {

  122.         final Field<T> field = sunPV.getDate().getField();
  123.         plusY  = new FieldPVCoordinates<>(field, PLUS_Y);
  124.         minusZ = new FieldPVCoordinates<>(field, MINUS_Z);

  126.         final FieldPVCoordinates<FieldDerivativeStructure<T>> sunPVDS = sunPV.toDerivativeStructurePV(ORDER);
  127.         this.svPV    = svPV;
  128.         this.svPVDS  = svPV.toDerivativeStructurePV(ORDER);
  129.         this.svbCos  = FieldVector3D.dotProduct(sunPVDS.getPosition(), svPVDS.getPosition()).
  130.                        divide(sunPVDS.getPosition().getNorm().
  131.                        multiply(svPVDS.getPosition().getNorm()));
  132.         this.beta    = FieldVector3D.angle(sunPVDS.getPosition(), svPVDS.getMomentum()).
  133.                        negate().
  134.                        add(0.5 * FastMath.PI);

  136.         // nominal yaw steering
  137.         this.nominalYaw =
  138.                         new TimeStampedFieldAngularCoordinates<>(svPV.getDate(),
  139.                                                                  svPV.normalize(),
  140.                                                                  sunPV.crossProduct(svPV).normalize(),
  141.                                                                  minusZ,
  142.                                                                  plusY,
  143.                                                                  1.0e-9);
  144.         this.nominalYawDS = nominalYaw.toDerivativeStructureRotation(ORDER);

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

  148.     }

  150.     /** {@inheritDoc} */
  151.     @Override
  152.     public FieldAbsoluteDate<T> getDate() {
  153.         return svPV.getDate();
  154.     }

  156.     /** Get the cosine of the angle between spacecraft and Sun direction.
  157.      * @return cosine of the angle between spacecraft and Sun direction.
  158.      */
  159.     public T getSVBcos() {
  160.         return svbCos.getValue();
  161.     }

  163.     /** Get the angle between Sun and orbital plane.
  164.      * @return angle between Sun and orbital plane
  165.      * @see #getBetaDS()
  166.      * @see #getSecuredBeta(TurnTimeRange)
  167.      */
  168.     public T getBeta() {
  169.         return beta.getValue();
  170.     }

  172.     /** Get the angle between Sun and orbital plane.
  173.      * @return angle between Sun and orbital plane
  174.      * @see #getBeta()
  175.      * @see #getSecuredBeta(TurnTimeRange)
  176.      */
  177.     public FieldDerivativeStructure<T> getBetaDS() {
  178.         return beta;
  179.     }

  181.     /** Get a Sun elevation angle that does not change sign within the turn.
  182.      * <p>
  183.      * This method either returns the current beta or replaces it with the
  184.      * value at turn start, so the sign remains constant throughout the
  185.      * turn. As of 9.2, it is only useful for GPS and Glonass.
  186.      * </p>
  187.      * @return secured Sun elevation angle
  188.      * @see #getBeta()
  189.      * @see #getBetaDS()
  190.      */
  191.     public T getSecuredBeta() {
  192.         return FastMath.abs(beta.getReal()) < BETA_SIGN_CHANGE_PROTECTION ?
  193.                beta.taylor(timeSinceTurnStart(getDate()).negate()) :
  194.                getBeta();
  195.     }

  197.     /** Get the nominal yaw.
  198.      * @return nominal yaw
  199.      */
  200.     public TimeStampedFieldAngularCoordinates<T> getNominalYaw() {
  201.         return nominalYaw;
  202.     }

  204.     /** Compute nominal yaw angle.
  205.      * @return nominal yaw angle
  206.      */
  207.     public T yawAngle() {
  208.         final FieldVector3D<T> xSat = nominalYaw.getRotation().revert().applyTo(Vector3D.PLUS_I);
  209.         return FastMath.copySign(FieldVector3D.angle(svPV.getVelocity(), xSat), -beta.getReal());
  210.     }

  212.     /** Compute nominal yaw angle.
  213.      * @return nominal yaw angle
  214.      */
  215.     public FieldDerivativeStructure<T> yawAngleDS() {
  216.         final FieldVector3D<FieldDerivativeStructure<T>> xSat    = nominalYawDS.revert().applyTo(Vector3D.PLUS_I);
  217.         final FDSFactory<T>                              factory = xSat.getX().getFactory();
  218.         final FieldVector3D<T>                           v       = svPV.getVelocity();
  219.         final FieldVector3D<FieldDerivativeStructure<T>> vDS     = new FieldVector3D<>(factory.constant(v.getX()),
  220.                                                                                        factory.constant(v.getY()),
  221.                                                                                        factory.constant(v.getZ()));
  222.         return FieldVector3D.angle(vDS, xSat).copySign(beta.getValue().negate());
  223.     }

  225.     /** Set up the midnight/noon turn region.
  226.      * @param cosNight limit cosine for the midnight turn
  227.      * @param cosNoon limit cosine for the noon turn
  228.      * @return true if spacecraft is in the midnight/noon turn region
  229.      */
  230.     public boolean setUpTurnRegion(final double cosNight, final double cosNoon) {
  231.         this.cNight = cosNight;
  232.         this.cNoon  = cosNoon;
  233.         if (svbCos.getValue().getReal() < cNight) {
  234.             // in eclipse turn mode
  235.             final FieldDerivativeStructure<T> absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos().negate().add(FastMath.PI));
  236.             delta = absDelta.copySign(absDelta.getPartialDerivative(1).negate());
  237.             return true;
  238.         } else if (svbCos.getValue().getReal() > cNoon) {
  239.             // in noon turn mode
  240.             final FieldDerivativeStructure<T> absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos());
  241.             delta = absDelta.copySign(absDelta.getPartialDerivative(1).negate());
  242.             return true;
  243.         } else {
  244.             return false;
  245.         }
  246.     }

  248.     /** Get the relative orbit angle to turn center.
  249.      * @return relative orbit angle to turn center
  250.      */
  251.     public T getDelta() {
  252.         return delta.getValue();
  253.     }

  255.     /** Get the relative orbit angle to turn center.
  256.      * @return relative orbit angle to turn center
  257.      */
  258.     public FieldDerivativeStructure<T> getDeltaDS() {
  259.         return delta;
  260.     }

  262.     /** Check if spacecraft is in the half orbit closest to Sun.
  263.      * @return true if spacecraft is in the half orbit closest to Sun
  264.      */
  265.     public boolean inSunSide() {
  266.         return svbCos.getValue().getReal() > 0;
  267.     }

  269.     /** Get yaw at turn start.
  270.      * @param sunBeta Sun elevation above orbital plane
  271.      * (it <em>may</em> be different from {@link #getBeta()} in
  272.      * some special cases)
  273.      * @return yaw at turn start
  274.      */
  275.     public T getYawStart(final T sunBeta) {
  276.         return computePhi(sunBeta, FastMath.copySign(halfSpan, svbCos.getValue()));
  277.     }

  279.     /** Get yaw at turn end.
  280.      * @param sunBeta Sun elevation above orbital plane
  281.      * (it <em>may</em> be different from {@link #getBeta()} in
  282.      * some special cases)
  283.      * @return yaw at turn end
  284.      */
  285.     public T getYawEnd(final T sunBeta) {
  286.         return computePhi(sunBeta, FastMath.copySign(halfSpan, svbCos.getValue().negate()));
  287.     }

  289.     /** Compute yaw rate.
  290.      * @param sunBeta Sun elevation above orbital plane
  291.      * (it <em>may</em> be different from {@link #getBeta()} in
  292.      * some special cases)
  293.      * @return yaw rate
  294.      */
  295.     public T yawRate(final T sunBeta) {
  296.         return getYawEnd(sunBeta).subtract(getYawStart(sunBeta)).divide(getTurnDuration());
  297.     }

  299.     /** Get the spacecraft angular velocity.
  300.      * @return spacecraft angular velocity
  301.      */
  302.     public T getMuRate() {
  303.         return muRate;
  304.     }

  306.     /** Project a spacecraft/Sun angle into orbital plane.
  307.      * <p>
  308.      * This method is intended to find the limits of the noon and midnight
  309.      * turns in orbital plane. The return angle is signed, depending on the
  310.      * spacecraft being before or after turn middle point.
  311.      * </p>
  312.      * @param angle spacecraft/Sun angle (or spacecraft/opposite-of-Sun)
  313.      * @return angle projected into orbital plane, always positive
  314.      */
  315.     private FieldDerivativeStructure<T> inOrbitPlaneAbsoluteAngle(final FieldDerivativeStructure<T> angle) {
  316.         return FastMath.acos(FastMath.cos(angle).divide(FastMath.cos(beta)));
  317.     }

  319.     /** Project a spacecraft/Sun angle into orbital plane.
  320.      * <p>
  321.      * This method is intended to find the limits of the noon and midnight
  322.      * turns in orbital plane. The return angle is always positive. The
  323.      * correct sign to apply depends on the spacecraft being before or
  324.      * after turn middle point.
  325.      * </p>
  326.      * @param angle spacecraft/Sun angle (or spacecraft/opposite-of-Sun)
  327.      * @return angle projected into orbital plane, always positive
  328.      */
  329.     public T inOrbitPlaneAbsoluteAngle(final T angle) {
  330.         return FastMath.acos(FastMath.cos(angle).divide(FastMath.cos(beta.getReal())));
  331.     }

  333.     /** Compute yaw.
  334.      * @param sunBeta Sun elevation above orbital plane
  335.      * (it <em>may</em> be different from {@link #getBeta()} in
  336.      * some special cases)
  337.      * @param inOrbitPlaneAngle in orbit angle between spacecraft
  338.      * and Sun (or opposite of Sun) projection
  339.      * @return yaw angle
  340.      */
  341.     public T computePhi(final T sunBeta, final T inOrbitPlaneAngle) {
  342.         return FastMath.atan2(FastMath.tan(sunBeta).negate(), FastMath.sin(inOrbitPlaneAngle));
  343.     }

  345.     /** Set turn half span and compute corresponding turn time range.
  346.      * @param halfSpan half span of the turn region, as an angle in orbit plane
  347.      */
  348.     public void setHalfSpan(final T halfSpan) {
  349.         this.halfSpan  = halfSpan;
  350.         this.turnStart = svPV.getDate().shiftedBy(delta.getValue().subtract(halfSpan).divide(muRate));
  351.         this.turnEnd   = svPV.getDate().shiftedBy(delta.getValue().add(halfSpan).divide(muRate));
  352.     }

  354.     /** Check if a date is within range.
  355.      * @param date date to check
  356.      * @param endMargin margin in seconds after turn end
  357.      * @return true if date is within range extended by end margin
  358.      */
  359.     public boolean inTurnTimeRange(final FieldAbsoluteDate<T> date, final double endMargin) {
  360.         return date.durationFrom(turnStart).getReal() > 0 &&
  361.                date.durationFrom(turnEnd).getReal()   < endMargin;
  362.     }

  364.     /** Get turn duration.
  365.      * @return turn duration
  366.      */
  367.     public T getTurnDuration() {
  368.         return halfSpan.multiply(2).divide(muRate);
  369.     }

  371.     /** Get elapsed time since turn start.
  372.      * @param date date to check
  373.      * @return elapsed time from turn start to specified date
  374.      */
  375.     public T timeSinceTurnStart(final FieldAbsoluteDate<T> date) {
  376.         return date.durationFrom(turnStart);
  377.     }

  379.     /** Generate an attitude with turn-corrected yaw.
  380.      * @param yaw yaw value to apply
  381.      * @param yawDot yaw first time derivative
  382.      * @return attitude with specified yaw
  383.      */
  384.     public TimeStampedFieldAngularCoordinates<T> turnCorrectedAttitude(final T yaw, final T yawDot) {
  385.         return turnCorrectedAttitude(beta.getFactory().build(yaw, yawDot, yaw.getField().getZero()));
  386.     }

  388.     /** Generate an attitude with turn-corrected yaw.
  389.      * @param yaw yaw value to apply
  390.      * @return attitude with specified yaw
  391.      */
  392.     public TimeStampedFieldAngularCoordinates<T> turnCorrectedAttitude(final FieldDerivativeStructure<T> yaw) {

  394.         // compute a linear yaw correction model
  395.         final FieldDerivativeStructure<T> nominalAngle   = yawAngleDS();
  396.         final TimeStampedFieldAngularCoordinates<T> correction =
  397.                         new TimeStampedFieldAngularCoordinates<>(nominalYaw.getDate(),
  398.                                                                  new FieldRotation<>(FieldVector3D.getPlusK(nominalAngle.getField()),
  399.                                                                                      yaw.subtract(nominalAngle),
  400.                                                                                      RotationConvention.VECTOR_OPERATOR));

  402.         // combine the two parts of the attitude
  403.         return correction.addOffset(getNominalYaw());

  405.     }

  407.     /** Compute Orbit Normal (ON) yaw.
  408.      * @return Orbit Normal yaw, using inertial frame as reference
  409.      */
  410.     public TimeStampedFieldAngularCoordinates<T> orbitNormalYaw() {
  411.         final FieldTransform<T> t = LOFType.VVLH.transformFromInertial(svPV.getDate(), svPV);
  412.         return new TimeStampedFieldAngularCoordinates<>(svPV.getDate(),
  413.                                                         t.getRotation(),
  414.                                                         t.getRotationRate(),
  415.                                                         t.getRotationAcceleration());
  416.     }

  418. }
Created with JaCoCo 0.7.9.201702052155