CR3BPDifferentialCorrection.java

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

  18. import org.hipparchus.geometry.euclidean.threed.Rotation;
  19. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  20. import org.hipparchus.linear.RealMatrix;
  21. import org.hipparchus.ode.events.Action;
  22. import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
  23. import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
  24. import org.hipparchus.util.FastMath;
  25. import org.orekit.attitudes.FrameAlignedProvider;
  26. import org.orekit.bodies.CR3BPSystem;
  27. import org.orekit.errors.OrekitException;
  28. import org.orekit.errors.OrekitMessages;
  29. import org.orekit.propagation.SpacecraftState;
  30. import org.orekit.propagation.events.HaloXZPlaneCrossingDetector;
  31. import org.orekit.propagation.numerical.NumericalPropagator;
  32. import org.orekit.propagation.numerical.cr3bp.CR3BPForceModel;
  33. import org.orekit.propagation.numerical.cr3bp.STMEquations;
  34. import org.orekit.time.AbsoluteDate;
  35. import org.orekit.utils.AbsolutePVCoordinates;
  36. import org.orekit.utils.PVCoordinates;


  37. /**
  38.  * Class implementing the differential correction method for Halo or Lyapunov
  39.  * Orbits. It is not a simple differential correction, it uses higher order
  40.  * terms to be more accurate and meet orbits requirements.
  41.  * @see "Three-dimensional, periodic, Halo Orbits by Kathleen Connor Howell, Stanford University"
  42.  * @author Vincent Mouraux
  43.  * @since 10.2
  44.  */
  45. public class CR3BPDifferentialCorrection {

  46.     /** Maximum number of iterations. */
  47.     private static final int MAX_ITER = 30;

  48.     /** Max check interval for crossing plane. */
  49.     private static final double CROSSING_MAX_CHECK = 3600.0;

  50.     /** Convergence tolerance for plane crossing time. */
  51.     private static final double CROSSING_TOLERANCE = 1.0e-10;

  52.     /** Arbitrary start date. */
  53.     private static final AbsoluteDate START_DATE = AbsoluteDate.ARBITRARY_EPOCH;

  54.     /** Boolean return true if the propagated trajectory crosses the plane. */
  55.     private boolean cross;

  56.     /** first guess PVCoordinates of the point to start differential correction. */
  57.     private final PVCoordinates firstGuess;

  58.     /** CR3BP System considered. */
  59.     private final CR3BPSystem syst;

  60.     /** orbitalPeriodApprox Orbital Period of the firstGuess. */
  61.     private final double orbitalPeriodApprox;

  62.     /** orbitalPeriod Orbital Period of the required orbit. */
  63.     private double orbitalPeriod;

  64.     /** Simple Constructor.
  65.      * <p> Standard constructor using DormandPrince853 integrator for the differential correction </p>
  66.      * @param firstguess first guess PVCoordinates of the point to start differential correction
  67.      * @param syst CR3BP System considered
  68.      * @param orbitalPeriod Orbital Period of the required orbit
  69.      */
  70.     public CR3BPDifferentialCorrection(final PVCoordinates firstguess,
  71.                                        final CR3BPSystem syst, final double orbitalPeriod) {
  72.         this.firstGuess = firstguess;
  73.         this.syst = syst;
  74.         this.orbitalPeriodApprox = orbitalPeriod;

  75.     }

  76.     /** Build the propagator.
  77.      * @return propagator
  78.      * @since 11.1
  79.      */
  80.     private NumericalPropagator buildPropagator() {

  81.         // Adaptive stepsize boundaries
  82.         final double minStep = 1E-12;
  83.         final double maxstep = 0.001;

  84.         // Integrator tolerances
  85.         final double positionTolerance = 1E-5;
  86.         final double velocityTolerance = 1E-5;
  87.         final double massTolerance = 1.0e-6;
  88.         final double[] vecAbsoluteTolerances = {positionTolerance, positionTolerance, positionTolerance, velocityTolerance, velocityTolerance, velocityTolerance, massTolerance};
  89.         final double[] vecRelativeTolerances = new double[vecAbsoluteTolerances.length];

  90.         // Integrator definition
  91.         final AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(minStep, maxstep,
  92.                                                                                      vecAbsoluteTolerances,
  93.                                                                                      vecRelativeTolerances);

  94.         // Propagator definition
  95.         final NumericalPropagator propagator =
  96.                         new NumericalPropagator(integrator, new FrameAlignedProvider(Rotation.IDENTITY, syst.getRotatingFrame()));

  97.         // CR3BP has no defined orbit type
  98.         propagator.setOrbitType(null);

  99.         // CR3BP has central Attraction
  100.         propagator.setIgnoreCentralAttraction(true);

  101.         // Add CR3BP Force Model to the propagator
  102.         propagator.addForceModel(new CR3BPForceModel(syst));

  103.         // Add event detector for crossing plane
  104.         propagator.addEventDetector(new HaloXZPlaneCrossingDetector(CROSSING_MAX_CHECK, CROSSING_TOLERANCE).
  105.                                     withHandler((state, detector, increasing) -> {
  106.                                         cross = true;
  107.                                         return Action.STOP;
  108.                                     }));

  109.         return propagator;

  110.     }

  111.     /**
  112.      * Return the real starting PVCoordinates on the Libration orbit type
  113.      * after differential correction from a first guess.
  114.      * @param type libration orbit type
  115.      * @return pv Position-Velocity of the starting point on the Halo Orbit
  116.      */
  117.     public PVCoordinates compute(final LibrationOrbitType type) {
  118.         return type == LibrationOrbitType.HALO ? computeHalo() : computeLyapunov();
  119.     }

  120.     /** Return the real starting PVCoordinates on the Halo orbit after differential correction from a first guess.
  121.      * @return pv Position-Velocity of the starting point on the Halo Orbit
  122.      */
  123.     private PVCoordinates computeHalo() {

  124.         // Initializing PVCoordinates with first guess
  125.         PVCoordinates pvHalo = firstGuess;

  126.         // Start a new differentially corrected propagation until it converges to a Halo Orbit
  127.         // Converge within 1E-8 tolerance and under 5 iterations
  128.         for (int iHalo = 0; iHalo < MAX_ITER; ++iHalo) {

  129.             // SpacecraftState initialization
  130.             final AbsolutePVCoordinates initialAbsPVHalo = new AbsolutePVCoordinates(syst.getRotatingFrame(), START_DATE, pvHalo);
  131.             final SpacecraftState       initialStateHalo = new SpacecraftState(initialAbsPVHalo);

  132.             // setup propagator
  133.             final NumericalPropagator propagator = buildPropagator();
  134.             final STMEquations        stm        = new STMEquations(syst);
  135.             propagator.addAdditionalDerivativesProvider(stm);
  136.             propagator.setInitialState(stm.setInitialPhi(initialStateHalo));

  137.             // boolean changed to true by crossing XZ plane during propagation. Has to be true for the differential correction to converge
  138.             cross = false;

  139.             // Propagate until trajectory crosses XZ Plane
  140.             final SpacecraftState finalStateHalo =
  141.                 propagator.propagate(START_DATE.shiftedBy(orbitalPeriodApprox));

  142.             // Stops computation if trajectory did not cross XZ Plane after one full orbital period
  143.             if (cross == false) {
  144.                 throw new OrekitException(OrekitMessages.TRAJECTORY_NOT_CROSSING_XZPLANE);
  145.             }

  146.             // Get State Transition Matrix phi
  147.             final RealMatrix phiHalo = stm.getStateTransitionMatrix(finalStateHalo);

  148.             // Gap from desired X and Z axis velocity value ()
  149.             final double dvxf = -finalStateHalo.getPVCoordinates().getVelocity().getX();
  150.             final double dvzf = -finalStateHalo.getPVCoordinates().getVelocity().getZ();

  151.             orbitalPeriod = 2 * finalStateHalo.getDate().durationFrom(START_DATE);

  152.             if (FastMath.abs(dvxf) <= 1E-8 && FastMath.abs(dvzf) <= 1E-8) {
  153.                 break;
  154.             }

  155.             // Y axis velocity
  156.             final double vy = finalStateHalo.getPVCoordinates().getVelocity().getY();

  157.             // Spacecraft acceleration
  158.             final Vector3D acc  = finalStateHalo.getPVCoordinates().getAcceleration();
  159.             final double   accx = acc.getX();
  160.             final double   accz = acc.getZ();

  161.             // Compute A coefficients
  162.             final double a11 = phiHalo.getEntry(3, 0) - accx * phiHalo.getEntry(1, 0) / vy;
  163.             final double a12 = phiHalo.getEntry(3, 4) - accx * phiHalo.getEntry(1, 4) / vy;
  164.             final double a21 = phiHalo.getEntry(5, 0) - accz * phiHalo.getEntry(1, 0) / vy;
  165.             final double a22 = phiHalo.getEntry(5, 4) - accz * phiHalo.getEntry(1, 4) / vy;

  166.             // A determinant used for matrix inversion
  167.             final double aDet = a11 * a22 - a21 * a12;

  168.             // Correction to apply to initial conditions
  169.             final double deltax0  = (a22 * dvxf - a12 * dvzf) / aDet;
  170.             final double deltavy0 = (-a21 * dvxf + a11 * dvzf) / aDet;

  171.             // Computation of the corrected initial PVCoordinates
  172.             final double newx  = pvHalo.getPosition().getX() + deltax0;
  173.             final double newvy = pvHalo.getVelocity().getY() + deltavy0;

  174.             pvHalo = new PVCoordinates(new Vector3D(newx,
  175.                                                     pvHalo.getPosition().getY(),
  176.                                                     pvHalo.getPosition().getZ()),
  177.                                        new Vector3D(pvHalo.getVelocity().getX(),
  178.                                                     newvy,
  179.                                                     pvHalo.getVelocity().getZ()));
  180.         }

  181.         // Return
  182.         return pvHalo;
  183.     }

  184.     /** Return the real starting PVCoordinates on the Lyapunov orbit after differential correction from a first guess.
  185.      * @return pv Position-Velocity of the starting point on the Lyapunov Orbit
  186.      */
  187.     public PVCoordinates computeLyapunov() {

  188.         // Initializing PVCoordinates with first guess
  189.         PVCoordinates pvLyapunov = firstGuess;

  190.         // Start a new differentially corrected propagation until it converges to a Halo Orbit
  191.         // Converge within 1E-8 tolerance and under 5 iterations
  192.         for (int iLyapunov = 0; iLyapunov < MAX_ITER; ++iLyapunov) {

  193.             // SpacecraftState initialization
  194.             final AbsolutePVCoordinates initialAbsPVLyapunov = new AbsolutePVCoordinates(syst.getRotatingFrame(), START_DATE, pvLyapunov);
  195.             final SpacecraftState       initialStateLyapunov = new SpacecraftState(initialAbsPVLyapunov);

  196.             // setup propagator
  197.             final NumericalPropagator propagator = buildPropagator();
  198.             final STMEquations        stm        = new STMEquations(syst);
  199.             propagator.addAdditionalDerivativesProvider(stm);
  200.             propagator.setInitialState(stm.setInitialPhi(initialStateLyapunov));

  201.             // boolean changed to true by crossing XZ plane during propagation. Has to be true for the differential correction to converge
  202.             cross = false;

  203.             // Propagate until trajectory crosses XZ Plane
  204.             final SpacecraftState finalStateLyapunov =
  205.                 propagator.propagate(START_DATE.shiftedBy(orbitalPeriodApprox));

  206.             // Stops computation if trajectory did not cross XZ Plane after one full orbital period
  207.             if (cross == false) {
  208.                 throw new OrekitException(OrekitMessages.TRAJECTORY_NOT_CROSSING_XZPLANE);
  209.             }

  210.             // Get State Transition Matrix phi
  211.             final RealMatrix phi = stm.getStateTransitionMatrix(finalStateLyapunov);

  212.             // Gap from desired y position and x velocity value ()
  213.             final double dvxf = -finalStateLyapunov.getPVCoordinates().getVelocity().getX();

  214.             orbitalPeriod = 2 * finalStateLyapunov.getDate().durationFrom(START_DATE);

  215.             if (FastMath.abs(dvxf) <= 1E-14) {
  216.                 break;
  217.             }

  218.             // Y axis velocity
  219.             final double vy = finalStateLyapunov.getPVCoordinates().getVelocity().getY();

  220.             // Spacecraft acceleration
  221.             final double accy = finalStateLyapunov.getPVCoordinates().getAcceleration().getY();

  222.             // Compute A coefficients
  223.             final double deltavy0 = dvxf / (phi.getEntry(3, 4) - accy * phi.getEntry(1, 4) / vy);

  224.             // Computation of the corrected initial PVCoordinates
  225.             final double newvy = pvLyapunov.getVelocity().getY() + deltavy0;

  226.             pvLyapunov = new PVCoordinates(new Vector3D(pvLyapunov.getPosition().getX(),
  227.                                                         pvLyapunov.getPosition().getY(),
  228.                                                         0),
  229.                                            new Vector3D(pvLyapunov.getVelocity().getX(),
  230.                                                         newvy,
  231.                                                         0));

  232.         }

  233.         // Return
  234.         return pvLyapunov;
  235.     }

  236.     /** Get the orbital period of the required orbit.
  237.      * @return the orbitalPeriod
  238.      */
  239.     public double getOrbitalPeriod() {
  240.         return orbitalPeriod;
  241.     }

  242. }