LambertSolver.java
/* Copyright 2002-2025 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.control.heuristics.lambert;
import org.hipparchus.analysis.differentiation.Gradient;
import org.hipparchus.exception.MathIllegalStateException;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.geometry.euclidean.twod.Vector2D;
import org.hipparchus.linear.DecompositionSolver;
import org.hipparchus.linear.LUDecomposition;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.util.FastMath;
import org.orekit.orbits.KeplerianMotionCartesianUtility;
import org.orekit.utils.FieldPVCoordinates;
import org.orekit.utils.PVCoordinates;
/**
* Lambert solver, assuming Keplerian motion.
* <p>
* An orbit is determined from two position vectors.
* </p>
* <p>
* References:
* Battin, R.H., An Introduction to the Mathematics and Methods of Astrodynamics, AIAA Education, 1999.
* Lancaster, E.R. and Blanchard, R.C., A Unified Form of Lambert’s Theorem, Goddard Space Flight Center, 1968.
* </p>
* @author Joris Olympio
* @author Romain Serra
* @see LambertBoundaryConditions
* @see LambertBoundaryVelocities
* @since 13.1
*/
public class LambertSolver {
/** gravitational constant. */
private final double mu;
/** Creator.
*
* @param mu gravitational constant
*/
public LambertSolver(final double mu) {
this.mu = mu;
}
/** Solve for the corresponding velocity vectors given two position vectors and a duration.
* <p>
* The logic for setting {@code posigrade} and {@code nRev} is that the
* sweep angle Δυ travelled by the object between {@code t1} and {@code t2} is
* 2π {@code nRev +1} - α if {@code posigrade} is false and 2π {@code nRev} + α
* if {@code posigrade} is true, where α is the separation angle between
* {@code p1} and {@code p2}, which is always computed between 0 and π
* (because in 3D without a normal reference, vector angles cannot go past π).
* </p>
* <p>
* This implies that {@code posigrade} should be set to true if {@code p2} is
* located in the half orbit starting at {@code p1} and it should be set to
* false if {@code p2} is located in the half orbit ending at {@code p1},
* regardless of the number of periods between {@code t1} and {@code t2},
* and {@code nRev} should be set accordingly.
* </p>
* <p>
* As an example, if {@code t2} is less than half a period after {@code t1},
* then {@code posigrade} should be {@code true} and {@code nRev} should be 0.
* If {@code t2} is more than half a period after {@code t1} but less than
* one period after {@code t1}, {@code posigrade} should be {@code false} and
* {@code nRev} should be 0.
* </p>
* <p>
* If solving fails completely, null is returned.
* If only the computation of terminal velocity fails, a partial pair of velocities is returned (with some NaNs).
* </p>
* @param posigrade flag indicating the direction of motion
* @param nRev number of revolutions
* @param boundaryConditions Lambert problem boundary conditions
* @return boundary velocity vectors
*/
public LambertBoundaryVelocities solve(final boolean posigrade, final int nRev,
final LambertBoundaryConditions boundaryConditions) {
final Vector3D p1 = boundaryConditions.getInitialPosition();
final Vector3D p2 = boundaryConditions.getTerminalPosition();
final double r1 = p1.getNorm();
final double r2 = p2.getNorm();
final double tau = boundaryConditions.getTerminalDate().durationFrom(boundaryConditions.getInitialDate());
// deal with backward case recursively
if (tau < 0.0) {
final LambertBoundaryConditions backwardConditions = new LambertBoundaryConditions(boundaryConditions.getTerminalDate(),
boundaryConditions.getTerminalPosition(), boundaryConditions.getInitialDate(), boundaryConditions.getInitialPosition(),
boundaryConditions.getReferenceFrame());
final LambertBoundaryVelocities solutionForward = solve(posigrade, nRev, backwardConditions);
if (solutionForward != null) {
return new LambertBoundaryVelocities(solutionForward.getTerminalVelocity(),
solutionForward.getInitialVelocity());
} else {
return null;
}
}
// normalizing constants
final double R = FastMath.max(r1, r2); // in m
final double V = FastMath.sqrt(mu / R); // in m/s
final double T = R / V; // in seconds
// sweep angle
double dth = Vector3D.angle(p1, p2);
// compute the number of revolutions
if (!posigrade) {
dth = 2 * FastMath.PI - dth;
}
// call Lambert's problem solver in the orbital plane, in the R-T frame
final Vector2D vDep = solveNormalized2D(r1 / R, r2 / R, dth, tau / T, nRev);
if (vDep != Vector2D.NaN) {
final double[] Vdep = vDep.toArray();
// basis vectors
// normal to the orbital arc plane
final Vector3D Pn = p1.crossProduct(p2);
// perpendicular to the radius vector, in the orbital arc plane
final Vector3D Pt = Pn.crossProduct(p1);
// tangential velocity norm
double RT = Pt.getNorm();
if (!posigrade) {
RT = -RT;
}
// velocity vector at P1
final Vector3D Vel1 = new Vector3D(V * Vdep[0] / r1, p1, V * Vdep[1] / RT, Pt);
// propagate to get terminal velocity
Vector3D terminalVelocity;
try {
final PVCoordinates pv2 = KeplerianMotionCartesianUtility.predictPositionVelocity(tau, p1, Vel1, mu);
terminalVelocity = pv2.getVelocity();
} catch (final MathIllegalStateException exception) { // failure can happen for hyperbolic orbits
terminalVelocity = Vector3D.NaN;
}
// form output
return new LambertBoundaryVelocities(Vel1, terminalVelocity);
}
return null;
}
/** Lambert's solver for the historical, planar problem.
* Assume mu=1.
*
* @param r1 radius 1
* @param r2 radius 2
* @param dth sweep angle
* @param tau time of flight
* @param mRev number of revs
* @return velocity at departure in (T, N) basis. Is Vector2D.NaN if solving fails
*/
public static Vector2D solveNormalized2D(final double r1, final double r2, final double dth, final double tau,
final int mRev) {
// decide whether to use the left or right branch (for multi-revolution
// problems), and the long- or short way.
final boolean leftbranch = dth < FastMath.PI;
int longway = 1;
if (dth > FastMath.PI) {
longway = -1;
}
final int m = FastMath.abs(mRev);
final double rtof = FastMath.abs(tau);
// non-dimensional chord ||r2-r1||
final double chord = FastMath.sqrt(r1 * r1 + r2 * r2 - 2 * r1 * r2 * FastMath.cos(dth));
// non-dimensional semi-perimeter of the triangle
final double speri = 0.5 * (r1 + r2 + chord);
// minimum energy ellipse semi-major axis
final double minSma = speri / 2.;
// lambda parameter (Eq 7.6)
final double lambda = longway * FastMath.sqrt(1 - chord / speri);
// reference tof value for the Newton solver
final double logt = FastMath.log(rtof);
// initialisation of the iterative root finding process (secant method)
// initial values
// -1 < x < 1 => Elliptical orbits
// x = 1 Parabolic orbit
// x > 1 Hyperbolic orbits
final double in1;
final double in2;
double x1;
double x2;
if (m == 0) {
// one revolution, one solution. Define the left and right asymptotes.
in1 = -0.6523333;
in2 = 0.6523333;
x1 = FastMath.log(1 + in1);
x2 = FastMath.log(1 + in2);
} else {
// select initial values, depending on the branch
if (!leftbranch) {
in1 = -0.523334;
in2 = -0.223334;
} else {
in1 = 0.723334;
in2 = 0.523334;
}
x1 = FastMath.tan(in1 * 0.5 * FastMath.PI);
x2 = FastMath.tan(in2 * 0.5 * FastMath.PI);
}
// initial estimates for the tof
final double tof1 = timeOfFlight(in1, longway, m, minSma, speri, chord);
final double tof2 = timeOfFlight(in2, longway, m, minSma, speri, chord);
// initial bounds for y
double y1;
double y2;
if (m == 0) {
y1 = FastMath.log(tof1) - logt;
y2 = FastMath.log(tof2) - logt;
} else {
y1 = tof1 - rtof;
y2 = tof2 - rtof;
}
// Solve for x with the secant method
double err = 1e20;
int iterations = 0;
final double tol = 1e-13;
final int maxiter = 50;
double xnew = 0;
while (err > tol && iterations < maxiter) {
// new x
xnew = (x1 * y2 - y1 * x2) / (y2 - y1);
// evaluate new time of flight
final double x;
if (m == 0) {
x = FastMath.exp(xnew) - 1;
} else {
x = FastMath.atan(xnew) * 2 / FastMath.PI;
}
final double tof = timeOfFlight(x, longway, m, minSma, speri, chord);
// new value of y
final double ynew;
if (m == 0) {
ynew = FastMath.log(tof) - logt;
} else {
ynew = tof - rtof;
}
// save previous and current values for the next iteration
x1 = x2;
x2 = xnew;
y1 = y2;
y2 = ynew;
// update error
err = FastMath.abs(x1 - xnew);
// increment number of iterations
++iterations;
}
// failure to converge
if (err > tol) {
return Vector2D.NaN;
}
// convert converged value of x
final double x;
if (m == 0) {
x = FastMath.exp(xnew) - 1;
} else {
x = FastMath.atan(xnew) * 2 / FastMath.PI;
}
// Solution for the semi-major axis (Eq. 7.20)
final double sma = minSma / (1 - x * x);
// compute velocities
final double eta;
if (x < 1) {
// ellipse, Eqs. 7.7, 7.17
final double alfa = 2 * FastMath.acos(x);
final double beta = longway * 2 * FastMath.asin(FastMath.sqrt((speri - chord) / (2. * sma)));
final double psi = (alfa - beta) / 2;
// Eq. 7.21
final double sinPsi = FastMath.sin(psi);
final double etaSq = 2 * sma * sinPsi * sinPsi / speri;
eta = FastMath.sqrt(etaSq);
} else {
// hyperbola
final double gamma = 2 * FastMath.acosh(x);
final double delta = longway * 2 * FastMath.asinh(FastMath.sqrt((chord - speri) / (2 * sma)));
//
final double psi = (gamma - delta) / 2.;
final double sinhPsi = FastMath.sinh(psi);
final double etaSq = -2 * sma * sinhPsi * sinhPsi / speri;
eta = FastMath.sqrt(etaSq);
}
// radial and tangential directions for departure velocity (Eq. 7.36)
final double VR1 = (1. / eta) * FastMath.sqrt(1. / minSma) * (2 * lambda * minSma / r1 - (lambda + x * eta));
final double VT1 = (1. / eta) * FastMath.sqrt(1. / minSma) * FastMath.sqrt(r2 / r1) * FastMath.sin(dth / 2);
return new Vector2D(VR1, VT1);
}
/** Compute the time of flight of a given arc of orbit.
* The time of flight is evaluated via the Lagrange expression.
*
* @param x x
* @param longway solution number; the long way or the short war
* @param mrev number of revolutions of the arc of orbit
* @param minSma minimum possible semi-major axis
* @param speri semi-parameter of the arc of orbit
* @param chord chord of the arc of orbit
* @return the time of flight for the given arc of orbit
*/
private static double timeOfFlight(final double x, final int longway, final int mrev, final double minSma,
final double speri, final double chord) {
final double a = minSma / (1 - x * x);
final double tof;
if (FastMath.abs(x) < 1) {
// Lagrange form of the time of flight equation Eq. (7.9)
// elliptical orbit (note: mu = 1)
final double beta = longway * 2 * FastMath.asin(FastMath.sqrt((speri - chord) / (2. * a)));
final double alfa = 2 * FastMath.acos(x);
tof = a * FastMath.sqrt(a) * ((alfa - FastMath.sin(alfa)) - (beta - FastMath.sin(beta)) + 2 * FastMath.PI * mrev);
} else {
// hyperbolic orbit
final double alfa = 2 * FastMath.acosh(x);
final double beta = longway * 2 * FastMath.asinh(FastMath.sqrt((speri - chord) / (-2. * a)));
tof = -a * FastMath.sqrt(-a) * ((FastMath.sinh(alfa) - alfa) - (FastMath.sinh(beta) - beta));
}
return tof;
}
/**
* Computes the Jacobian matrix of the Lambert solution.
* The rows represent the initial and terminal velocity vectors.
* The columns represent the parameters: initial time, initial position, terminal time, terminal velocity.
* <p>
* Reference:
* Di Lizia, P., Armellin, R., Zazzera, F. B., and Berz, M.
* High Order Expansion of the Solution of Two-Point Boundary Value Problems using Differential Algebra:
* Applications to Spacecraft Dynamics.
* </p>
* @param posigrade direction flag
* @param nRev number of revolutions
* @param boundaryConditions Lambert boundary conditions
* @return Jacobian matrix
*/
public RealMatrix computeJacobian(final boolean posigrade, final int nRev,
final LambertBoundaryConditions boundaryConditions) {
final LambertBoundaryVelocities velocities = solve(posigrade, nRev, boundaryConditions);
if (velocities != null) {
return computeNonTrivialCase(boundaryConditions, velocities);
} else {
final RealMatrix nanMatrix = MatrixUtils.createRealMatrix(8, 8);
for (int i = 0; i < nanMatrix.getRowDimension(); i++) {
for (int j = 0; j < nanMatrix.getColumnDimension(); j++) {
nanMatrix.setEntry(i, j, Double.NaN);
}
}
return nanMatrix;
}
}
/**
* Compute Jacobian matrix assuming there is a solution.
* @param boundaryConditions Lambert boundary conditions
* @param velocities Lambert solution
* @return Jacobian matrix
*/
private RealMatrix computeNonTrivialCase(final LambertBoundaryConditions boundaryConditions,
final LambertBoundaryVelocities velocities) {
// propagate with automatic differentiation, using initial position as independent variables
final int freeParameters = 8;
final Vector3D nominalInitialPosition = boundaryConditions.getInitialPosition();
final FieldVector3D<Gradient> initialPosition = new FieldVector3D<>(Gradient.variable(freeParameters, 5, nominalInitialPosition.getX()),
Gradient.variable(freeParameters, 6, nominalInitialPosition.getY()),
Gradient.variable(freeParameters, 7, nominalInitialPosition.getZ()));
final Vector3D nominalInitialVelocity = velocities.getInitialVelocity();
final FieldVector3D<Gradient> initialVelocity = new FieldVector3D<>(Gradient.variable(freeParameters, 1, nominalInitialVelocity.getX()),
Gradient.variable(freeParameters, 2, nominalInitialVelocity.getY()),
Gradient.variable(freeParameters, 3, nominalInitialVelocity.getZ()));
final double dt = boundaryConditions.getTerminalDate().durationFrom(boundaryConditions.getInitialDate());
final Gradient fieldDt = Gradient.variable(freeParameters, 4, dt).subtract(Gradient.variable(freeParameters, 0, 0));
final FieldPVCoordinates<Gradient> terminalPV = KeplerianMotionCartesianUtility.predictPositionVelocity(fieldDt,
initialPosition, initialVelocity, fieldDt.newInstance(mu));
// fill in intermediate Jacobian matrix
final RealMatrix intermediate = MatrixUtils.createRealMatrix(6, freeParameters);
final FieldVector3D<Gradient> terminalVelocity = terminalPV.getVelocity();
intermediate.setRow(0, initialVelocity.getX().getGradient());
intermediate.setRow(1, initialVelocity.getY().getGradient());
intermediate.setRow(2, initialVelocity.getZ().getGradient());
intermediate.setRow(3, terminalVelocity.getX().getGradient());
intermediate.setRow(4, terminalVelocity.getY().getGradient());
intermediate.setRow(5, terminalVelocity.getZ().getGradient());
// swap variables (position becomes dependent)
final RealMatrix matrixToInvert = MatrixUtils.createRealIdentityMatrix(freeParameters);
matrixToInvert.setRow(1, initialPosition.getX().getGradient());
matrixToInvert.setRow(2, initialPosition.getY().getGradient());
matrixToInvert.setRow(3, initialPosition.getZ().getGradient());
final FieldVector3D<Gradient> terminalPosition = terminalPV.getPosition();
matrixToInvert.setRow(5, terminalPosition.getX().getGradient());
matrixToInvert.setRow(6, terminalPosition.getY().getGradient());
matrixToInvert.setRow(7, terminalPosition.getZ().getGradient());
final DecompositionSolver solver = new LUDecomposition(matrixToInvert).getSolver();
final RealMatrix inverse = solver.getInverse();
return intermediate.multiply(inverse);
}
}