IodGooding.java

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

  18. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  19. import org.hipparchus.util.FastMath;
  20. import org.orekit.estimation.measurements.AngularAzEl;
  21. import org.orekit.estimation.measurements.AngularRaDec;
  22. import org.orekit.frames.Frame;
  23. import org.orekit.orbits.CartesianOrbit;
  24. import org.orekit.orbits.Orbit;
  25. import org.orekit.time.AbsoluteDate;
  26. import org.orekit.utils.PVCoordinates;

  27. /**
  28.  * Gooding angles only Initial Orbit Determination (IOD) algorithm, assuming Keplerian motion.
  29.  * <p>
  30.  * An orbit is determined from three lines of sight w.r.t. their respective observers
  31.  * inertial positions vectors. Gooding algorithm can handle multiple satellite's revolutions.
  32.  *
  33.  * Reference:
  34.  *    Gooding, R.H., A New Procedure for Orbit Determination Based on Three Lines of Sight (Angles only),
  35.  *    Technical Report 93004, April 1993
  36.  * </p>
  37.  * @author Joris Olympio
  38.  * @since 8.0
  39.  */
  40. public class IodGooding {

  41.     /** Gravitationnal constant. */
  42.     private final double mu;

  43.     /** Normalizing constant for distances. */
  44.     private double R;

  45.     /** Normalizing constant for velocities. */
  46.     private double V;

  47.     /** Normalizing constant for duration. */
  48.     private double T;

  49.     /** observer position 1. */
  50.     private Vector3D vObserverPosition1;

  51.     /** observer position 2. */
  52.     private Vector3D vObserverPosition2;

  53.     /** observer position 3. */
  54.     private Vector3D vObserverPosition3;

  55.     /** Date of the first observation. * */
  56.     private AbsoluteDate date1;

  57.     /** Radius of point 1 (X-R1). */
  58.     private double R1;
  59.     /** Radius of point 2 (X-R2). */
  60.     private double R2;
  61.     /** Radius of point 3 (X-R3). */
  62.     private double R3;

  63.     /** Range of point 1 (O1-R1). */
  64.     private double rho1;

  65.     /** Range of point 2 (O1-R1). */
  66.     private double rho2;

  67.     /** Range of point 1 (O1-R1). */
  68.     private double rho3;

  69.     /** working variable. */
  70.     private double D1;

  71.     /** working variable. */
  72.     private double D3;

  73.     /** factor for FD. */
  74.     private double facFiniteDiff;

  75.     /** Simple Lambert's problem solver. */
  76.     private IodLambert lambert;

  77.     /**
  78.      * Constructor.
  79.      *
  80.      * @param mu gravitational constant
  81.      */
  82.     public IodGooding(final double mu) {
  83.         this.mu   = mu;

  84.         this.rho1 = 0;
  85.         this.rho2 = 0;
  86.         this.rho3 = 0;
  87.     }

  88.     /** Get range for observation (1).
  89.      *
  90.      * @return the range for observation (1)
  91.      */
  92.     public double getRange1() {
  93.         return rho1 * R;
  94.     }

  95.     /** Get range for observation (2).
  96.      *
  97.      * @return the range for observation (2)
  98.      */
  99.     public double getRange2() {
  100.         return rho2 * R;
  101.     }

  102.     /** Get range for observation (3).
  103.      *
  104.      * @return the range for observation (3)
  105.      */
  106.     public double getRange3() {
  107.         return rho3 * R;
  108.     }

  109.     /** Estimate orbit from three angular observations.
  110.      * <p>
  111.      * This signature assumes there was less than an half revolution between start and final date
  112.      * </p>
  113.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  114.      * @param azEl1 first angular observation
  115.      * @param azEl2 second angular observation
  116.      * @param azEl3 third angular observation
  117.      * @param rho1init initial guess of the range problem. range 1, in meters
  118.      * @param rho3init initial guess of the range problem. range 3, in meters
  119.      * @return an estimate of the Keplerian orbit at the central date
  120.      *         (i.e., date of the second angular observation)
  121.      * @since 12.0
  122.      */
  123.     public Orbit estimate(final Frame outputFrame, final AngularAzEl azEl1,
  124.                           final AngularAzEl azEl2, final AngularAzEl azEl3,
  125.                           final double rho1init, final double rho3init) {
  126.         return estimate(outputFrame, azEl1, azEl2, azEl3, rho1init, rho3init, 0, true);
  127.     }

  128.     /** Estimate orbit from three angular observations.
  129.      *
  130.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  131.      * @param azEl1 first angular observation
  132.      * @param azEl2 second angular observation
  133.      * @param azEl3 third angular observation
  134.      * @param rho1init initial guess of the range problem. range 1, in meters
  135.      * @param rho3init initial guess of the range problem. range 3, in meters
  136.      * @param nRev number of complete revolutions between observation 1 and 3
  137.      * @param direction true if posigrade (short way)
  138.      * @return an estimate of the Keplerian orbit at the central date
  139.      *         (i.e., date of the second angular observation)
  140.      * @since 11.0
  141.      */
  142.     public Orbit estimate(final Frame outputFrame, final AngularAzEl azEl1,
  143.                           final AngularAzEl azEl2, final AngularAzEl azEl3,
  144.                           final double rho1init, final double rho3init,
  145.                           final int nRev, final boolean direction) {
  146.         return estimate(outputFrame,
  147.                         azEl1.getGroundStationPosition(outputFrame),
  148.                         azEl2.getGroundStationPosition(outputFrame),
  149.                         azEl3.getGroundStationPosition(outputFrame),
  150.                         azEl1.getObservedLineOfSight(outputFrame), azEl1.getDate(),
  151.                         azEl2.getObservedLineOfSight(outputFrame), azEl2.getDate(),
  152.                         azEl3.getObservedLineOfSight(outputFrame), azEl3.getDate(),
  153.                         rho1init, rho3init, nRev, direction);
  154.     }

  155.     /** Estimate orbit from three angular observations.
  156.      * <p>
  157.      * This signature assumes there was less than an half revolution between start and final date
  158.      * </p>
  159.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  160.      * @param raDec1 first angular observation
  161.      * @param raDec2 second angular observation
  162.      * @param raDec3 third angular observation
  163.      * @param rho1init initial guess of the range problem. range 1, in meters
  164.      * @param rho3init initial guess of the range problem. range 3, in meters
  165.      * @return an estimate of the Keplerian orbit at the central date
  166.      *         (i.e., date of the second angular observation)
  167.      * @since 11.0
  168.      */
  169.     public Orbit estimate(final Frame outputFrame, final AngularRaDec raDec1,
  170.                           final AngularRaDec raDec2, final AngularRaDec raDec3,
  171.                           final double rho1init, final double rho3init) {
  172.         return estimate(outputFrame, raDec1, raDec2, raDec3, rho1init, rho3init, 0, true);
  173.     }

  174.     /** Estimate orbit from three angular observations.
  175.      *
  176.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  177.      * @param raDec1 first angular observation
  178.      * @param raDec2 second angular observation
  179.      * @param raDec3 third angular observation
  180.      * @param rho1init initial guess of the range problem. range 1, in meters
  181.      * @param rho3init initial guess of the range problem. range 3, in meters
  182.      * @param nRev number of complete revolutions between observation 1 and 3
  183.      * @param direction true if posigrade (short way)
  184.      * @return an estimate of the Keplerian orbit at the central date
  185.      *         (i.e., date of the second angular observation)
  186.      * @since 11.0
  187.      */
  188.     public Orbit estimate(final Frame outputFrame, final AngularRaDec raDec1,
  189.                           final AngularRaDec raDec2, final AngularRaDec raDec3,
  190.                           final double rho1init, final double rho3init,
  191.                           final int nRev, final boolean direction) {
  192.         return estimate(outputFrame,
  193.                         raDec1.getGroundStationPosition(outputFrame),
  194.                         raDec2.getGroundStationPosition(outputFrame),
  195.                         raDec3.getGroundStationPosition(outputFrame),
  196.                         raDec1.getObservedLineOfSight(outputFrame), raDec1.getDate(),
  197.                         raDec2.getObservedLineOfSight(outputFrame), raDec2.getDate(),
  198.                         raDec3.getObservedLineOfSight(outputFrame), raDec3.getDate(),
  199.                         rho1init, rho3init, nRev, direction);
  200.     }

  201.     /** Estimate orbit from three line of sight.
  202.      * <p>
  203.      * This signature assumes there was less than an half revolution between start and final date
  204.      * </p>
  205.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  206.      * @param O1 Observer position 1
  207.      * @param O2 Observer position 2
  208.      * @param O3 Observer position 3
  209.      * @param lineOfSight1 line of sight 1
  210.      * @param dateObs1 date of observation 1
  211.      * @param lineOfSight2 line of sight 2
  212.      * @param dateObs2 date of observation 1
  213.      * @param lineOfSight3 line of sight 3
  214.      * @param dateObs3 date of observation 1
  215.      * @param rho1init initial guess of the range problem. range 1, in meters
  216.      * @param rho3init initial guess of the range problem. range 3, in meters
  217.      * @return an estimate of the Keplerian orbit at the central date
  218.      *         (i.e., date of the second angular observation)
  219.      */
  220.     public Orbit estimate(final Frame outputFrame, final Vector3D O1, final Vector3D O2, final Vector3D O3,
  221.                           final Vector3D lineOfSight1, final AbsoluteDate dateObs1,
  222.                           final Vector3D lineOfSight2, final AbsoluteDate dateObs2,
  223.                           final Vector3D lineOfSight3, final AbsoluteDate dateObs3,
  224.                           final double rho1init, final double rho3init) {
  225.         return this.estimate(outputFrame, O1, O2, O3, lineOfSight1, dateObs1, lineOfSight2, dateObs2,
  226.                              lineOfSight3, dateObs3, rho1init, rho3init, 0, true);
  227.     }

  228.     /** Estimate orbit from three line of sight.
  229.      *
  230.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  231.      * @param O1 Observer position 1
  232.      * @param O2 Observer position 2
  233.      * @param O3 Observer position 3
  234.      * @param lineOfSight1 line of sight 1
  235.      * @param dateObs1 date of observation 1
  236.      * @param lineOfSight2 line of sight 2
  237.      * @param dateObs2 date of observation 2
  238.      * @param lineOfSight3 line of sight 3
  239.      * @param dateObs3 date of observation 3
  240.      * @param rho1init initial guess of the range problem. range 1, in meters
  241.      * @param rho3init initial guess of the range problem. range 3, in meters
  242.      * @param nRev number of complete revolutions between observation1  and 3
  243.      * @param direction true if posigrade (short way)
  244.      * @return an estimate of the Keplerian orbit at the central date
  245.      *         (i.e., date of the second angular observation)
  246.      */
  247.     public Orbit estimate(final Frame outputFrame,
  248.                           final Vector3D O1, final Vector3D O2, final Vector3D O3,
  249.                           final Vector3D lineOfSight1, final AbsoluteDate dateObs1,
  250.                           final Vector3D lineOfSight2, final AbsoluteDate dateObs2,
  251.                           final Vector3D lineOfSight3, final AbsoluteDate dateObs3,
  252.                           final double rho1init, final double rho3init, final int nRev,
  253.                           final boolean direction) {

  254.         this.date1 = dateObs1;

  255.         // normalizing coefficients
  256.         R = FastMath.max(rho1init, rho3init);
  257.         V = FastMath.sqrt(mu / R);
  258.         T = R / V;

  259.         // Initialize Lambert's problem solver for non-dimensional units.
  260.         lambert = new IodLambert(1.);

  261.         this.vObserverPosition1 = O1.scalarMultiply(1. / R);
  262.         this.vObserverPosition2 = O2.scalarMultiply(1. / R);
  263.         this.vObserverPosition3 = O3.scalarMultiply(1. / R);

  264.         // solve the range problem
  265.         solveRangeProblem(outputFrame,
  266.                           rho1init / R, rho3init / R,
  267.                           dateObs3.durationFrom(dateObs1) / T, dateObs2.durationFrom(dateObs1) / T,
  268.                           nRev,
  269.                           direction,
  270.                           lineOfSight1, lineOfSight2, lineOfSight3);

  271.         // use the Gibbs problem to get the orbit now that we have three position vectors.
  272.         final IodGibbs gibbs = new IodGibbs(mu);
  273.         final Vector3D p1    = vObserverPosition1.add(lineOfSight1.scalarMultiply(rho1)).scalarMultiply(R);
  274.         final Vector3D p2    = vObserverPosition2.add(lineOfSight2.scalarMultiply(rho2)).scalarMultiply(R);
  275.         final Vector3D p3    = vObserverPosition3.add(lineOfSight3.scalarMultiply(rho3)).scalarMultiply(R);
  276.         return gibbs.estimate(outputFrame, p1, dateObs1, p2, dateObs2, p3, dateObs3);
  277.     }

  278.     /** Solve the range problem when three line of sight are given.
  279.      * @param frame frame to be used (orbit frame)
  280.      * @param rho1init   initial value for range R1, in meters
  281.      * @param rho3init   initial value for range R3, in meters
  282.      * @param T13   time of flight 1->3, in seconds
  283.      * @param T12   time of flight 1->2, in seconds
  284.      * @param nRev number of revolutions
  285.      * @param direction  posigrade (true) or retrograde
  286.      * @param lineOfSight1  line of sight 1
  287.      * @param lineOfSight2  line of sight 2
  288.      * @param lineOfSight3  line of sight 3
  289.      */
  290.     private void solveRangeProblem(final Frame frame,
  291.                                    final double rho1init, final double rho3init,
  292.                                    final double T13, final double T12,
  293.                                    final int nRev,
  294.                                    final boolean direction,
  295.                                    final Vector3D lineOfSight1,
  296.                                    final Vector3D lineOfSight2,
  297.                                    final Vector3D lineOfSight3) {
  298.         final int maxIterations = 100;
  299.         final double ARBF = 1e-6;   // finite differences step
  300.         boolean withHalley = true;  // use Halley's method
  301.         final double cvtol = 1e-14; // convergence tolerance

  302.         rho1 = rho1init;
  303.         rho3 = rho3init;

  304.         int iter = 0;
  305.         double stoppingCriterion = 10 * cvtol;
  306.         while (iter < maxIterations && FastMath.abs(stoppingCriterion) > cvtol)  {
  307.             facFiniteDiff = ARBF;

  308.             // proposed in the original algorithm by Gooding.
  309.             // We change the threshold to maxIterations / 2.
  310.             if (iter >= (maxIterations / 2)) {
  311.                 withHalley = true;
  312.             }

  313.             // tentative position for R2
  314.             final Vector3D P2 = getPositionOnLoS2(frame,
  315.                                                   lineOfSight1, rho1,
  316.                                                   lineOfSight3, rho3,
  317.                                                   T13, T12, nRev, direction);

  318.             if (P2 == null) {
  319.                 modifyIterate(lineOfSight1, lineOfSight3);
  320.             } else {
  321.                 //
  322.                 R2 = P2.getNorm();

  323.                 // compute current line of sight for (2) and the associated range
  324.                 final Vector3D C = P2.subtract(vObserverPosition2);
  325.                 rho2 = C.getNorm();
  326.                 // indicate how close we are to the direction of sight for measurement (2)
  327.                 // for convergence test
  328.                 final double CR = lineOfSight2.dotProduct(C);

  329.                 // construct a basis and the function f and g.
  330.                 // Specifically, f is the P-coordinate and g the N-coordinate.
  331.                 // They should be zero when line of sight 2 and current direction for 2 from O2 are aligned.
  332.                 final Vector3D u = lineOfSight2.crossProduct(C);
  333.                 final Vector3D P = (u.crossProduct(lineOfSight2)).normalize();
  334.                 final Vector3D ENt = lineOfSight2.crossProduct(P);

  335.                 // if ENt is zero we have a solution!
  336.                 final double ENR = ENt.getNorm();
  337.                 if (ENR == 0.) {
  338.                     break;
  339.                 }

  340.                 //Normalize EN
  341.                 final Vector3D EN = ENt.normalize();

  342.                 // Coordinate along 'F function'
  343.                 final double Fc = P.dotProduct(C);
  344.                 //Gc = EN.dotProduct(C);

  345.                 // Now get partials, by finite differences
  346.                 final double[] FD = new double[2];
  347.                 final double[] GD = new double[2];
  348.                 computeDerivatives(frame,
  349.                                    rho1, rho3,
  350.                                    lineOfSight1, lineOfSight3,
  351.                                    P, EN,
  352.                                    Fc,
  353.                                    T13, T12,
  354.                                    withHalley,
  355.                                    nRev,
  356.                                    direction,
  357.                                    FD, GD);

  358.                 // terms of the Jacobian
  359.                 final double fr1 = FD[0];
  360.                 final double fr3 = FD[1];
  361.                 final double gr1 = GD[0];
  362.                 final double gr3 = GD[1];
  363.                 // determinant of the Jacobian matrix
  364.                 final double detj = fr1 * gr3 - fr3 * gr1;

  365.                 // compute the Newton step
  366.                 D3 = -gr3 * Fc / detj;
  367.                 D1 = gr1 * Fc / detj;

  368.                 // update current values of ranges
  369.                 rho1 = rho1 + D3;
  370.                 rho3 = rho3 + D1;

  371.                 // convergence tests
  372.                 final double den = FastMath.max(CR, R2);
  373.                 stoppingCriterion = Fc / den;
  374.             }

  375.             ++iter;
  376.         } // end while
  377.     }

  378.     /** Change the current iterate if Lambert's problem solver failed to find a solution.
  379.      *
  380.      * @param lineOfSight1 line of sight 1
  381.      * @param lineOfSight3 line of sight 3
  382.      */
  383.     private void modifyIterate(final Vector3D lineOfSight1,
  384.                                final Vector3D lineOfSight3) {
  385.         // This is a modifier proposed in the initial paper of Gooding.
  386.         // Try to avoid Lambert-fail, by common-perpendicular starters
  387.         final Vector3D R13 = vObserverPosition3.subtract(vObserverPosition1);
  388.         D1 = R13.dotProduct(lineOfSight1);
  389.         D3 = R13.dotProduct(lineOfSight3);
  390.         final double D2 = lineOfSight1.dotProduct(lineOfSight3);
  391.         final double D4 = 1. - D2 * D2;
  392.         rho1 = FastMath.max((D1 - D3 * D2) / D4, 0.);
  393.         rho3 = FastMath.max((D1 * D2 - D3) / D4, 0.);
  394.     }

  395.     /** Compute the derivatives by finite-differences for the range problem.
  396.      * Specifically, we are trying to solve the problem:
  397.      *      f(x, y) = 0
  398.      *      g(x, y) = 0
  399.      * So, in a Newton-Raphson process, we would need the derivatives:
  400.      *  fx, fy, gx, gy
  401.      * Enventually,
  402.      *    dx =-f*gy / D
  403.      *    dy = f*gx / D
  404.      * where D is the determinant of the Jacobian matrix.
  405.      *
  406.      * @param frame frame to be used (orbit frame)
  407.      * @param x    current range 1
  408.      * @param y    current range 3
  409.      * @param lineOfSight1  line of sight
  410.      * @param lineOfSight3  line of sight
  411.      * @param Pin   basis vector
  412.      * @param Ein   basis vector
  413.      * @param F     value of the f-function
  414.      * @param T13   time of flight 1->3, in seconds
  415.      * @param T12   time of flight 1->2, in seconds
  416.      * @param withHalley    use Halley iterative method
  417.      * @param nRev  number of revolutions
  418.      * @param direction direction of motion
  419.      * @param FD    derivatives of f wrt (rho1, rho3) by finite differences
  420.      * @param GD    derivatives of g wrt (rho1, rho3) by finite differences
  421.      */
  422.     private void computeDerivatives(final Frame frame,
  423.                                     final double x, final double y,
  424.                                     final Vector3D lineOfSight1, final Vector3D lineOfSight3,
  425.                                     final Vector3D Pin,
  426.                                     final Vector3D Ein,
  427.                                     final double F,
  428.                                     final double T13, final double T12,
  429.                                     final boolean withHalley,
  430.                                     final int nRev,
  431.                                     final boolean direction,
  432.                                     final double[] FD, final double[] GD) {

  433.         final Vector3D P = Pin.normalize();
  434.         final Vector3D EN = Ein.normalize();

  435.         // Now get partials, by finite differences
  436.         // steps for the differentiation
  437.         final double dx = facFiniteDiff * x;
  438.         final double dy = facFiniteDiff * y;

  439.         final Vector3D Cm1 = getPositionOnLoS2 (frame,
  440.                                                 lineOfSight1, x - dx,
  441.                                                 lineOfSight3, y,
  442.                                                 T13, T12, nRev, direction).subtract(vObserverPosition2);

  443.         final double Fm1 = P.dotProduct(Cm1);
  444.         final double Gm1 = EN.dotProduct(Cm1);

  445.         final Vector3D Cp1 = getPositionOnLoS2 (frame,
  446.                                                 lineOfSight1, x + dx,
  447.                                                 lineOfSight3, y,
  448.                                                 T13, T12, nRev, direction).subtract(vObserverPosition2);

  449.         final double Fp1  = P.dotProduct(Cp1);
  450.         final double Gp1 = EN.dotProduct(Cp1);

  451.         // derivatives df/drho1 and dg/drho1
  452.         final double Fx = (Fp1 - Fm1) / (2. * dx);
  453.         final double Gx = (Gp1 - Gm1) / (2. * dx);

  454.         final Vector3D Cm3 = getPositionOnLoS2 (frame,
  455.                                                 lineOfSight1, x,
  456.                                                 lineOfSight3, y - dy,
  457.                                                 T13, T12, nRev, direction).subtract(vObserverPosition2);

  458.         final double Fm3 = P.dotProduct(Cm3);
  459.         final double Gm3 = EN.dotProduct(Cm3);

  460.         final Vector3D Cp3 = getPositionOnLoS2 (frame,
  461.                                                 lineOfSight1, x,
  462.                                                 lineOfSight3, y + dy,
  463.                                                 T13, T12, nRev, direction).subtract(vObserverPosition2);

  464.         final double Fp3 = P.dotProduct(Cp3);
  465.         final double Gp3 = EN.dotProduct(Cp3);

  466.         // derivatives df/drho3 and dg/drho3
  467.         final double Fy = (Fp3 - Fm3) / (2. * dy);
  468.         final double Gy = (Gp3 - Gm3) / (2. * dy);
  469.         final double detJac = Fx * Gy - Fy * Gx;

  470.         // Coefficients for the classical Newton-Raphson iterative method
  471.         FD[0] = Fx;
  472.         FD[1] = Fy;
  473.         GD[0] = Gx;
  474.         GD[1] = Gy;

  475.         // Modified Newton-Raphson process, with Halley's method to have cubic convergence.
  476.         // This requires computing second order derivatives.
  477.         if (withHalley) {
  478.             //
  479.             final double hrho1Sq = dx * dx;
  480.             final double hrho3Sq = dy * dy;

  481.             // Second order derivatives: d^2f / drho1^2 and d^2g / drho3^2
  482.             final double Fxx = (Fp1 + Fm1 - 2 * F) / hrho1Sq;
  483.             final double Gxx = (Gp1 + Gm1 - 2 * F) / hrho1Sq;
  484.             final double Fyy = (Fp3 + Fp3 - 2 * F) / hrho3Sq;
  485.             final double Gyy = (Gm3 + Gm3 - 2 * F) / hrho3Sq;

  486.             final Vector3D Cp13 = getPositionOnLoS2 (frame,
  487.                                                      lineOfSight1, x + dx,
  488.                                                      lineOfSight3, y + dy,
  489.                                                      T13, T12, nRev, direction).subtract(vObserverPosition2);

  490.             // f function value at (x1+dx1, x3+dx3)
  491.             final double Fp13 = P.dotProduct(Cp13);
  492.             // g function value at (x1+dx1, x3+dx3)
  493.             final double Gp13 = EN.dotProduct(Cp13);

  494.             final Vector3D Cm13 = getPositionOnLoS2 (frame,
  495.                                                      lineOfSight1, x - dx,
  496.                                                      lineOfSight3, y - dy,
  497.                                                      T13, T12, nRev, direction).subtract(vObserverPosition2);

  498.             // f function value at (x1-dx1, x3-dx3)
  499.             final double Fm13 = P.dotProduct(Cm13);
  500.             // g function value at (x1-dx1, x3-dx3)
  501.             final double Gm13 = EN.dotProduct(Cm13);

  502.             // Second order derivatives:
  503.             final double Fxy = (Fp13 + Fm13) / (2 * dx * dy) - 0.5 * (Fxx * dx / dy + Fyy * dy / dx) - F / (dx * dy);
  504.             final double Gxy = (Gp13 + Gm13) / (2 * dx * dy) - 0.5 * (Gxx * dx / dy + Gyy * dy / dx) - F / (dx * dy);

  505.             // delta Newton Raphson, 1st order step
  506.             final double dx3NR = -Gy * F / detJac;
  507.             final double dx1NR = Gx * F / detJac;

  508.             // terms of the Jacobian, considering the development, after linearization
  509.             // of the second order Taylor expansion around the Newton Raphson iterate:
  510.             // (fx + 1/2 * fxx * dx* + 1/2 * fxy * dy*) * dx
  511.             //      + (fy + 1/2 * fyy * dy* + 1/2 * fxy * dx*) * dy
  512.             // where: dx* and dy* would be the step of the Newton raphson process.
  513.             final double FxH = Fx + 0.5 * (Fxx * dx3NR + Fxy * dx1NR);
  514.             final double FyH = Fy + 0.5 * (Fxy * dx3NR + Fxx * dx1NR);
  515.             final double GxH = Gx + 0.5 * (Gxx * dx3NR + Gxy * dx1NR);
  516.             final double GyH = Gy + 0.5 * (Gxy * dx3NR + Gyy * dx1NR);

  517.             // New Halley's method "Jacobian"
  518.             FD[0] = FxH;
  519.             FD[1] = FyH;
  520.             GD[0] = GxH;
  521.             GD[1] = GyH;
  522.         }
  523.     }

  524.     /** Calculate the position along sight-line.
  525.      * @param frame frame to be used (orbit frame)
  526.      * @param E1 line of sight 1
  527.      * @param RO1 distance along E1
  528.      * @param E3 line of sight 3
  529.      * @param RO3 distance along E3
  530.      * @param T12   time of flight
  531.      * @param T13   time of flight
  532.      * @param nRev  number of revolutions
  533.      * @param posigrade direction of motion
  534.      * @return (R2-O2)
  535.      */
  536.     private Vector3D getPositionOnLoS2(final Frame frame,
  537.                                        final Vector3D E1, final double RO1,
  538.                                        final Vector3D E3, final double RO3,
  539.                                        final double T13, final double T12,
  540.                                        final int nRev, final boolean posigrade) {
  541.         final Vector3D P1 = vObserverPosition1.add(E1.scalarMultiply(RO1));
  542.         R1 = P1.getNorm();

  543.         final Vector3D P3 = vObserverPosition3.add(E3.scalarMultiply(RO3));
  544.         R3 = P3.getNorm();

  545.         final Vector3D P13 = P1.crossProduct(P3);

  546.         // sweep angle
  547.         // (Fails only if either R1 or R2 is zero)
  548.         double TH = FastMath.atan2(P13.getNorm(), P1.dotProduct(P3));

  549.         // compute the number of revolutions
  550.         if (!posigrade) {
  551.             TH = 2 * FastMath.PI - TH;
  552.         }

  553.         // Solve the Lambert's problem to get the velocities at endpoints
  554.         final double[] V1 = new double[2];
  555.         // work with non-dimensional units (MU=1)
  556.         final boolean exitflag = lambert.solveLambertPb(R1, R3, TH, T13, nRev, V1);

  557.         if (exitflag) {
  558.             // basis vectors
  559.             final Vector3D Pn = P1.crossProduct(P3);
  560.             final Vector3D Pt = Pn.crossProduct(P1);

  561.             // tangential velocity norm
  562.             double RT = Pt.getNorm();
  563.             if (!posigrade) {
  564.                 RT = -RT;
  565.             }

  566.             // velocity vector at P1
  567.             final Vector3D Vel1 = new  Vector3D(V1[0] / R1, P1, V1[1] / RT, Pt);

  568.             // estimate the position at the second observation time
  569.             // propagate (P1, V1) during TAU + T12 to get (P2, V2)
  570.             final PVCoordinates pv1   = new PVCoordinates(P1, Vel1);
  571.             final Orbit         orbit = new CartesianOrbit(pv1, frame, date1, 1.);

  572.             return orbit.shiftedBy(T12).getPosition();
  573.         }

  574.         return null;
  575.     }

  576. }