IodLaplace.java

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

  19. import org.hipparchus.analysis.solvers.LaguerreSolver;
  20. import org.hipparchus.complex.Complex;
  21. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  22. import org.hipparchus.linear.Array2DRowRealMatrix;
  23. import org.hipparchus.linear.LUDecomposition;
  24. import org.hipparchus.util.FastMath;
  25. import org.orekit.estimation.measurements.AngularAzEl;
  26. import org.orekit.estimation.measurements.AngularRaDec;
  27. import org.orekit.frames.Frame;
  28. import org.orekit.orbits.CartesianOrbit;
  29. import org.orekit.orbits.Orbit;
  30. import org.orekit.time.AbsoluteDate;
  31. import org.orekit.utils.PVCoordinates;

  33. /**
  34.  * Laplace angles-only Initial Orbit Determination (IOD) algorithm, assuming Keplerian motion.
  35.  * <p>
  36.  * Laplace algorithm is one of the first method to determine orbits.
  37.  * An orbit is determined from three lines of sight w.r.t. their respective observers
  38.  * inertial positions vectors. For Laplace method, the observer is identical for all
  39.  * observations.
  40.  *
  41.  * Reference:
  42.  *    Bate, R., Mueller, D. D., &amp; White, J. E. (1971). Fundamentals of astrodynamics.
  43.  *    New York: Dover Publications.
  44.  * </p>
  45.  * @author Shiva Iyer
  46.  * @since 10.1
  47.  */
  48. public class IodLaplace {

  50.     /** Gravitational constant. */
  51.     private final double mu;

  53.     /** Constructor.
  54.      *
  55.      * @param mu  gravitational constant
  56.      */
  57.     public IodLaplace(final double mu) {
  58.         this.mu = mu;
  59.     }

  61.     /** Estimate the orbit from three angular observations at the same location.
  62.      *
  63.      * @param outputFrame Observer coordinates at time of raDec2
  64.      * @param azEl1 first angular observation
  65.      * @param azEl2 second angular observation
  66.      * @param azEl3 third angular observation
  67.      * @return estimate of the orbit at the central date or null if
  68.      *         no estimate is possible with the given data
  69.      * @since 12.0
  70.      */
  71.     public Orbit estimate(final Frame outputFrame,
  72.                           final AngularAzEl azEl1, final AngularAzEl azEl2,
  73.                           final AngularAzEl azEl3) {
  74.         return estimate(outputFrame, azEl2.getGroundStationCoordinates(outputFrame),
  75.                         azEl1.getDate(), azEl1.getObservedLineOfSight(outputFrame),
  76.                         azEl2.getDate(), azEl2.getObservedLineOfSight(outputFrame),
  77.                         azEl3.getDate(), azEl3.getObservedLineOfSight(outputFrame));
  78.     }

  80.     /** Estimate the orbit from three angular observations at the same location.
  81.      *
  82.      * @param outputFrame Observer coordinates at time of raDec2
  83.      * @param raDec1 first angular observation
  84.      * @param raDec2 second angular observation
  85.      * @param raDec3 third angular observation
  86.      * @return estimate of the orbit at the central date or null if
  87.      *         no estimate is possible with the given data
  88.      * @since 11.0
  89.      */
  90.     public Orbit estimate(final Frame outputFrame,
  91.                           final AngularRaDec raDec1, final AngularRaDec raDec2,
  92.                           final AngularRaDec raDec3) {
  93.         return estimate(outputFrame, raDec2.getGroundStationCoordinates(outputFrame),
  94.                         raDec1.getDate(), raDec1.getObservedLineOfSight(outputFrame),
  95.                         raDec2.getDate(), raDec2.getObservedLineOfSight(outputFrame),
  96.                         raDec3.getDate(), raDec3.getObservedLineOfSight(outputFrame));
  97.     }

  99.     /** Estimate orbit from three line of sight angles at the same location.
  100.      *
  101.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  102.      * @param obsPva Observer coordinates at time obsDate2
  103.      * @param obsDate1 date of observation 1
  104.      * @param los1 line of sight unit vector 1
  105.      * @param obsDate2 date of observation 2
  106.      * @param los2 line of sight unit vector 2
  107.      * @param obsDate3 date of observation 3
  108.      * @param los3 line of sight unit vector 3
  109.      * @return estimate of the orbit at the central date obsDate2 or null if
  110.      *         no estimate is possible with the given data
  111.      */
  112.     public Orbit estimate(final Frame outputFrame, final PVCoordinates obsPva,
  113.                           final AbsoluteDate obsDate1, final Vector3D los1,
  114.                           final AbsoluteDate obsDate2, final Vector3D los2,
  115.                           final AbsoluteDate obsDate3, final Vector3D los3) {

  117.         // The first observation is taken as t1 = 0
  118.         final double t2 = obsDate2.durationFrom(obsDate1);
  119.         final double t3 = obsDate3.durationFrom(obsDate1);

  121.         // Calculate the first and second derivatives of the Line Of Sight vector at t2
  122.         final Vector3D Ldot = los1.scalarMultiply((t2 - t3) / (t2 * t3)).
  123.                                   add(los2.scalarMultiply((2.0 * t2 - t3) / (t2 * (t2 - t3)))).
  124.                                   add(los3.scalarMultiply(t2 / (t3 * (t3 - t2))));
  125.         final Vector3D Ldotdot = los1.scalarMultiply(2.0 / (t2 * t3)).
  126.                                      add(los2.scalarMultiply(2.0 / (t2 * (t2 - t3)))).
  127.                                      add(los3.scalarMultiply(2.0 / (t3 * (t3 - t2))));

  129.         // The determinant will vanish if the observer lies in the plane of the orbit at t2
  130.         final double D = 2.0 * getDeterminant(los2, Ldot, Ldotdot);
  131.         if (FastMath.abs(D) < 1.0E-14) {
  132.             return null;
  133.         }

  135.         final double Dsq   = D * D;
  136.         final double R     = obsPva.getPosition().getNorm();
  137.         final double RdotL = obsPva.getPosition().dotProduct(los2);

  139.         final double D1 = getDeterminant(los2, Ldot, obsPva.getAcceleration());
  140.         final double D2 = getDeterminant(los2, Ldot, obsPva.getPosition());

  142.         // Coefficients of the 8th order polynomial we need to solve to determine "r"
  143.         final double[] coeff = new double[] {-4.0 * mu * mu * D2 * D2 / Dsq,
  144.                                              0.0,
  145.                                              0.0,
  146.                                              4.0 * mu * D2 * (RdotL / D - 2.0 * D1 / Dsq),
  147.                                              0.0,
  148.                                              0.0,
  149.                                              4.0 * D1 * RdotL / D - 4.0 * D1 * D1 / Dsq - R * R, 0.0,
  150.                                              1.0};

  152.         // Use the Laguerre polynomial solver and take the initial guess to be
  153.         // 5 times the observer's position magnitude
  154.         final LaguerreSolver solver = new LaguerreSolver(1E-10, 1E-10, 1E-10);
  155.         final Complex[]      roots  = solver.solveAllComplex(coeff, 5.0 * R);

  157.         // We consider "r" to be the positive real root with the largest magnitude
  158.         double rMag = 0.0;
  159.         for (Complex root : roots) {
  160.             if (root.getReal() > rMag &&
  161.                 FastMath.abs(root.getImaginary()) < solver.getAbsoluteAccuracy()) {
  162.                 rMag = root.getReal();
  163.             }
  164.         }
  165.         if (rMag == 0.0) {
  166.             return null;
  167.         }

  169.         // Calculate rho, the slant range from the observer to the satellite at t2.
  170.         // This yields the "r" vector, which is the satellite's position vector at t2.
  171.         final double   rCubed = rMag * rMag * rMag;
  172.         final double   rho    = -2.0 * D1 / D - 2.0 * mu * D2 / (D * rCubed);
  173.         final Vector3D posVec = los2.scalarMultiply(rho).add(obsPva.getPosition());

  175.         // Calculate rho_dot at t2, which will yield the satellite's velocity vector at t2
  176.         final double D3     = getDeterminant(los2, obsPva.getAcceleration(), Ldotdot);
  177.         final double D4     = getDeterminant(los2, obsPva.getPosition(), Ldotdot);
  178.         final double rhoDot = -D3 / D - mu * D4 / (D * rCubed);
  179.         final Vector3D velVec = los2.scalarMultiply(rhoDot).
  180.                                     add(Ldot.scalarMultiply(rho)).
  181.                                     add(obsPva.getVelocity());

  183.         // Return the estimated orbit
  184.         return new CartesianOrbit(new PVCoordinates(posVec, velVec), outputFrame, obsDate2, mu);
  185.     }

  187.     /** Calculate the determinant of the matrix with given column vectors.
  188.      *
  189.      * @param col0 Matrix column 0
  190.      * @param col1 Matrix column 1
  191.      * @param col2 Matrix column 2
  192.      * @return matrix determinant
  193.      *
  194.      */
  195.     private double getDeterminant(final Vector3D col0, final Vector3D col1, final Vector3D col2) {
  196.         final Array2DRowRealMatrix mat = new Array2DRowRealMatrix(3, 3);
  197.         mat.setColumn(0, col0.toArray());
  198.         mat.setColumn(1, col1.toArray());
  199.         mat.setColumn(2, col2.toArray());
  200.         return new LUDecomposition(mat).getDeterminant();
  201.     }

  203. }
Created with JaCoCo 0.8.12.202403310830