DerivativeStateUtils.java

  1. /* Copyright 2022-2025 Romain Serra
  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.utils;

  19. import org.hipparchus.analysis.differentiation.Gradient;
  20. import org.hipparchus.analysis.differentiation.GradientField;
  21. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  22. import org.hipparchus.linear.RealMatrix;
  23. import org.orekit.attitudes.Attitude;
  24. import org.orekit.attitudes.AttitudeProvider;
  25. import org.orekit.attitudes.FieldAttitude;
  26. import org.orekit.frames.Frame;
  27. import org.orekit.orbits.FieldOrbit;
  28. import org.orekit.orbits.Orbit;
  29. import org.orekit.orbits.PositionAngleBased;
  30. import org.orekit.orbits.PositionAngleType;
  31. import org.orekit.propagation.FieldSpacecraftState;
  32. import org.orekit.propagation.SpacecraftState;
  33. import org.orekit.time.FieldAbsoluteDate;

  35. /**
  36.  * Utility class used to convert state vectors in Taylor differential algebra.
  37.  *
  38.  * @author Romain Serra
  39.  * @since 13.1
  40.  * @see Gradient
  41.  */
  42. public class DerivativeStateUtils {

  44.     /** Private constructor. */
  45.     private DerivativeStateUtils() {
  46.         // Empty constructor
  47.     }

  49.     /**
  50.      * Method creating a Gradient version of the input state from a given transition matrix.
  51.      * The number of independent variables equals the number of columns.
  52.      * The number of rows tells how many state variables are considered to be the dependent variables in the Taylor differential algebra.
  53.      * If the number of state variables is greater than 6, mass will be considered one.
  54.      * Additional data and derivatives are ignored.
  55.      * @param state full state
  56.      * @param partialDerivatives Jacobian matrix of state variables to consider as dependent ones, w.r.t. unknown parameters
  57.      * @param attitudeProvider provider to recompute attitude, can be null
  58.      * @return fielded state
  59.      * @see FieldSpacecraftState
  60.      * @see SpacecraftState
  61.      */
  62.     public static FieldSpacecraftState<Gradient> buildSpacecraftStateTransitionGradient(final SpacecraftState state,
  63.                                                                                         final RealMatrix partialDerivatives,
  64.                                                                                         final AttitudeProvider attitudeProvider) {
  65.         final int freeParameters = partialDerivatives.getColumnDimension();
  66.         final GradientField field = GradientField.getField(freeParameters);
  67.         final int j = partialDerivatives.getRowDimension();
  68.         final double mass = state.getMass();
  69.         final Gradient fieldMass = (j >= 7) ? new Gradient(mass, partialDerivatives.getRow(6)) : Gradient.constant(freeParameters, mass);
  70.         if (state.isOrbitDefined()) {
  71.             final double[] stateValues = new double[6];
  72.             final double[] stateDerivatives = stateValues.clone();
  73.             final Orbit orbit = state.getOrbit();
  74.             final PositionAngleType positionAngleType = extractPositionAngleType(orbit);
  75.             orbit.getType().mapOrbitToArray(orbit, positionAngleType, stateValues, stateDerivatives);
  76.             final Gradient[] stateVariables = new Gradient[stateValues.length];
  77.             for (int i = 0; i < stateVariables.length; i++) {
  78.                 stateVariables[i] = (i < freeParameters) ? new Gradient(stateValues[i], partialDerivatives.getRow(i)) :
  79.                         Gradient.constant(freeParameters, stateValues[i]);
  80.             }
  81.             final FieldOrbit<Gradient> fieldOrbit = buildFieldOrbit(field, orbit, stateVariables, stateDerivatives);
  82.             return buildFieldStateFromFieldOrbit(fieldOrbit, fieldMass, state.getAttitude(), attitudeProvider);
  83.         } else {
  84.             // state is not orbit defined
  85.             final AbsolutePVCoordinates coordinates = state.getAbsPVA();
  86.             final double[] constants = buildPVArray(coordinates.getPVCoordinates());
  87.             final Gradient[] stateVariables = new Gradient[constants.length];
  88.             for (int i = 0; i < stateVariables.length; i++) {
  89.                 stateVariables[i] = (i < freeParameters) ? new Gradient(constants[i], partialDerivatives.getRow(i)) :
  90.                         Gradient.constant(freeParameters, constants[i]);
  91.             }
  92.             final FieldVector3D<Gradient> position = new FieldVector3D<>(stateVariables[0], stateVariables[1],
  93.                     stateVariables[2]);
  94.             final FieldVector3D<Gradient> velocity = new FieldVector3D<>(stateVariables[3], stateVariables[4],
  95.                     stateVariables[5]);
  96.             final FieldAbsolutePVCoordinates<Gradient> fieldPV = buildFieldAbsolutePV(position, velocity, coordinates);
  97.             return buildFieldStateFromFieldPV(fieldPV, fieldMass, state.getAttitude(), attitudeProvider);
  98.         }
  99.     }

  101.     /**
  102.      * Method creating a Gradient version of the input state, using the state vector as the independent variables of a first-
  103.      * order Taylor algebra. If the number of variables is greater than 6, mass will be considered one.
  104.      * Additional data and derivatives are ignored.
  105.      * @param field gradient field
  106.      * @param state full state
  107.      * @param attitudeProvider provider to recompute attitude, can be null
  108.      * @return fielded state
  109.      * @see FieldSpacecraftState
  110.      * @see SpacecraftState
  111.      */
  112.     public static FieldSpacecraftState<Gradient> buildSpacecraftStateGradient(final GradientField field,
  113.                                                                               final SpacecraftState state,
  114.                                                                               final AttitudeProvider attitudeProvider) {
  115.         final int freeParameters = field.getZero().getFreeParameters();
  116.         final double mass = state.getMass();
  117.         final Gradient fieldMass = (freeParameters >= 7) ? Gradient.variable(freeParameters, 6, mass) :
  118.                 Gradient.constant(freeParameters, mass);
  119.         final Attitude oldAttitude = state.getAttitude();
  120.         if (state.isOrbitDefined()) {
  121.             final FieldOrbit<Gradient> fieldOrbit = buildOrbitGradient(field, state.getOrbit());
  122.             return buildFieldStateFromFieldOrbit(fieldOrbit, fieldMass, oldAttitude, attitudeProvider);
  123.         } else {
  124.             // state is not orbit defined
  125.             final FieldAbsolutePVCoordinates<Gradient> fieldPV = buildAbsolutePVGradient(field, state.getAbsPVA());
  126.             return buildFieldStateFromFieldPV(fieldPV, fieldMass, oldAttitude, attitudeProvider);
  127.         }
  128.     }

  130.     /**
  131.      * Add or recompute attitude to fill in full state from orbit.
  132.      * @param fieldOrbit orbit
  133.      * @param fieldMass mass
  134.      * @param attitude constant attitude
  135.      * @param attitudeProvider provider to recompute attitude, can be null
  136.      * @return state
  137.      */
  138.     private static FieldSpacecraftState<Gradient> buildFieldStateFromFieldOrbit(final FieldOrbit<Gradient> fieldOrbit,
  139.                                                                                 final Gradient fieldMass, final Attitude attitude,
  140.                                                                                 final AttitudeProvider attitudeProvider) {
  141.         final FieldAttitude<Gradient> fieldAttitude = (attitudeProvider == null) ?
  142.                 new FieldAttitude<>(fieldMass.getField(), attitude) :
  143.                 attitudeProvider.getAttitude(fieldOrbit, fieldOrbit.getDate(), fieldOrbit.getFrame());
  144.         return new FieldSpacecraftState<>(fieldOrbit, fieldAttitude).withMass(fieldMass);
  145.     }

  147.     /**
  148.      * Add or recompute attitude to fill in full state.
  149.      * @param fieldPV coordinates
  150.      * @param fieldMass mass
  151.      * @param attitude constant attitude
  152.      * @param attitudeProvider provider to recompute attitude, can be null
  153.      * @return state
  154.      */
  155.     private static FieldSpacecraftState<Gradient> buildFieldStateFromFieldPV(final FieldAbsolutePVCoordinates<Gradient> fieldPV,
  156.                                                                              final Gradient fieldMass, final Attitude attitude,
  157.                                                                              final AttitudeProvider attitudeProvider) {
  158.         final FieldAttitude<Gradient> fieldAttitude = (attitudeProvider == null) ?
  159.                 new FieldAttitude<>(fieldMass.getField(), attitude) :
  160.                 attitudeProvider.getAttitude(fieldPV, fieldPV.getDate(), fieldPV.getFrame());
  161.         return new FieldSpacecraftState<>(fieldPV, fieldAttitude).withMass(fieldMass);
  162.     }

  164.     /**
  165.      * Method creating a Gradient version of the input orbit, using the state vector as the independent variables of a first-
  166.      * order Taylor algebra. If the number of variables is greater than 6, mass will be considered one.
  167.      * @param field gradient field
  168.      * @param orbit orbit
  169.      * @return fielded orbit
  170.      * @see org.orekit.orbits.FieldOrbit
  171.      * @see org.orekit.orbits.Orbit
  172.      */
  173.     public static FieldOrbit<Gradient> buildOrbitGradient(final GradientField field, final Orbit orbit) {
  174.         final int freeParameters = field.getZero().getFreeParameters();
  175.         final double[] stateValues = new double[6];
  176.         final double[] stateDerivatives = stateValues.clone();
  177.         final PositionAngleType positionAngleType = extractPositionAngleType(orbit);
  178.         orbit.getType().mapOrbitToArray(orbit, positionAngleType, stateValues, stateDerivatives);
  179.         final Gradient[] stateVariables = new Gradient[stateValues.length];
  180.         for (int i = 0; i < stateVariables.length; i++) {
  181.             stateVariables[i] = (i < freeParameters) ? Gradient.variable(freeParameters, i, stateValues[i]) :
  182.                     Gradient.constant(freeParameters, stateValues[i]);
  183.         }
  184.         return buildFieldOrbit(field, orbit, stateVariables, stateDerivatives);
  185.     }

  187.     /**
  188.      * Create the field orbit.
  189.      * @param field gradient field
  190.      * @param orbit orbit
  191.      * @param stateVariables state in Taylor differential algebra
  192.      * @param stateDerivatives state derivatives
  193.      * @return orbit in Taylor differential algebra
  194.      */
  195.     private static FieldOrbit<Gradient> buildFieldOrbit(final GradientField field, final Orbit orbit,
  196.                                                         final Gradient[] stateVariables, final double[] stateDerivatives) {
  197.         final FieldAbsoluteDate<Gradient> date = new FieldAbsoluteDate<>(field, orbit.getDate());
  198.         final int freeParameters = field.getZero().getFreeParameters();
  199.         final Gradient mu = Gradient.constant(freeParameters, orbit.getMu());
  200.         final Gradient[] fieldStateDerivatives = new Gradient[stateVariables.length];
  201.         for (int i = 0; i < stateVariables.length; i++) {
  202.             fieldStateDerivatives[i] = Gradient.constant(freeParameters, stateDerivatives[i]);
  203.         }
  204.         final PositionAngleType positionAngleType = extractPositionAngleType(orbit);
  205.         final Frame frame = orbit.getFrame();
  206.         return orbit.getType().mapArrayToOrbit(stateVariables, fieldStateDerivatives, positionAngleType, date, mu, frame);
  207.     }

  209.     /**
  210.      * Extract position angle type.
  211.      * @param orbit orbit
  212.      * @return angle type
  213.      */
  214.     private static PositionAngleType extractPositionAngleType(final Orbit orbit) {
  215.         if (orbit instanceof PositionAngleBased<?>) {
  216.             final PositionAngleBased<?> positionAngleBased = (PositionAngleBased<?>) orbit;
  217.             return positionAngleBased.getCachedPositionAngleType();
  218.         }
  219.         return null;
  220.     }

  222.     /**
  223.      * Method creating a Gradient version of the input coordinates, using the state vector as the independent variables of a first-
  224.      * order Taylor algebra. If the number of variables is greater than 6, mass will be considered one.
  225.      * @param field gradient field
  226.      * @param coordinates absolute coordinates
  227.      * @return fielded coordinates
  228.      * @see AbsolutePVCoordinates
  229.      * @see FieldAbsolutePVCoordinates
  230.      */
  231.     public static FieldAbsolutePVCoordinates<Gradient> buildAbsolutePVGradient(final GradientField field,
  232.                                                                                final AbsolutePVCoordinates coordinates) {
  233.         final int freeParameters = field.getZero().getFreeParameters();
  234.         final double[] constants = buildPVArray(coordinates.getPVCoordinates());
  235.         final Gradient[] stateVariables = new Gradient[constants.length];
  236.         for (int i = 0; i < stateVariables.length; i++) {
  237.             stateVariables[i] = (i < freeParameters) ? Gradient.variable(freeParameters, i, constants[i]) :
  238.                     Gradient.constant(freeParameters, constants[i]);
  239.         }
  240.         final FieldVector3D<Gradient> position = new FieldVector3D<>(stateVariables[0], stateVariables[1],
  241.                 stateVariables[2]);
  242.         final FieldVector3D<Gradient> velocity = new FieldVector3D<>(stateVariables[3], stateVariables[4],
  243.                 stateVariables[5]);
  244.         return buildFieldAbsolutePV(position, velocity, coordinates);
  245.     }

  247.     /**
  248.      * Build array from position-velocity.
  249.      * @param pvCoordinates coordinates
  250.      * @return array
  251.      */
  252.     private static double[] buildPVArray(final PVCoordinates pvCoordinates) {
  253.         final double[] constants = new double[6];
  254.         System.arraycopy(pvCoordinates.getPosition().toArray(), 0, constants, 0, 3);
  255.         System.arraycopy(pvCoordinates.getVelocity().toArray(), 0, constants, 3, 3);
  256.         return constants;
  257.     }

  259.     /**
  260.      * Create field position-velocity.
  261.      * @param fieldPosition position in Taylor differential algebra
  262.      * @param fieldVelocity velocity in Taylor differential algebra
  263.      * @param coordinates coordinates
  264.      * @return fielded position-velocity
  265.      */
  266.     private static FieldAbsolutePVCoordinates<Gradient> buildFieldAbsolutePV(final FieldVector3D<Gradient> fieldPosition,
  267.                                                                              final FieldVector3D<Gradient> fieldVelocity,
  268.                                                                              final AbsolutePVCoordinates coordinates) {
  269.         final GradientField field = fieldPosition.getX().getField();
  270.         final FieldVector3D<Gradient> acceleration = new FieldVector3D<>(field, coordinates.getAcceleration());
  271.         final FieldAbsoluteDate<Gradient> date = new FieldAbsoluteDate<>(field, coordinates.getDate());
  272.         return new FieldAbsolutePVCoordinates<>(coordinates.getFrame(), date, fieldPosition, fieldVelocity, acceleration);
  273.     }
  274. }
Created with JaCoCo 0.8.12.202403310830