PositionAngleDetector.java

  1. /* Copyright 2002-2019 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.propagation.events;

  19. import java.util.function.Function;

  21. import org.hipparchus.analysis.UnivariateFunction;
  22. import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
  23. import org.hipparchus.util.FastMath;
  24. import org.hipparchus.util.MathUtils;
  25. import org.orekit.errors.OrekitIllegalArgumentException;
  26. import org.orekit.errors.OrekitMessages;
  27. import org.orekit.orbits.CircularOrbit;
  28. import org.orekit.orbits.EquinoctialOrbit;
  29. import org.orekit.orbits.KeplerianOrbit;
  30. import org.orekit.orbits.Orbit;
  31. import org.orekit.orbits.OrbitType;
  32. import org.orekit.orbits.PositionAngle;
  33. import org.orekit.propagation.SpacecraftState;
  34. import org.orekit.propagation.events.handlers.EventHandler;
  35. import org.orekit.propagation.events.handlers.StopOnIncreasing;
  36. import org.orekit.time.AbsoluteDate;
  37. import org.orekit.utils.TimeSpanMap;

  39. /** Detector for in-orbit position angle.
  40.  * <p>
  41.  * The detector is based on anomaly for {@link OrbitType#KEPLERIAN Keplerian}
  42.  * orbits, latitude argument for {@link OrbitType#CIRCULAR circular} orbits,
  43.  * or longitude argument for {@link OrbitType#EQUINOCTIAL equinoctial} orbits.
  44.  * It does not support {@link OrbitType#CARTESIAN Cartesian} orbits. The
  45.  * angles can be either {@link PositionAngle#TRUE true}, {link {@link PositionAngle#MEAN
  46.  * mean} or {@link PositionAngle#ECCENTRIC eccentric} angles.
  47.  * </p>
  48.  * @author Luc Maisonobe
  49.  * @since 7.1
  50.  */
  51. public class PositionAngleDetector extends AbstractDetector<PositionAngleDetector> {

  53.     /** Serializable UID. */
  54.     private static final long serialVersionUID = 20180919L;

  56.     /** Orbit type defining the angle type. */
  57.     private final OrbitType orbitType;

  59.     /** Type of position angle. */
  60.     private final PositionAngle positionAngle;

  62.     /** Fixed angle to be crossed. */
  63.     private final double angle;

  65.     /** Position angle extraction function. */
  66.     private final Function<Orbit, Double> positionAngleExtractor;

  68.     /** Estimators for the offset angle, taking care of 2π wrapping and g function continuity. */
  69.     private TimeSpanMap<OffsetEstimator> offsetEstimators;

  71.     /** Build a new detector.
  72.      * <p>The new instance uses default values for maximal checking interval
  73.      * ({@link #DEFAULT_MAXCHECK}) and convergence threshold ({@link
  74.      * #DEFAULT_THRESHOLD}).</p>
  75.      * @param orbitType orbit type defining the angle type
  76.      * @param positionAngle type of position angle
  77.      * @param angle fixed angle to be crossed
  78.      * @exception OrekitIllegalArgumentException if orbit type is {@link OrbitType#CARTESIAN}
  79.      */
  80.     public PositionAngleDetector(final OrbitType orbitType, final PositionAngle positionAngle,
  81.                                  final double angle)
  82.         throws OrekitIllegalArgumentException {
  83.         this(DEFAULT_MAXCHECK, DEFAULT_THRESHOLD, orbitType, positionAngle, angle);
  84.     }

  86.     /** Build a detector.
  87.      * @param maxCheck maximal checking interval (s)
  88.      * @param threshold convergence threshold (s)
  89.      * @param orbitType orbit type defining the angle type
  90.      * @param positionAngle type of position angle
  91.      * @param angle fixed angle to be crossed
  92.      * @exception OrekitIllegalArgumentException if orbit type is {@link OrbitType#CARTESIAN}
  93.      */
  94.     public PositionAngleDetector(final double maxCheck, final double threshold,
  95.                                  final OrbitType orbitType, final PositionAngle positionAngle,
  96.                                  final double angle)
  97.         throws OrekitIllegalArgumentException {
  98.         this(maxCheck, threshold, DEFAULT_MAX_ITER, new StopOnIncreasing<PositionAngleDetector>(),
  99.              orbitType, positionAngle, angle);
  100.     }

  102.     /** Private constructor with full parameters.
  103.      * <p>
  104.      * This constructor is private as users are expected to use the builder
  105.      * API with the various {@code withXxx()} methods to set up the instance
  106.      * in a readable manner without using a huge amount of parameters.
  107.      * </p>
  108.      * @param maxCheck maximum checking interval (s)
  109.      * @param threshold convergence threshold (s)
  110.      * @param maxIter maximum number of iterations in the event time search
  111.      * @param handler event handler to call at event occurrences
  112.      * @param orbitType orbit type defining the angle type
  113.      * @param positionAngle type of position angle
  114.      * @param angle fixed angle to be crossed
  115.      * @exception OrekitIllegalArgumentException if orbit type is {@link OrbitType#CARTESIAN}
  116.      */
  117.     private PositionAngleDetector(final double maxCheck, final double threshold,
  118.                                      final int maxIter, final EventHandler<? super PositionAngleDetector> handler,
  119.                                      final OrbitType orbitType, final PositionAngle positionAngle,
  120.                                      final double angle)
  121.         throws OrekitIllegalArgumentException {

  123.         super(maxCheck, threshold, maxIter, handler);

  125.         this.orbitType        = orbitType;
  126.         this.positionAngle    = positionAngle;
  127.         this.angle            = angle;
  128.         this.offsetEstimators = null;

  130.         switch (orbitType) {
  131.             case KEPLERIAN:
  132.                 positionAngleExtractor = o -> ((KeplerianOrbit) orbitType.convertType(o)).getAnomaly(positionAngle);
  133.                 break;
  134.             case CIRCULAR:
  135.                 positionAngleExtractor = o -> ((CircularOrbit) orbitType.convertType(o)).getAlpha(positionAngle);
  136.                 break;
  137.             case EQUINOCTIAL:
  138.                 positionAngleExtractor = o -> ((EquinoctialOrbit) orbitType.convertType(o)).getL(positionAngle);
  139.                 break;
  140.             default:
  141.                 final String sep = ", ";
  142.                 throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_TYPE_NOT_ALLOWED,
  143.                                                          orbitType,
  144.                                                          OrbitType.KEPLERIAN   + sep +
  145.                                                          OrbitType.CIRCULAR    + sep +
  146.                                                          OrbitType.EQUINOCTIAL);
  147.         }

  149.     }

  151.     /** {@inheritDoc} */
  152.     @Override
  153.     protected PositionAngleDetector create(final double newMaxCheck, final double newThreshold,
  154.                                               final int newMaxIter,
  155.                                               final EventHandler<? super PositionAngleDetector> newHandler) {
  156.         return new PositionAngleDetector(newMaxCheck, newThreshold, newMaxIter, newHandler,
  157.                                          orbitType, positionAngle, angle);
  158.     }

  160.     /** Get the orbit type defining the angle type.
  161.      * @return orbit type defining the angle type
  162.      */
  163.     public OrbitType getOrbitType() {
  164.         return orbitType;
  165.     }

  167.     /** Get the type of position angle.
  168.      * @return type of position angle
  169.      */
  170.     public PositionAngle getPositionAngle() {
  171.         return positionAngle;
  172.     }

  174.     /** Get the fixed angle to be crossed (radians).
  175.      * @return fixed angle to be crossed (radians)
  176.      */
  177.     public double getAngle() {
  178.         return angle;
  179.     }

  181.     /** {@inheritDoc} */
  182.     public void init(final SpacecraftState s0, final AbsoluteDate t) {
  183.         super.init(s0, t);
  184.         offsetEstimators = new TimeSpanMap<>(new OffsetEstimator(s0.getOrbit(), +1.0));
  185.     }

  187.     /** Compute the value of the detection function.
  188.      * <p>
  189.      * The value is the angle difference between the spacecraft and the fixed
  190.      * angle to be crossed, with some sign tweaks to ensure continuity.
  191.      * These tweaks imply the {@code increasing} flag in events detection becomes
  192.      * irrelevant here! As an example, the angle always increase in a Keplerian
  193.      * orbit, but this g function will increase and decrease so it
  194.      * will cross the zero value once per orbit, in increasing and decreasing
  195.      * directions on alternate orbits..
  196.      * </p>
  197.      * @param s the current state information: date, kinematics, attitude
  198.      * @return angle difference between the spacecraft and the fixed
  199.      * angle, with some sign tweaks to ensure continuity
  200.      */
  201.     public double g(final SpacecraftState s) {

  203.         final Orbit orbit = s.getOrbit();

  205.         // angle difference
  206.         OffsetEstimator estimator = offsetEstimators.get(s.getDate());
  207.         double          delta     = estimator.delta(orbit);

  209.         // we use a value greater than π for handover in order to avoid
  210.         // several switches to be estimated as the calling propagator
  211.         // and Orbit.shiftedBy have different accuracy. It is sufficient
  212.         // to have a handover roughly opposite to the detected position angle
  213.         while (FastMath.abs(delta) >= 3.5) {
  214.             // we are too far away from the current estimator, we need to set up a new one
  215.             // ensuring that we do have a crossing event in the current orbit
  216.             // and we ensure sign continuity with the current estimator

  218.             // find when the previous estimator becomes invalid
  219.             final AbsoluteDate handover = estimator.dateForOffset(FastMath.copySign(FastMath.PI, delta), orbit);

  221.             // perform handover to a new estimator at this date
  222.             estimator = new OffsetEstimator(orbit, delta);
  223.             delta     = estimator.delta(orbit);
  224.             if (isForward()) {
  225.                 offsetEstimators.addValidAfter(estimator, handover.getDate());
  226.             } else {
  227.                 offsetEstimators.addValidBefore(estimator, handover.getDate());
  228.             }

  230.         }

  232.         return delta;

  234.     }

  236.     /** Local class for estimating offset angle, handling 2π wrap-up and sign continuity. */
  237.     private class OffsetEstimator {

  239.         /** Target angle. */
  240.         private final double target;

  242.         /** Sign correction to offset. */
  243.         private final double sign;

  245.         /** Reference angle. */
  246.         private final double r0;

  248.         /** Slope of the linearized model. */
  249.         private final double r1;

  251.         /** Reference date. */
  252.         private final AbsoluteDate t0;

  254.         /** Simple constructor.
  255.          * @param orbit current orbit
  256.          * @param currentSign desired sign of the offset at current orbit time (magnitude is ignored)
  257.          */
  258.         OffsetEstimator(final Orbit orbit, final double currentSign) {
  259.             r0     = positionAngleExtractor.apply(orbit);
  260.             target = MathUtils.normalizeAngle(angle, r0);
  261.             sign   = FastMath.copySign(1.0, (r0 - target) * currentSign);
  262.             r1     = orbit.getKeplerianMeanMotion();
  263.             t0     = orbit.getDate();
  264.         }

  266.         /** Compute offset from reference angle.
  267.          * @param orbit current orbit
  268.          * @return offset between current angle and reference angle
  269.          */
  270.         public double delta(final Orbit orbit) {
  271.             final double rawAngle        = positionAngleExtractor.apply(orbit);
  272.             final double linearReference = r0 + r1 * orbit.getDate().durationFrom(t0);
  273.             final double linearizedAngle = MathUtils.normalizeAngle(rawAngle, linearReference);
  274.             return sign * (linearizedAngle - target);
  275.         }

  277.         /** Find date at which offset reaches specified value.
  278.          * <p>
  279.          * This computation is an approximation because it relies on
  280.          * {@link Orbit#shiftedBy(double)} only.
  281.          * </p>
  282.          * @param offset target value for offset angle
  283.          * @param orbit current orbit
  284.          * @return approximate date at which offset reached specified value
  285.          */
  286.         public AbsoluteDate dateForOffset(final double offset, final Orbit orbit) {

  288.             // bracket the search
  289.             final double period = orbit.getKeplerianPeriod();
  290.             final double delta0 = delta(orbit);
  291.             final double searchInf;
  292.             final double searchSup;
  293.             if ((delta0 - offset) * sign >= 0) {
  294.                 // the date is before current orbit
  295.                 searchInf = -period;
  296.                 searchSup = 0;
  297.             } else {
  298.                 // the date is after current orbit
  299.                 searchInf = 0;
  300.                 searchSup = +period;
  301.             }

  303.             // find the date as an offset from current orbit
  304.             final BracketingNthOrderBrentSolver solver = new BracketingNthOrderBrentSolver(getThreshold(), 5);
  305.             final UnivariateFunction            f      = dt -> delta(orbit.shiftedBy(dt)) - offset;
  306.             final double                        root   = solver.solve(getMaxIterationCount(), f, searchInf, searchSup);

  308.             return orbit.getDate().shiftedBy(root);

  310.         }

  312.     }

  314. }
Created with JaCoCo 0.8.2.201808211720