CR3BPDifferentialCorrection.java
/* Copyright 2002-2020 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.orbits;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.ode.events.Action;
import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
import org.hipparchus.util.FastMath;
import org.orekit.annotation.DefaultDataContext;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.bodies.CR3BPSystem;
import org.orekit.data.DataContext;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.events.EventDetector;
import org.orekit.propagation.events.HaloXZPlaneCrossingDetector;
import org.orekit.propagation.events.handlers.EventHandler;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.propagation.numerical.cr3bp.CR3BPForceModel;
import org.orekit.propagation.numerical.cr3bp.STMEquations;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeScale;
import org.orekit.utils.AbsolutePVCoordinates;
import org.orekit.utils.PVCoordinates;
/**
* Class implementing the differential correction method for Halo or Lyapunov
* Orbits. It is not a simple differential correction, it uses higher order
* terms to be more accurate and meet orbits requirements.
* @see "Three-dimensional, periodic, Halo Orbits by Kathleen Connor Howell, Stanford University"
* @author Vincent Mouraux
* @since 10.2
*/
public class CR3BPDifferentialCorrection {
/** Boolean return true if the propagated trajectory crosses the plane. */
private boolean cross;
/** first guess PVCoordinates of the point to start differential correction. */
private final PVCoordinates firstGuess;
/** CR3BP System considered. */
private final CR3BPSystem syst;
/** orbitalPeriodApprox Orbital Period of the firstGuess. */
private final double orbitalPeriodApprox;
/** orbitalPeriod Orbital Period of the required orbit. */
private double orbitalPeriod;
/** Propagator. */
private final NumericalPropagator propagator;
/** UTC time scale. */
private final TimeScale utc;
/** Simple Constructor.
* <p> Standard constructor using DormandPrince853 integrator for the differential correction </p>
* @param firstguess first guess PVCoordinates of the point to start differential correction
* @param syst CR3BP System considered
* @param orbitalPeriod Orbital Period of the required orbit
*/
@DefaultDataContext
public CR3BPDifferentialCorrection(final PVCoordinates firstguess,
final CR3BPSystem syst, final double orbitalPeriod) {
this(firstguess, syst, orbitalPeriod,
Propagator.getDefaultLaw(DataContext.getDefault().getFrames()),
DataContext.getDefault().getTimeScales().getUTC());
}
/** Simple Constructor.
* <p> Standard constructor using DormandPrince853 integrator for the differential correction </p>
* @param firstguess first guess PVCoordinates of the point to start differential correction
* @param syst CR3BP System considered
* @param orbitalPeriod Orbital Period of the required orbit
* @param attitudeProvider the attitude law for the numerocal propagator
* @param utc UTC time scale
*/
public CR3BPDifferentialCorrection(final PVCoordinates firstguess,
final CR3BPSystem syst,
final double orbitalPeriod,
final AttitudeProvider attitudeProvider,
final TimeScale utc) {
this.firstGuess = firstguess;
this.syst = syst;
this.orbitalPeriodApprox = orbitalPeriod;
this.utc = utc;
// Adaptive stepsize boundaries
final double minStep = 1E-12;
final double maxstep = 0.001;
// Integrator tolerances
final double positionTolerance = 1E-5;
final double velocityTolerance = 1E-5;
final double massTolerance = 1.0e-6;
final double[] vecAbsoluteTolerances = {positionTolerance, positionTolerance, positionTolerance, velocityTolerance, velocityTolerance, velocityTolerance, massTolerance};
final double[] vecRelativeTolerances = new double[vecAbsoluteTolerances.length];
// Integrator definition
final AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(minStep, maxstep,
vecAbsoluteTolerances,
vecRelativeTolerances);
// Propagator definition
this.propagator = new NumericalPropagator(integrator, attitudeProvider);
}
/**
* Return the real starting PVCoordinates on the Libration orbit type
* after differential correction from a first guess.
* @param type libration orbit type
* @return pv Position-Velocity of the starting point on the Halo Orbit
*/
public PVCoordinates compute(final LibrationOrbitType type) {
// Event detector settings
final double maxcheck = 10;
final double threshold = 1E-10;
// Event detector definition
final EventDetector XZPlaneCrossing =
new HaloXZPlaneCrossingDetector(maxcheck, threshold).withHandler(new PlaneCrossingHandler());
// Additional equations set in order to compute the State Transition Matrix along the propagation
final STMEquations stm = new STMEquations(syst);
// CR3BP has no defined orbit type
propagator.setOrbitType(null);
// CR3BP has central Attraction
propagator.setIgnoreCentralAttraction(true);
// Add CR3BP Force Model to the propagator
propagator.addForceModel(new CR3BPForceModel(syst));
// Add previously set additional equations to the propagator
propagator.addAdditionalEquations(stm);
// Add previously set event detector to the propagator
propagator.addEventDetector(XZPlaneCrossing);
return type == LibrationOrbitType.HALO ? computeHalo(stm) : computeLyapunov(stm);
}
/** Return the real starting PVCoordinates on the Halo orbit after
* differential correction from a first guess.
* @param stm additional equations
* @return pv Position-Velocity of the starting point on the Halo Orbit
*/
private PVCoordinates computeHalo(final STMEquations stm) {
// number of iteration
double iHalo = 0;
// Time settings (this date has no effect on the result, this is only for code structure purpose)
final AbsoluteDate startDateHalo = new AbsoluteDate(1996, 06, 25, 0, 0, 00.000, utc);
// Initializing PVCoordinates with first guess
PVCoordinates pvHalo = firstGuess;
// Start a new differentially corrected propagation until it converges to a Halo Orbit
do {
// SpacecraftState initialization
final AbsolutePVCoordinates initialAbsPVHalo = new AbsolutePVCoordinates(syst.getRotatingFrame(), startDateHalo, pvHalo);
final SpacecraftState initialStateHalo = new SpacecraftState(initialAbsPVHalo);
// Additional equations initialization
final SpacecraftState augmentedInitialStateHalo = stm.setInitialPhi(initialStateHalo);
// boolean changed to true by crossing XZ plane during propagation. Has to be true for the differential correction to converge
cross = false;
// Propagator initialization
propagator.setInitialState(augmentedInitialStateHalo);
// Propagate until trajectory crosses XZ Plane
final SpacecraftState finalStateHalo =
propagator.propagate(startDateHalo.shiftedBy(orbitalPeriodApprox));
// Stops computation if trajectory did not cross XZ Plane after one full orbital period
if (cross == false) {
throw new OrekitException(OrekitMessages.TRAJECTORY_NOT_CROSSING_XZPLANE);
}
// Get State Transition Matrix phi
final RealMatrix phiHalo = stm.getStateTransitionMatrix(finalStateHalo);
// Gap from desired X and Z axis velocity value ()
final double dvxf = -finalStateHalo.getPVCoordinates().getVelocity().getX();
final double dvzf = -finalStateHalo.getPVCoordinates().getVelocity().getZ();
orbitalPeriod = 2 * finalStateHalo.getDate().durationFrom(startDateHalo);
if (FastMath.abs(dvxf) > 1E-8 || FastMath.abs(dvzf) > 1E-8) {
// Y axis velocity
final double vy =
finalStateHalo.getPVCoordinates().getVelocity().getY();
// Spacecraft acceleration
final Vector3D acc = finalStateHalo.getPVCoordinates().getAcceleration();
final double accx = acc.getX();
final double accz = acc.getZ();
// Compute A coefficients
final double a11 = phiHalo.getEntry(3, 0) - accx * phiHalo.getEntry(1, 0) / vy;
final double a12 = phiHalo.getEntry(3, 4) - accx * phiHalo.getEntry(1, 4) / vy;
final double a21 = phiHalo.getEntry(5, 0) - accz * phiHalo.getEntry(1, 0) / vy;
final double a22 = phiHalo.getEntry(5, 4) - accz * phiHalo.getEntry(1, 4) / vy;
// A determinant used for matrix inversion
final double aDet = a11 * a22 - a21 * a12;
// Correction to apply to initial conditions
final double deltax0 = (a22 * dvxf - a12 * dvzf) / aDet;
final double deltavy0 = (-a21 * dvxf + a11 * dvzf) / aDet;
// Computation of the corrected initial PVCoordinates
final double newx = pvHalo.getPosition().getX() + deltax0;
final double newvy = pvHalo.getVelocity().getY() + deltavy0;
pvHalo = new PVCoordinates(new Vector3D(newx,
pvHalo.getPosition().getY(),
pvHalo.getPosition().getZ()),
new Vector3D(pvHalo.getVelocity().getX(),
newvy,
pvHalo.getVelocity().getZ()));
++iHalo;
} else {
break;
}
} while (iHalo < 30); // Converge within 1E-8 tolerance and under 5 iterations
// Return
return pvHalo;
}
/** Return the real starting PVCoordinates on the Lyapunov orbit after
* differential correction from a first guess.
* @param stm additional equations
* @return pv Position-Velocity of the starting point on the Lyapunov Orbit
*/
public PVCoordinates computeLyapunov(final STMEquations stm) {
// number of iteration
double iLyapunov = 0;
// Time settings (this date has no effect on the result, this is only for code structure purpose)
final AbsoluteDate startDateLyapunov = new AbsoluteDate(1996, 06, 25, 0, 0, 00.000, utc);
// Initializing PVCoordinates with first guess
PVCoordinates pvLyapunov = firstGuess;
// Start a new differentially corrected propagation until it converges to a Halo Orbit
do {
// SpacecraftState initialization
final AbsolutePVCoordinates initialAbsPVLyapunov = new AbsolutePVCoordinates(syst.getRotatingFrame(), startDateLyapunov, pvLyapunov);
final SpacecraftState initialStateLyapunov = new SpacecraftState(initialAbsPVLyapunov);
// Additional equations initialization
final SpacecraftState augmentedInitialStateLyapunov = stm.setInitialPhi(initialStateLyapunov);
// boolean changed to true by crossing XZ plane during propagation. Has to be true for the differential correction to converge
cross = false;
// Propagator initialization
propagator.setInitialState(augmentedInitialStateLyapunov);
// Propagate until trajectory crosses XZ Plane
final SpacecraftState finalStateLyapunov =
propagator.propagate(startDateLyapunov.shiftedBy(orbitalPeriodApprox));
// Stops computation if trajectory did not cross XZ Plane after one full orbital period
if (cross == false) {
throw new OrekitException(OrekitMessages.TRAJECTORY_NOT_CROSSING_XZPLANE);
}
// Get State Transition Matrix phi
final RealMatrix phi = stm.getStateTransitionMatrix(finalStateLyapunov);
// Gap from desired y position and x velocity value ()
final double dvxf = -finalStateLyapunov.getPVCoordinates().getVelocity().getX();
orbitalPeriod = 2 * finalStateLyapunov.getDate().durationFrom(startDateLyapunov);
if (FastMath.abs(dvxf) > 1E-14) {
// Y axis velocity
final double vy = finalStateLyapunov.getPVCoordinates().getVelocity().getY();
// Spacecraft acceleration
final double accy = finalStateLyapunov.getPVCoordinates().getAcceleration().getY();
// Compute A coefficients
final double deltavy0 = dvxf / (phi.getEntry(3, 4) - accy * phi.getEntry(1, 4) / vy);
// Computation of the corrected initial PVCoordinates
final double newvy = pvLyapunov.getVelocity().getY() + deltavy0;
pvLyapunov = new PVCoordinates(new Vector3D(pvLyapunov.getPosition().getX(),
pvLyapunov.getPosition().getY(),
0),
new Vector3D(pvLyapunov.getVelocity().getX(),
newvy,
0));
++iLyapunov;
} else {
break;
}
} while (iLyapunov < 30); // Converge within 1E-8 tolerance and under 5 iterations
// Return
return pvLyapunov;
}
/** Get the orbital period of the required orbit.
* @return the orbitalPeriod
*/
public double getOrbitalPeriod() {
return orbitalPeriod;
}
/**
* Static class for event detection.
*/
private class PlaneCrossingHandler implements EventHandler<HaloXZPlaneCrossingDetector> {
/** {@inheritDoc} */
public Action eventOccurred(final SpacecraftState s,
final HaloXZPlaneCrossingDetector detector,
final boolean increasing) {
cross = true;
return Action.STOP;
}
}
}