LeastSquaresTleGenerationAlgorithm.java

  1. /* Copyright 2002-2024 Mark Rutten
  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.  * Mark Rutten 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.propagation.analytical.tle.generation;

  19. import java.util.function.UnaryOperator;

  21. import org.hipparchus.CalculusFieldElement;
  22. import org.hipparchus.Field;
  23. import org.hipparchus.analysis.differentiation.Gradient;
  24. import org.hipparchus.analysis.differentiation.GradientField;
  25. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  26. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  27. import org.hipparchus.linear.DiagonalMatrix;
  28. import org.hipparchus.linear.FieldVector;
  29. import org.hipparchus.linear.MatrixUtils;
  30. import org.hipparchus.linear.RealMatrix;
  31. import org.hipparchus.linear.RealVector;
  32. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
  33. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
  34. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
  35. import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
  36. import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
  37. import org.hipparchus.util.Pair;
  38. import org.orekit.annotation.DefaultDataContext;
  39. import org.orekit.data.DataContext;
  40. import org.orekit.frames.Frame;
  41. import org.orekit.orbits.FieldKeplerianOrbit;
  42. import org.orekit.orbits.KeplerianOrbit;
  43. import org.orekit.propagation.FieldSpacecraftState;
  44. import org.orekit.propagation.SpacecraftState;
  45. import org.orekit.propagation.analytical.tle.FieldTLE;
  46. import org.orekit.propagation.analytical.tle.FieldTLEPropagator;
  47. import org.orekit.propagation.analytical.tle.TLE;
  48. import org.orekit.propagation.analytical.tle.TLEPropagator;
  49. import org.orekit.time.AbsoluteDate;
  50. import org.orekit.time.FieldAbsoluteDate;
  51. import org.orekit.time.TimeScale;
  52. import org.orekit.utils.FieldPVCoordinates;
  53. import org.orekit.utils.PVCoordinates;
  54. import org.orekit.utils.ParameterDriver;

  56. /**
  57.  * Least squares method to generate a usable TLE from a spacecraft state.
  58.  *
  59.  * @author Mark Rutten
  60.  * @since 12.0
  61.  */
  62. public class LeastSquaresTleGenerationAlgorithm implements TleGenerationAlgorithm {

  64.     /** Default value for maximum number of iterations.*/
  65.     public static final int DEFAULT_MAX_ITERATIONS = 1000;

  67.     /** UTC scale. */
  68.     private final TimeScale utc;

  70.     /** TEME frame. */
  71.     private final Frame teme;

  73.     /** Maximum number of iterations. */
  74.     private final int maxIterations;

  76.     /** RMS. */
  77.     private double rms;

  79.     /**
  80.      * Default constructor.
  81.      * <p>
  82.      * Uses the {@link DataContext#getDefault() default data context}  as well as
  83.      * {@link #DEFAULT_MAX_ITERATIONS}.
  84.      * </p>
  85.      */
  86.     @DefaultDataContext
  87.     public LeastSquaresTleGenerationAlgorithm() {
  88.         this(DEFAULT_MAX_ITERATIONS);
  89.     }

  91.     /**
  92.      * Default constructor.
  93.      * <p>
  94.      * Uses the {@link DataContext#getDefault() default data context}.
  95.      * </p>
  96.      * @param maxIterations maximum number of iterations for convergence
  97.      */
  98.     @DefaultDataContext
  99.     public LeastSquaresTleGenerationAlgorithm(final int maxIterations) {
  100.         this(maxIterations, DataContext.getDefault().getTimeScales().getUTC(),
  101.              DataContext.getDefault().getFrames().getTEME());
  102.     }

  104.     /**
  105.      * Constructor.
  106.      * @param maxIterations maximum number of iterations for convergence
  107.      * @param utc UTC time scale
  108.      * @param teme TEME frame
  109.      */
  110.     public LeastSquaresTleGenerationAlgorithm(final int maxIterations,
  111.                                               final TimeScale utc, final Frame teme) {
  112.         this.maxIterations = maxIterations;
  113.         this.utc           = utc;
  114.         this.teme          = teme;
  115.         this.rms           = Double.MAX_VALUE;
  116.     }

  118.     /** {@inheritDoc} */
  119.     @Override
  120.     public TLE generate(final SpacecraftState state, final TLE templateTLE) {

  122.         // Generation epoch
  123.         final AbsoluteDate epoch = state.getDate();

  125.         // State vector
  126.         final RealVector stateVector = MatrixUtils.createRealVector(6);
  127.         // Position/Velocity
  128.         final Vector3D position = state.getPVCoordinates().getPosition();
  129.         final Vector3D velocity = state.getPVCoordinates().getVelocity();

  131.         // Fill state vector
  132.         stateVector.setEntry(0, position.getX());
  133.         stateVector.setEntry(1, position.getY());
  134.         stateVector.setEntry(2, position.getZ());
  135.         stateVector.setEntry(3, velocity.getX());
  136.         stateVector.setEntry(4, velocity.getY());
  137.         stateVector.setEntry(5, velocity.getZ());

  139.         // Create the initial guess of the least squares problem
  140.         final RealVector startState = MatrixUtils.createRealVector(7);
  141.         startState.setSubVector(0, stateVector.getSubVector(0, 6));

  143.         // Weights
  144.         final double[] weights = new double[6];
  145.         final double velocityWeight = state.getPVCoordinates().getVelocity().getNorm() * state.getPosition().getNormSq() / state.getMu();
  146.         for (int i = 0; i < 3; i++) {
  147.             weights[i] = 1.0;
  148.             weights[i + 3] = velocityWeight;
  149.         }

  151.         // Time difference between template TLE and spacecraft state
  152.         final double dt = state.getDate().durationFrom(templateTLE.getDate());

  154.         // Construct least squares problem
  155.         final LeastSquaresProblem problem =
  156.                         new LeastSquaresBuilder().maxIterations(maxIterations)
  157.                                                  .maxEvaluations(maxIterations)
  158.                                                  .model(new ObjectiveFunction(templateTLE, dt))
  159.                                                  .target(stateVector)
  160.                                                  .weight(new DiagonalMatrix(weights))
  161.                                                  .start(startState)
  162.                                                  .build();

  164.         // Solve least squares
  165.         final LevenbergMarquardtOptimizer optimizer = new LevenbergMarquardtOptimizer();
  166.         final LeastSquaresOptimizer.Optimum optimum = optimizer.optimize(problem);
  167.         rms = optimum.getRMS();

  169.         // Create new TLE from mean state
  170.         final Vector3D positionEstimated  = new Vector3D(optimum.getPoint().getSubVector(0, 3).toArray());
  171.         final Vector3D velocityEstimated  = new Vector3D(optimum.getPoint().getSubVector(3, 3).toArray());
  172.         final PVCoordinates pvCoordinates = new PVCoordinates(positionEstimated, velocityEstimated);
  173.         final KeplerianOrbit orbit = new KeplerianOrbit(pvCoordinates, teme, epoch, state.getMu());
  174.         final TLE generated = TleGenerationUtil.newTLE(orbit, templateTLE, templateTLE.getBStar(), utc);

  176.         // Verify if parameters are estimated
  177.         for (final ParameterDriver templateDrivers : templateTLE.getParametersDrivers()) {
  178.             if (templateDrivers.isSelected()) {
  179.                 // Set to selected for the new TLE
  180.                 generated.getParameterDriver(templateDrivers.getName()).setSelected(true);
  181.             }
  182.         }

  184.         // Return
  185.         return generated;

  187.     }

  189.     /**
  190.      * Get the Root Mean Square of the TLE estimation.
  191.      * <p>
  192.      * Be careful that the RMS is updated each time the
  193.      * {@link LeastSquaresTleGenerationAlgorithm#generate(SpacecraftState, TLE)}
  194.      * method is called.
  195.      * </p>
  196.      * @return the RMS
  197.      */
  198.     public double getRms() {
  199.         return rms;
  200.     }

  202.     /** {@inheritDoc} */
  203.     @Override
  204.     public <T extends CalculusFieldElement<T>> FieldTLE<T> generate(final FieldSpacecraftState<T> state,
  205.                                                                     final FieldTLE<T> templateTLE) {
  206.         throw new UnsupportedOperationException();
  207.     }

  209.     /** Least squares model. */
  210.     private class ObjectiveFunction implements MultivariateJacobianFunction {

  212.         /** Template TLE. */
  213.         private final FieldTLE<Gradient> templateTLE;

  215.         /** Time difference between template TLE and spacecraft state (s). */
  216.         private final double dt;

  218.         /**
  219.          * Constructor.
  220.          * @param templateTLE template TLE
  221.          * @param dt time difference between template TLE and spacecraft state (s)
  222.          */
  223.         ObjectiveFunction(final TLE templateTLE, final double dt) {
  224.             this.dt = dt;
  225.             // Conversion of template TLE to a field TLE
  226.             final Field<Gradient> field = GradientField.getField(7);
  227.             this.templateTLE = new FieldTLE<>(field, templateTLE.getLine1(), templateTLE.getLine2(), utc);
  228.         }

  230.         /**  {@inheritDoc} */
  231.         @Override
  232.         public Pair<RealVector, RealMatrix> value(final RealVector point) {
  233.             final RealVector objectiveOscState = MatrixUtils.createRealVector(6);
  234.             final RealMatrix objectiveJacobian = MatrixUtils.createRealMatrix(6, 7);
  235.             getTransformedAndJacobian(state -> meanStateToPV(state), point, objectiveOscState, objectiveJacobian);
  236.             return new Pair<>(objectiveOscState, objectiveJacobian);
  237.         }

  239.         /**
  240.          * Fill model.
  241.          * @param operator state vector propagation
  242.          * @param state state vector
  243.          * @param transformed value to fill
  244.          * @param jacobian Jacobian to fill
  245.          */
  246.         private void getTransformedAndJacobian(final UnaryOperator<FieldVector<Gradient>> operator,
  247.                                                final RealVector state, final RealVector transformed,
  248.                                                final RealMatrix jacobian) {

  250.             // State dimension
  251.             final int stateDim = state.getDimension();

  253.             // Initialise the state as field to calculate the gradient
  254.             final GradientField field = GradientField.getField(stateDim);
  255.             final FieldVector<Gradient> fieldState = MatrixUtils.createFieldVector(field, stateDim);
  256.             for (int i = 0; i < stateDim; ++i) {
  257.                 fieldState.setEntry(i, Gradient.variable(stateDim, i, state.getEntry(i)));
  258.             }

  260.             // Call operator
  261.             final FieldVector<Gradient> fieldTransformed = operator.apply(fieldState);

  263.             // Output dimension
  264.             final int outDim = fieldTransformed.getDimension();

  266.             // Extract transform and Jacobian as real values
  267.             for (int i = 0; i < outDim; ++i) {
  268.                 transformed.setEntry(i, fieldTransformed.getEntry(i).getReal());
  269.                 jacobian.setRow(i, fieldTransformed.getEntry(i).getGradient());
  270.             }

  272.         }

  274.         /**
  275.          * Operator to propagate the state vector.
  276.          * @param state state vector
  277.          * @return propagated state vector
  278.          */
  279.         private FieldVector<Gradient> meanStateToPV(final FieldVector<Gradient> state) {
  280.             // Epoch
  281.             final FieldAbsoluteDate<Gradient> epoch = templateTLE.getDate();

  283.             // B*
  284.             final Gradient[] bStar = state.getSubVector(6, 1).toArray();

  286.             // Field
  287.             final Field<Gradient> field = epoch.getField();

  289.             // Extract mean state
  290.             final FieldVector3D<Gradient> position = new FieldVector3D<>(state.getSubVector(0, 3).toArray());
  291.             final FieldVector3D<Gradient> velocity = new FieldVector3D<>(state.getSubVector(3, 3).toArray());
  292.             final FieldPVCoordinates<Gradient> pvCoordinates = new FieldPVCoordinates<>(position, velocity);
  293.             final FieldKeplerianOrbit<Gradient> orbit = new FieldKeplerianOrbit<>(pvCoordinates, teme, epoch, field.getZero().newInstance(TLEPropagator.getMU()));

  295.             // Convert to TLE
  296.             final FieldTLE<Gradient> tle = TleGenerationUtil.newTLE(orbit, templateTLE, bStar[0], utc);

  298.             // Propagate to epoch
  299.             final FieldPVCoordinates<Gradient> propagatedCoordinates =
  300.                     FieldTLEPropagator.selectExtrapolator(tle, teme, bStar).getPVCoordinates(epoch.shiftedBy(dt), bStar);

  302.             // Osculating
  303.             final FieldVector<Gradient> osculating = MatrixUtils.createFieldVector(field, 6);
  304.             osculating.setEntry(0, propagatedCoordinates.getPosition().getX());
  305.             osculating.setEntry(1, propagatedCoordinates.getPosition().getY());
  306.             osculating.setEntry(2, propagatedCoordinates.getPosition().getZ());
  307.             osculating.setEntry(3, propagatedCoordinates.getVelocity().getX());
  308.             osculating.setEntry(4, propagatedCoordinates.getVelocity().getY());
  309.             osculating.setEntry(5, propagatedCoordinates.getVelocity().getZ());

  311.             // Return
  312.             return osculating;

  314.         }

  316.     }

  318. }
Created with JaCoCo 0.8.12.202403310830