IodLambert.java

/* Copyright 2002-2022 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.estimation.iod;

import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.estimation.measurements.Position;
import org.orekit.frames.Frame;
import org.orekit.orbits.KeplerianOrbit;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinates;

/**
 * Lambert initial orbit determination, assuming Keplerian motion.
 * An orbit is determined from two position vectors.
 *
 * 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.
 *
 * @author Joris Olympio
 * @since 8.0
 */
public class IodLambert {

    /** gravitational constant. */
    private final double mu;

    /** Creator.
     *
     * @param mu gravitational constant
     */
    public IodLambert(final double mu) {
        this.mu = mu;
    }

    /** Estimate an initial orbit from two position measurements.
     * <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>
     * @param frame     measurements frame
     * @param posigrade flag indicating the direction of motion
     * @param nRev      number of revolutions
     * @param p1        first position measurement
     * @param p2        second position measurement
     * @return an initial orbit estimation
     * @since 11.0
     */
    public KeplerianOrbit estimate(final Frame frame, final boolean posigrade,
                                   final int nRev, final Position p1,  final Position p2) {
        return estimate(frame, posigrade, nRev,
                        p1.getPosition(), p1.getDate(), p2.getPosition(), p2.getDate());
    }

    /** Estimate a Keplerian orbit 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>
     * @param frame     frame
     * @param posigrade flag indicating the direction of motion
     * @param nRev      number of revolutions
     * @param p1        position vector 1
     * @param t1        date of observation 1
     * @param p2        position vector 2
     * @param t2        date of observation 2
     * @return  an initial Keplerian orbit estimate
     */
    public KeplerianOrbit estimate(final Frame frame, final boolean posigrade,
                                   final int nRev,
                                   final Vector3D p1, final AbsoluteDate t1,
                                   final Vector3D p2, final AbsoluteDate t2) {

        final double r1 = p1.getNorm();
        final double r2 = p2.getNorm();
        final double tau = t2.durationFrom(t1); // in seconds

        // Exception if t2 < t1
        if (tau < 0.0) {
            throw new OrekitException(OrekitMessages.NON_CHRONOLOGICAL_DATES_FOR_OBSERVATIONS, t1, t2, -tau);
        }

        // 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;
        }

        // velocity vectors in the orbital plane, in the R-T frame
        final double[] Vdep = new double[2];

        // call Lambert's problem solver
        final boolean exitflag = solveLambertPb(r1 / R, r2 / R, dth, tau / T, nRev, Vdep);

        if (exitflag) {
            // 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);

            // compute the equivalent Keplerian orbit
            return new KeplerianOrbit(new PVCoordinates(p1, Vel1), frame, t1, mu);
        }

        return null;
    }

    /** Lambert's solver.
     * 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
     * @param V1 velocity at departure in (T, N) basis
     * @return something
     */
    boolean solveLambertPb(final double r1, final double r2, final double dth, final double tau,
                           final int mRev, final double[] V1) {
        // 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);
        final double theta = dth;

        // non-dimensional chord ||r2-r1||
        final double chord = FastMath.sqrt(r1 * r1 + r2 * r2 - 2 * r1 * r2 * FastMath.cos(theta));

        // 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 false;
        }

        // 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);
        V1[0] = VR1;
        V1[1] = VT1;

        return true;
    }

    /** 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 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;
    }
}