IodGauss.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.linear.RealMatrix;
  25. import org.hipparchus.linear.RealVector;
  26. import org.hipparchus.util.FastMath;
  27. import org.orekit.estimation.measurements.AngularAzEl;
  28. import org.orekit.estimation.measurements.AngularRaDec;
  29. import org.orekit.frames.Frame;
  30. import org.orekit.orbits.CartesianOrbit;
  31. import org.orekit.orbits.Orbit;
  32. import org.orekit.time.AbsoluteDate;
  33. import org.orekit.utils.PVCoordinates;

  35. /**
  36.  * Gauss angles-only Initial Orbit Determination (IOD) algorithm.
  37.  * <p>
  38.  * The algorithm works best when the separation between observation is less than about 60°.
  39.  * The method performs remarkably well when the data is separated by 10° or less.
  40.  *
  41.  * An orbit is determined from three lines of sight w.r.t. their respective observers
  42.  * inertial positions vectors.
  43.  * </p><p>
  44.  * References:
  45.  *   Vallado, D., Fundamentals of Astrodynamics and Applications
  46.  *   Curtis, Orbital Mechanics for Engineering Students
  47.  * </p>
  48.  * @author Julien Asquier
  49.  * @since 12.0
  50.  */
  51. public class IodGauss {

  53.     /** Gravitational constant. */
  54.     private final double mu;

  56.     /**
  57.      * Constructor.
  58.      *
  59.      * @param mu gravitational constant
  60.      */
  61.     public IodGauss(final double mu) {
  62.         this.mu = mu;
  63.     }

  65.     /**
  66.      * Estimate and orbit based on Gauss Intial Orbit Determination method.
  67.      *
  68.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  69.      * @param azEl1 first angular observation
  70.      * @param azEl2 second angular observation
  71.      * @param azEl3 third angular observation
  72.      * @return an estimate of the orbit at the central date or null if
  73.      *         no estimate is possible with the given data
  74.      */
  75.     public Orbit estimate(final Frame outputFrame, final AngularAzEl azEl1,
  76.                           final AngularAzEl azEl2, final AngularAzEl azEl3) {
  77.         return estimate(outputFrame,
  78.                         azEl1.getGroundStationPosition(outputFrame), azEl1.getDate(), azEl1.getObservedLineOfSight(outputFrame),
  79.                         azEl2.getGroundStationPosition(outputFrame), azEl2.getDate(), azEl2.getObservedLineOfSight(outputFrame),
  80.                         azEl3.getGroundStationPosition(outputFrame), azEl3.getDate(), azEl3.getObservedLineOfSight(outputFrame));
  81.     }

  83.     /**
  84.      * Estimate and orbit based on Gauss Intial Orbit Determination method.
  85.      *
  86.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  87.      * @param raDec1 first angular observation
  88.      * @param raDec2 second angular observation
  89.      * @param raDec3 third angular observation
  90.      * @return an estimate of the orbit at the central date or null if
  91.      *         no estimate is possible with the given data
  92.      */
  93.     public Orbit estimate(final Frame outputFrame, final AngularRaDec raDec1,
  94.                           final AngularRaDec raDec2, final AngularRaDec raDec3) {
  95.         return estimate(outputFrame,
  96.                         raDec1.getGroundStationPosition(outputFrame), raDec1.getDate(), raDec1.getObservedLineOfSight(outputFrame),
  97.                         raDec2.getGroundStationPosition(outputFrame), raDec2.getDate(), raDec2.getObservedLineOfSight(outputFrame),
  98.                         raDec3.getGroundStationPosition(outputFrame), raDec3.getDate(), raDec3.getObservedLineOfSight(outputFrame));
  99.     }

  101.     /**
  102.      * Estimate and orbit based on Gauss Intial Orbit Determination method.
  103.      *
  104.      * @param outputFrame inertial frame for observer coordinates and orbit estimate
  105.      * @param obsP1 observer position at obsDate1
  106.      * @param obsDate1 date of the 1st observation
  107.      * @param los1 line of sight unit vector at obsDate1
  108.      * @param obsP2 observer position at obsDate2
  109.      * @param obsDate2 date of the 2nd observation
  110.      * @param los2 line of sight unit vector at obsDate2
  111.      * @param obsP3 observer position at obsDate3
  112.      * @param obsDate3 date of the 3rd observation
  113.      * @param los3 line of sight unit vector at obsDate3
  114.      * @return an estimate of the orbit at the central date obsDate2 or null if
  115.      *         no estimate is possible with the given data
  116.      */
  117.     public Orbit estimate(final Frame outputFrame,
  118.                           final Vector3D obsP1, final AbsoluteDate obsDate1, final Vector3D los1,
  119.                           final Vector3D obsP2, final AbsoluteDate obsDate2, final Vector3D los2,
  120.                           final Vector3D obsP3, final AbsoluteDate obsDate3, final Vector3D los3) {

  122.         // Getting the difference of time between 1st and 3rd observation with 2nd observation
  123.         final double tau1 = obsDate1.getDate().durationFrom(obsDate2.getDate());
  124.         final double tau3 = obsDate3.getDate().durationFrom(obsDate2.getDate());

  126.         final double diffTau3Tau1 = tau3 - tau1;

  128.         // mathematical coefficients, see Vallado 7.3.2
  129.         final double a1  = tau3 / diffTau3Tau1;
  130.         final double a3  = -tau1 / diffTau3Tau1;
  131.         final double a1u = tau3 * ((diffTau3Tau1 * diffTau3Tau1) - tau3 * tau3) / (6. * diffTau3Tau1);
  132.         final double a3u = -tau1 * ((diffTau3Tau1 * diffTau3Tau1) - tau1 * tau1) / (6. * diffTau3Tau1);

  134.         // Creating the line of Sight Matrix and inverting it
  135.         final RealMatrix losMatrix = new Array2DRowRealMatrix(3, 3);
  136.         losMatrix.setColumn(0, los1.toArray());
  137.         losMatrix.setColumn(1, los2.toArray());
  138.         losMatrix.setColumn(2, los3.toArray());

  140.         final RealMatrix invertedLosMatrix = new LUDecomposition(losMatrix).getSolver().getInverse();

  142.         // Creating the position of observations matrix
  143.         final RealMatrix rSite = new Array2DRowRealMatrix(3, 3);
  144.         rSite.setColumn(0, obsP1.toArray());
  145.         rSite.setColumn(1, obsP2.toArray());
  146.         rSite.setColumn(2, obsP3.toArray());

  148.         // mathematical matrix and coefficients, see Vallado 7.3.2
  149.         final RealMatrix m = invertedLosMatrix.multiply(rSite);

  151.         final double d1 = m.getEntry(1, 0) * a1 - m.getEntry(1, 1) + m.getEntry(1, 2) * a3;
  152.         final double d2 = m.getEntry(1, 0) * a1u + m.getEntry(1, 2) * a3u;
  153.         final double C  = los2.dotProduct(obsP2);

  155.         // norm of the position of the second observation
  156.         final double r2Norm = obsP2.getNorm();

  158.         // Coefficients of the 8th order polynomial we need to solve to determine "r"
  159.         final double[] coeff = new double[] { -mu * mu * d2 * d2, 0., 0., -2. * mu * (C * d2 + d1 * d2), 0.,
  160.                                               0., -(d1 * d1 + 2. * C * d1 + r2Norm * r2Norm), 0., 1.0 };
  161.         final LaguerreSolver solver = new LaguerreSolver(1E-10,
  162.                                                          1E-10, 1E-10);
  163.         final Complex[] roots = solver.solveAllComplex(coeff, 5. * r2Norm);

  165.         // Looking for the first adequate root of the equation (7-16) of Vallado
  166.         double r2Mag = 0.0;
  167.         for (final Complex root : roots) {
  168.             if (root.getReal() > r2Mag &&
  169.                     FastMath.abs(root.getImaginary()) < solver.getAbsoluteAccuracy()) {
  170.                 r2Mag = root.getReal();
  171.             }
  172.         }
  173.         if (r2Mag == 0.0) {
  174.             return null;
  175.         }

  177.         // mathematical matrix and coefficients, see Vallado 7.3.2
  178.         final double u = mu / (r2Mag * r2Mag * r2Mag);

  180.         final double c1 = -(-a1 - a1u * u);
  181.         final double c2 = -1;
  182.         final double c3 = -(-a3 - a3u * u);

  184.         final RealMatrix cCoeffMatrix = new Array2DRowRealMatrix(3, 1);
  185.         cCoeffMatrix.setEntry(0, 0, -c1);
  186.         cCoeffMatrix.setEntry(1, 0, -c2);
  187.         cCoeffMatrix.setEntry(2, 0, -c3);

  189.         final RealMatrix B = m.multiply(cCoeffMatrix.scalarMultiply(1));

  191.         final RealMatrix A = new Array2DRowRealMatrix(3, 3);
  192.         A.setEntry(0, 0, c1);
  193.         A.setEntry(1, 1, c2);
  194.         A.setEntry(2, 2, c3);

  196.         // Slant ranges matrix
  197.         final RealMatrix slantRanges = new LUDecomposition(A).getSolver().solve(B);

  199.         // Position Matrix of the satellite corresponding to the 1st, 2nd and 3rd observation
  200.         final RealMatrix posMatrix = new Array2DRowRealMatrix(3, 3);
  201.         for (int i = 0; i <= 2; i++) {
  202.             final RealVector position = losMatrix.getColumnVector(i).
  203.                                                   mapMultiply(slantRanges.getEntry(i, 0)).add(rSite.getColumnVector(i));
  204.             posMatrix.setRowVector(i, position);
  205.         }
  206.         // At this point, the proper Gauss Initial Orbit determination is ending because we have the 3 positions of the
  207.         // satellite from the 3 observations. However, we could also calculate the velocity using the next f and g
  208.         // coefficients, see Vallado 7.3.2 and Vallado 2.3.1 for more details
  209.         final double pos2Norm     = posMatrix.getRowVector(1).getNorm();
  210.         final double pos2NormCubed = pos2Norm * pos2Norm * pos2Norm;

  212.         // mathematical matrix and coefficients, see Curtis algorithms 5.5 and 5.6. It is still the IOD GAUSS
  213.         final double f1 = 1. - (0.5 * mu * tau1 * tau1 / pos2NormCubed);
  214.         final double f3 = 1. - (0.5 * mu * tau3 * tau3 / pos2NormCubed);
  215.         final double g1 = tau1 - ((1. / 6.) * mu * tau1 * tau1 * tau1 / pos2NormCubed);
  216.         final double g3 = tau3 - ((1. / 6.) * mu * tau3 * tau3 * tau3 / pos2NormCubed);

  218.         final double v2EquationCoeff = 1. / (f1 * g3 - f3 * g1);
  219.         // velocity at the central position of the satellite, corresponding to the second observation
  220.         final RealVector v2 = (posMatrix.getRowVector(0).mapMultiply(-f3).add(
  221.                 posMatrix.getRowVector(2).mapMultiply(f1))).mapMultiply(v2EquationCoeff);

  223.         // position at the central position of the satellite, corresponding to the second observation
  224.         final RealVector p2 = posMatrix.getRowVector(1);

  226.         // We can finally build the Orekit Object, PVCoordinates and Orbit from p2 and v2
  227.         final Vector3D p2Vector3D = new Vector3D(p2.toArray());
  228.         final Vector3D v2Vector3D = new Vector3D(v2.toArray());
  229.         return new CartesianOrbit(new PVCoordinates(p2Vector3D, v2Vector3D), outputFrame, obsDate2, mu);
  230.     }

  232. }
Created with JaCoCo 0.8.12.202403310830