DerivativeStateUtils.java
/* Copyright 2022-2025 Romain Serra
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.utils;
import org.hipparchus.analysis.differentiation.Gradient;
import org.hipparchus.analysis.differentiation.GradientField;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.linear.RealMatrix;
import org.orekit.attitudes.Attitude;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.FieldAttitude;
import org.orekit.frames.Frame;
import org.orekit.orbits.FieldOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.PositionAngleBased;
import org.orekit.orbits.PositionAngleType;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.SpacecraftState;
import org.orekit.time.FieldAbsoluteDate;
/**
* Utility class used to convert state vectors in Taylor differential algebra.
*
* @author Romain Serra
* @since 13.1
* @see Gradient
*/
public class DerivativeStateUtils {
/** Private constructor. */
private DerivativeStateUtils() {
// Empty constructor
}
/**
* Method creating a Gradient version of the input state from a given transition matrix.
* The number of independent variables equals the number of columns.
* The number of rows tells how many state variables are considered to be the dependent variables in the Taylor differential algebra.
* If the number of state variables is greater than 6, mass will be considered one.
* Additional data and derivatives are ignored.
* @param state full state
* @param partialDerivatives Jacobian matrix of state variables to consider as dependent ones, w.r.t. unknown parameters
* @param attitudeProvider provider to recompute attitude, can be null
* @return fielded state
* @see FieldSpacecraftState
* @see SpacecraftState
*/
public static FieldSpacecraftState<Gradient> buildSpacecraftStateTransitionGradient(final SpacecraftState state,
final RealMatrix partialDerivatives,
final AttitudeProvider attitudeProvider) {
final int freeParameters = partialDerivatives.getColumnDimension();
final GradientField field = GradientField.getField(freeParameters);
final int j = partialDerivatives.getRowDimension();
final double mass = state.getMass();
final Gradient fieldMass = (j >= 7) ? new Gradient(mass, partialDerivatives.getRow(6)) : Gradient.constant(freeParameters, mass);
if (state.isOrbitDefined()) {
final double[] stateValues = new double[6];
final double[] stateDerivatives = stateValues.clone();
final Orbit orbit = state.getOrbit();
final PositionAngleType positionAngleType = extractPositionAngleType(orbit);
orbit.getType().mapOrbitToArray(orbit, positionAngleType, stateValues, stateDerivatives);
final Gradient[] stateVariables = new Gradient[stateValues.length];
for (int i = 0; i < stateVariables.length; i++) {
stateVariables[i] = (i < freeParameters) ? new Gradient(stateValues[i], partialDerivatives.getRow(i)) :
Gradient.constant(freeParameters, stateValues[i]);
}
final FieldOrbit<Gradient> fieldOrbit = buildFieldOrbit(field, orbit, stateVariables, stateDerivatives);
return buildFieldStateFromFieldOrbit(fieldOrbit, fieldMass, state.getAttitude(), attitudeProvider);
} else {
// state is not orbit defined
final AbsolutePVCoordinates coordinates = state.getAbsPVA();
final double[] constants = buildPVArray(coordinates.getPVCoordinates());
final Gradient[] stateVariables = new Gradient[constants.length];
for (int i = 0; i < stateVariables.length; i++) {
stateVariables[i] = (i < freeParameters) ? new Gradient(constants[i], partialDerivatives.getRow(i)) :
Gradient.constant(freeParameters, constants[i]);
}
final FieldVector3D<Gradient> position = new FieldVector3D<>(stateVariables[0], stateVariables[1],
stateVariables[2]);
final FieldVector3D<Gradient> velocity = new FieldVector3D<>(stateVariables[3], stateVariables[4],
stateVariables[5]);
final FieldAbsolutePVCoordinates<Gradient> fieldPV = buildFieldAbsolutePV(position, velocity, coordinates);
return buildFieldStateFromFieldPV(fieldPV, fieldMass, state.getAttitude(), attitudeProvider);
}
}
/**
* Method creating a Gradient version of the input state, using the state vector as the independent variables of a first-
* order Taylor algebra. If the number of variables is greater than 6, mass will be considered one.
* Additional data and derivatives are ignored.
* @param field gradient field
* @param state full state
* @param attitudeProvider provider to recompute attitude, can be null
* @return fielded state
* @see FieldSpacecraftState
* @see SpacecraftState
*/
public static FieldSpacecraftState<Gradient> buildSpacecraftStateGradient(final GradientField field,
final SpacecraftState state,
final AttitudeProvider attitudeProvider) {
final int freeParameters = field.getZero().getFreeParameters();
final double mass = state.getMass();
final Gradient fieldMass = (freeParameters >= 7) ? Gradient.variable(freeParameters, 6, mass) :
Gradient.constant(freeParameters, mass);
final Attitude oldAttitude = state.getAttitude();
if (state.isOrbitDefined()) {
final FieldOrbit<Gradient> fieldOrbit = buildOrbitGradient(field, state.getOrbit());
return buildFieldStateFromFieldOrbit(fieldOrbit, fieldMass, oldAttitude, attitudeProvider);
} else {
// state is not orbit defined
final FieldAbsolutePVCoordinates<Gradient> fieldPV = buildAbsolutePVGradient(field, state.getAbsPVA());
return buildFieldStateFromFieldPV(fieldPV, fieldMass, oldAttitude, attitudeProvider);
}
}
/**
* Add or recompute attitude to fill in full state from orbit.
* @param fieldOrbit orbit
* @param fieldMass mass
* @param attitude constant attitude
* @param attitudeProvider provider to recompute attitude, can be null
* @return state
*/
private static FieldSpacecraftState<Gradient> buildFieldStateFromFieldOrbit(final FieldOrbit<Gradient> fieldOrbit,
final Gradient fieldMass, final Attitude attitude,
final AttitudeProvider attitudeProvider) {
final FieldAttitude<Gradient> fieldAttitude = (attitudeProvider == null) ?
new FieldAttitude<>(fieldMass.getField(), attitude) :
attitudeProvider.getAttitude(fieldOrbit, fieldOrbit.getDate(), fieldOrbit.getFrame());
return new FieldSpacecraftState<>(fieldOrbit, fieldAttitude).withMass(fieldMass);
}
/**
* Add or recompute attitude to fill in full state.
* @param fieldPV coordinates
* @param fieldMass mass
* @param attitude constant attitude
* @param attitudeProvider provider to recompute attitude, can be null
* @return state
*/
private static FieldSpacecraftState<Gradient> buildFieldStateFromFieldPV(final FieldAbsolutePVCoordinates<Gradient> fieldPV,
final Gradient fieldMass, final Attitude attitude,
final AttitudeProvider attitudeProvider) {
final FieldAttitude<Gradient> fieldAttitude = (attitudeProvider == null) ?
new FieldAttitude<>(fieldMass.getField(), attitude) :
attitudeProvider.getAttitude(fieldPV, fieldPV.getDate(), fieldPV.getFrame());
return new FieldSpacecraftState<>(fieldPV, fieldAttitude).withMass(fieldMass);
}
/**
* Method creating a Gradient version of the input orbit, using the state vector as the independent variables of a first-
* order Taylor algebra. If the number of variables is greater than 6, mass will be considered one.
* @param field gradient field
* @param orbit orbit
* @return fielded orbit
* @see org.orekit.orbits.FieldOrbit
* @see org.orekit.orbits.Orbit
*/
public static FieldOrbit<Gradient> buildOrbitGradient(final GradientField field, final Orbit orbit) {
final int freeParameters = field.getZero().getFreeParameters();
final double[] stateValues = new double[6];
final double[] stateDerivatives = stateValues.clone();
final PositionAngleType positionAngleType = extractPositionAngleType(orbit);
orbit.getType().mapOrbitToArray(orbit, positionAngleType, stateValues, stateDerivatives);
final Gradient[] stateVariables = new Gradient[stateValues.length];
for (int i = 0; i < stateVariables.length; i++) {
stateVariables[i] = (i < freeParameters) ? Gradient.variable(freeParameters, i, stateValues[i]) :
Gradient.constant(freeParameters, stateValues[i]);
}
return buildFieldOrbit(field, orbit, stateVariables, stateDerivatives);
}
/**
* Create the field orbit.
* @param field gradient field
* @param orbit orbit
* @param stateVariables state in Taylor differential algebra
* @param stateDerivatives state derivatives
* @return orbit in Taylor differential algebra
*/
private static FieldOrbit<Gradient> buildFieldOrbit(final GradientField field, final Orbit orbit,
final Gradient[] stateVariables, final double[] stateDerivatives) {
final FieldAbsoluteDate<Gradient> date = new FieldAbsoluteDate<>(field, orbit.getDate());
final int freeParameters = field.getZero().getFreeParameters();
final Gradient mu = Gradient.constant(freeParameters, orbit.getMu());
final Gradient[] fieldStateDerivatives = new Gradient[stateVariables.length];
for (int i = 0; i < stateVariables.length; i++) {
fieldStateDerivatives[i] = Gradient.constant(freeParameters, stateDerivatives[i]);
}
final PositionAngleType positionAngleType = extractPositionAngleType(orbit);
final Frame frame = orbit.getFrame();
return orbit.getType().mapArrayToOrbit(stateVariables, fieldStateDerivatives, positionAngleType, date, mu, frame);
}
/**
* Extract position angle type.
* @param orbit orbit
* @return angle type
*/
private static PositionAngleType extractPositionAngleType(final Orbit orbit) {
if (orbit instanceof PositionAngleBased<?>) {
final PositionAngleBased<?> positionAngleBased = (PositionAngleBased<?>) orbit;
return positionAngleBased.getCachedPositionAngleType();
}
return null;
}
/**
* Method creating a Gradient version of the input coordinates, using the state vector as the independent variables of a first-
* order Taylor algebra. If the number of variables is greater than 6, mass will be considered one.
* @param field gradient field
* @param coordinates absolute coordinates
* @return fielded coordinates
* @see AbsolutePVCoordinates
* @see FieldAbsolutePVCoordinates
*/
public static FieldAbsolutePVCoordinates<Gradient> buildAbsolutePVGradient(final GradientField field,
final AbsolutePVCoordinates coordinates) {
final int freeParameters = field.getZero().getFreeParameters();
final double[] constants = buildPVArray(coordinates.getPVCoordinates());
final Gradient[] stateVariables = new Gradient[constants.length];
for (int i = 0; i < stateVariables.length; i++) {
stateVariables[i] = (i < freeParameters) ? Gradient.variable(freeParameters, i, constants[i]) :
Gradient.constant(freeParameters, constants[i]);
}
final FieldVector3D<Gradient> position = new FieldVector3D<>(stateVariables[0], stateVariables[1],
stateVariables[2]);
final FieldVector3D<Gradient> velocity = new FieldVector3D<>(stateVariables[3], stateVariables[4],
stateVariables[5]);
return buildFieldAbsolutePV(position, velocity, coordinates);
}
/**
* Build array from position-velocity.
* @param pvCoordinates coordinates
* @return array
*/
private static double[] buildPVArray(final PVCoordinates pvCoordinates) {
final double[] constants = new double[6];
System.arraycopy(pvCoordinates.getPosition().toArray(), 0, constants, 0, 3);
System.arraycopy(pvCoordinates.getVelocity().toArray(), 0, constants, 3, 3);
return constants;
}
/**
* Create field position-velocity.
* @param fieldPosition position in Taylor differential algebra
* @param fieldVelocity velocity in Taylor differential algebra
* @param coordinates coordinates
* @return fielded position-velocity
*/
private static FieldAbsolutePVCoordinates<Gradient> buildFieldAbsolutePV(final FieldVector3D<Gradient> fieldPosition,
final FieldVector3D<Gradient> fieldVelocity,
final AbsolutePVCoordinates coordinates) {
final GradientField field = fieldPosition.getX().getField();
final FieldVector3D<Gradient> acceleration = new FieldVector3D<>(field, coordinates.getAcceleration());
final FieldAbsoluteDate<Gradient> date = new FieldAbsoluteDate<>(field, coordinates.getDate());
return new FieldAbsolutePVCoordinates<>(coordinates.getFrame(), date, fieldPosition, fieldVelocity, acceleration);
}
}