Commit fe0d81dc authored by Luc Maisonobe's avatar Luc Maisonobe
Browse files

Simplified API for State Transition Matrix and Jacobians computation.

parent 79a79ef6
Pipeline #1528 passed with stages
in 27 minutes and 28 seconds
......@@ -16,6 +16,8 @@
*/
package org.orekit.propagation;
import java.util.List;
import org.hipparchus.linear.RealMatrix;
/** Interface for extracting State Transition Matrices and Jacobians matrices from {@link SpacecraftState spacecraft state}.
......@@ -32,7 +34,8 @@ public interface MatricesHarvester {
/** Extract state transition matrix from state.
* @param state spacecraft state
* @return state transition matrix, with semantics consistent with propagation
* @return state transition matrix, with semantics consistent with propagation,
* or null if no state transition matrix is available
* {@link org.orekit.orbits.OrbitType orbit type}.
*/
RealMatrix getStateTransitionMatrix(SpacecraftState state);
......@@ -44,4 +47,18 @@ public interface MatricesHarvester {
*/
RealMatrix getParametersJacobian(SpacecraftState state);
/** Get the names of the parameters in the matrix returned by {@link #getParametersJacobian}.
* <p>
* Beware that the names of the parameters are fully known only at propagation start,
* since applications may configure the propagator, retrieve the matrices harvester first
* and select the force model parameters to retrieve afterwards (but obviously before
* starting propagation). So the method may return wrong results if called too early.
* </p>
* <p>
* The names are returned in the Jacobians matrix columns order
* </p>
* @return names of the parameters (i.e. columns) of the Jacobian matrix
*/
List<String> getJacobiansColumnsNames();
}
......@@ -376,6 +376,9 @@ public abstract class AbstractIntegratedPropagator extends AbstractPropagator {
// make sure the integrator will be reset properly even if we change its events handlers and step handlers
try (IntegratorResetter resetter = new IntegratorResetter(integrator)) {
// prepare handling of STM and Jacobian matrices
setUpStmAndJacobianHandling();
if (!tStart.equals(getInitialState().getDate())) {
// if propagation start date is not initial date,
// propagate from initial to start date without event detection
......@@ -401,6 +404,13 @@ public abstract class AbstractIntegratedPropagator extends AbstractPropagator {
}
/** Set up State Transition Matrix and Jacobian matrix handling.
* @since 11.1
*/
protected void setUpStmAndJacobianHandling() {
// nothing to do by default
}
/** Propagation with or without event detection.
* @param tEnd target date to which orbit should be propagated
* @return state at end of propagation
......
......@@ -16,6 +16,9 @@
*/
package org.orekit.propagation.integration;
import java.util.List;
import java.util.stream.Collectors;
import org.hipparchus.linear.Array2DRowRealMatrix;
import org.hipparchus.linear.RealMatrix;
import org.orekit.propagation.MatricesHarvester;
......@@ -104,6 +107,12 @@ public abstract class AbstractJacobiansMapper implements MatricesHarvester {
}
}
/** {@inheritDoc} */
@Override
public List<String> getJacobiansColumnsNames() {
return parameters.getDrivers().stream().map(d -> d.getName()).collect(Collectors.toList());
}
/** Set the Jacobian with respect to state into a one-dimensional additional state array.
* @param state spacecraft state
* @param dY1dY0 Jacobian of current state at time t₁
......
......@@ -19,6 +19,7 @@ package org.orekit.propagation.numerical;
import org.hipparchus.exception.LocalizedCoreFormats;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.QRDecomposition;
import org.hipparchus.util.Precision;
import org.orekit.errors.OrekitException;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
......@@ -38,7 +39,7 @@ class JacobianColumnGenerator implements IntegrableGenerator {
private static final int STATE_DIMENSION = 2 * SPACE_DIMENSION;
/** Threshold for matrix solving. */
private static final double THRESHOLD = 1.0e-6;
private static final double THRESHOLD = Precision.SAFE_MIN;
/** Generator for State Transition Matrix. */
private final StateTransitionMatrixGenerator stmGenerator;
......
......@@ -16,6 +16,7 @@
*/
package org.orekit.propagation.numerical;
import java.util.Collections;
import java.util.List;
import org.hipparchus.linear.MatrixUtils;
......@@ -25,45 +26,99 @@ import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.MatricesHarvester;
import org.orekit.propagation.SpacecraftState;
import org.orekit.utils.DoubleArrayDictionary;
/** Mapper between two-dimensional Jacobian matrices and one-dimensional {@link
/** Harvester between two-dimensional Jacobian matrices and one-dimensional {@link
* SpacecraftState#getAdditionalState(String) additional state arrays}.
* <p>
* This class does not hold the states by itself. Instances of this class are guaranteed
* to be immutable.
* </p>
* @author Luc Maisonobe
* @since 11.1
*/
public class MatricesMapper implements MatricesHarvester {
class NumericalPropagationHarvester implements MatricesHarvester {
/** State dimension, fixed to 6. */
public static final int STATE_DIMENSION = 6;
/** Columns names for parameters. */
private final List<String> columns;
/** Initial State Transition Matrix. */
private final RealMatrix initialStm;
/** Initial columns of the Jacobians matrix with respect to parameters. */
private final DoubleArrayDictionary initialJacobianColumns;
/** Name. */
private String name;
/** State Transition Matrix state name. */
private final String stmName;
/** Columns names for parameters. */
private List<String> columnsNames;
/** Orbit type. */
private final OrbitType orbitType;
private OrbitType orbitType;
/** Position angle type. */
private final PositionAngle angleType;
private PositionAngle positionAngleType;
/** Simple constructor.
* @param name name of the State Transition Matrix additional state
* @param columns names of the parameters for Jacobian colimns
* <p>
* The arguments for initial matrices <em>must</em> be compatible with the {@link #setOrbitType(OrbitType) orbit type}
* and {@link #setPositionAngleType(PositionAngle) position angle} that will be ultimately
* selected when propagation starts
* </p>
* @param stmName State Transition Matrix state name
* @param initialStm initial State Transition Matrix ∂Y/∂Y₀,
* if null (which is the most frequent case), assumed to be 6x6 identity
* @param initialJacobianColumns initial columns of the Jacobians matrix with respect to parameters,
* if null or if some selected parameters are missing from the dictionary, the corresponding
* initial column is assumed to be 0
*/
NumericalPropagationHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
this.stmName = stmName;
this.initialStm = initialStm == null ? MatrixUtils.createRealIdentityMatrix(STATE_DIMENSION) : initialStm;
this.initialJacobianColumns = initialJacobianColumns == null ? new DoubleArrayDictionary() : initialJacobianColumns;
this.columnsNames = Collections.emptyList();
}
/** Get the State Transition Matrix state name.
* @return State Transition Matrix state name
*/
String getStmName() {
return stmName;
}
/** Set Jacobian matrix columns names.
* @param columnsNames names of the parameters for Jacobian columns, in desired matrix order
*/
void setColumnsNames(final List<String> columnsNames) {
this.columnsNames = columnsNames;
}
/** Set orbit type.
* @param orbitType orbit type
* @param angleType position angle type
*/
MatricesMapper(final String name, final List<String> columns,
final OrbitType orbitType, final PositionAngle angleType) {
void setOrbitType(final OrbitType orbitType) {
this.orbitType = orbitType;
this.angleType = angleType;
this.columns = columns;
this.name = name;
}
/** Set position angle type.
* @param positionAngleType angle type
*/
void setPositionAngleType(final PositionAngle positionAngleType) {
this.positionAngleType = positionAngleType;
}
/** Get the initial State Transition Matrix.
* @return initial State Transition Matrix
*/
RealMatrix getInitialStateTransitionMatrix() {
return initialStm;
}
/** Get the initial column of Jacobian matrix with respect to named parameter.
* @param columnName name of the column
* @return initial column of the Jacobian matrix
*/
double[] getInitialJacobianColumn(final String columnName) {
final DoubleArrayDictionary.Entry entry = initialJacobianColumns.getEntry(columnName);
return entry == null ? new double[STATE_DIMENSION] : entry.getValue();
}
/** Get the conversion Jacobian between state parameters and parameters used for derivatives.
......@@ -83,11 +138,18 @@ public class MatricesMapper implements MatricesHarvester {
final double[][] dYdC = new double[STATE_DIMENSION][STATE_DIMENSION];
// make sure the state is in the desired orbit type
final Orbit orbit = orbitType.convertType(state.getOrbit());
if (state.isOrbitDefined()) {
// make sure the state is in the desired orbit type
final Orbit orbit = orbitType.convertType(state.getOrbit());
// compute the Jacobian, taking the position angle type into account
orbit.getJacobianWrtCartesian(angleType, dYdC);
// compute the Jacobian, taking the position angle type into account
orbit.getJacobianWrtCartesian(positionAngleType, dYdC);
} else {
// for absolute PVA, parameters are already Cartesian
for (int i = 0; i < STATE_DIMENSION; ++i) {
dYdC[i][i] = 1.0;
}
}
return dYdC;
......@@ -97,11 +159,15 @@ public class MatricesMapper implements MatricesHarvester {
@Override
public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {
if (stmName == null || !state.hasAdditionalState(stmName)) {
return null;
}
// get the conversion Jacobian
final double[][] dYdC = getConversionJacobian(state);
// extract the additional state
final double[] p = state.getAdditionalState(name);
final double[] p = state.getAdditionalState(stmName);
// compute dYdY0 = dYdC * dCdY0
final RealMatrix dYdY0 = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
......@@ -126,7 +192,7 @@ public class MatricesMapper implements MatricesHarvester {
@Override
public RealMatrix getParametersJacobian(final SpacecraftState state) {
if (columns.isEmpty()) {
if (columnsNames.isEmpty()) {
return null;
}
......@@ -134,9 +200,9 @@ public class MatricesMapper implements MatricesHarvester {
final double[][] dYdC = getConversionJacobian(state);
// compute dYdP = dYdC * dCdP
final RealMatrix dYdP = MatrixUtils.createRealMatrix(STATE_DIMENSION, columns.size());
for (int j = 0; j < columns.size(); j++) {
final double[] p = state.getAdditionalState(columns.get(j));
final RealMatrix dYdP = MatrixUtils.createRealMatrix(STATE_DIMENSION, columnsNames.size());
for (int j = 0; j < columnsNames.size(); j++) {
final double[] p = state.getAdditionalState(columnsNames.get(j));
for (int i = 0; i < STATE_DIMENSION; ++i) {
final double[] dYdCi = dYdC[i];
double sum = 0;
......@@ -151,4 +217,10 @@ public class MatricesMapper implements MatricesHarvester {
}
/** {@inheritDoc} */
@Override
public List<String> getJacobiansColumnsNames() {
return Collections.unmodifiableList(columnsNames);
}
}
......@@ -20,8 +20,10 @@ import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;
import java.util.stream.Collectors;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.ode.ODEIntegrator;
import org.hipparchus.util.FastMath;
import org.orekit.annotation.DefaultDataContext;
......@@ -39,16 +41,20 @@ import org.orekit.frames.Frame;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.MatricesHarvester;
import org.orekit.propagation.PropagationType;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.events.EventDetector;
import org.orekit.propagation.integration.AbstractIntegratedPropagator;
import org.orekit.propagation.integration.IntegrableGenerator;
import org.orekit.propagation.integration.StateMapper;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.AbsolutePVCoordinates;
import org.orekit.utils.DoubleArrayDictionary;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversList;
import org.orekit.utils.ParameterObserver;
import org.orekit.utils.TimeStampedPVCoordinates;
......@@ -69,13 +75,16 @@ import org.orekit.utils.TimeStampedPVCoordinates;
* <li>the various force models ({@link #addForceModel(ForceModel)},
* {@link #removeForceModels()})</li>
* <li>the {@link OrbitType type} of orbital parameters to be used for propagation
* ({@link #setOrbitType(OrbitType)}),
* ({@link #setOrbitType(OrbitType)}),</li>
* <li>the {@link PositionAngle type} of position angle to be used in orbital parameters
* to be used for propagation where it is relevant ({@link
* #setPositionAngleType(PositionAngle)}),
* <li>whether {@link org.orekit.propagation.integration.AdditionalEquations additional equations}
* (for example {@link PartialDerivativesEquations Jacobians}) should be propagated along with orbital state
* ({@link #addAdditionalEquations(org.orekit.propagation.integration.AdditionalEquations)}),
* #setPositionAngleType(PositionAngle)}),</li>
* <li>whether {@link MatricesHarvester state transition matrices and Jacobians matrices}
* should be propagated along with orbital state ({@link
* #getMatricesHarvester(String, RealMatrix, DoubleArrayDictionary)}),</li>
* <li>whether {@link org.orekit.propagation.integration.IntegrableGenerator integrable generators}
* should be propagated along with orbital state ({@link
* #addIntegrableGenerator(org.orekit.propagation.integration.IntegrableGenerator)}),</li>
* <li>the discrete events that should be triggered during propagation
* ({@link #addEventDetector(EventDetector)},
* {@link #clearEventsDetectors()})</li>
......@@ -107,7 +116,7 @@ import org.orekit.utils.TimeStampedPVCoordinates;
* <li>the {@link org.orekit.orbits.CartesianOrbit Cartesian orbit parameters} (x, y, z, v<sub>x</sub>,
* v<sub>y</sub>, v<sub>z</sub>) in meters and meters per seconds.
* </ul>
* <p> The last element is the mass in kilograms.
* <p> The last element is the mass in kilograms and changes only during thrusters firings
*
* <p>The following code snippet shows a typical setting for Low Earth Orbit propagation in
* equinoctial parameters and true longitude argument:</p>
......@@ -147,7 +156,10 @@ public class NumericalPropagator extends AbstractIntegratedPropagator {
private final List<ForceModel> forceModels;
/** boolean to ignore or not the creation of a NewtonianAttraction. */
private boolean ignoreCentralAttraction = false;
private boolean ignoreCentralAttraction;
/** Harvester for State Transition Matrix and Jacobian matrix. */
private NumericalPropagationHarvester harvester;
/** Create a new instance of NumericalPropagator, based on orbit definition mu.
* After creation, the instance is empty, i.e. the attitude provider is set to an
......@@ -186,7 +198,9 @@ public class NumericalPropagator extends AbstractIntegratedPropagator {
public NumericalPropagator(final ODEIntegrator integrator,
final AttitudeProvider attitudeProvider) {
super(integrator, PropagationType.MEAN);
forceModels = new ArrayList<ForceModel>();
forceModels = new ArrayList<ForceModel>();
ignoreCentralAttraction = false;
harvester = null;
initMapper();
setAttitudeProvider(attitudeProvider);
clearStepHandlers();
......@@ -367,12 +381,96 @@ public class NumericalPropagator extends AbstractIntegratedPropagator {
setStartDate(state.getDate());
}
/** Set up computation of State Transition Matrix and Jacobians matrix with respect to parameters.
* <p>
* If this method is called, both State Transition Matrix and Jacobians with respect to the
* force models parameters that will be selected when propagation starts will be automatically
* computed, and the harvester will allow to retrieve them.
* </p>
* <p>
* The arguments for initial matrices <em>must</em> be compatible with the {@link #setOrbitType(OrbitType) orbit type}
* and {@link #setPositionAngleType(PositionAngle) position angle} that will be ultimately
* selected when propagation starts
* </p>
* @param stmName State Transition Matrix state name
* @param initialStm initial State Transition Matrix ∂Y/∂Y₀,
* if null (which is the most frequent case), assumed to be 6x6 identity
* @param initialJacobianColumns initial columns of the Jacobians matrix with respect to parameters,
* if null or if some selected parameters are missing from the dictionary, the corresponding
* initial column is assumed to be 0
* @return harvester to retrieve computed matrices during and after propagation
* @since 11.1
*/
public MatricesHarvester getMatricesHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
harvester = new NumericalPropagationHarvester(stmName, initialStm, initialJacobianColumns);
return harvester;
}
/** {@inheritDoc} */
@Override
protected void setUpStmAndJacobianHandling() {
if (harvester != null) {
for (final IntegrableGenerator generator : getIntegrableGenerators()) {
if (generator.getName().equals(harvester.getStmName())) {
// we have already set up the generators in a previous propagation
return;
}
}
// add the STM generator corresponding to the current settings, and setup state accordingly
final StateTransitionMatrixGenerator stmGenerator =
new StateTransitionMatrixGenerator(harvester.getStmName(), getAllForceModels(), getAttitudeProvider());
setInitialState(stmGenerator.setInitialStateTransitionMatrix(getInitialState(),
harvester.getInitialStateTransitionMatrix(),
getOrbitType(),
getPositionAngleType()));
addIntegrableGenerator(stmGenerator);
// first pass: gather all parameters, binding similar names together
final ParameterDriversList selected = new ParameterDriversList();
for (final ForceModel provider : getAllForceModels()) {
for (final ParameterDriver driver : provider.getParametersDrivers()) {
selected.add(driver);
}
}
// second pass: now that shared parameter names are bound together,
// their selections status have been synchronized, we can filter them
selected.filter(true);
// third pass: sort parameters lexicographically
selected.sort();
for (final ParameterDriver driver : selected.getDrivers()) {
// add the Jacobians column generator corresponding to this parameter, and setup state accordingly
final JacobianColumnGenerator generator =
new JacobianColumnGenerator(stmGenerator, driver.getName());
setInitialState(generator.setInitialColumn(getInitialState(),
harvester.getInitialJacobianColumn(driver.getName()),
getOrbitType(),
getPositionAngleType()));
addIntegrableGenerator(generator);
}
harvester.setColumnsNames(selected.getDrivers().stream().map(d -> d.getName()).collect(Collectors.toList()));
harvester.setOrbitType(getOrbitType());
harvester.setPositionAngleType(getPositionAngleType());
}
}
/** {@inheritDoc} */
@Override
public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
return propagate(date).getPVCoordinates(frame);
}
/** {@inheritDoc} */
@Override
protected StateMapper createMapper(final AbsoluteDate referenceDate, final double mu,
final OrbitType orbitType, final PositionAngle positionAngleType,
final AttitudeProvider attitudeProvider, final Frame frame) {
......
......@@ -126,19 +126,9 @@ public class JacobianColumnGeneratorTest {
}
}
StateTransitionMatrixGenerator stmGenerator =
new StateTransitionMatrixGenerator("stm",
propagator.getAllForceModels(),
propagator.getAttitudeProvider());
JacobianColumnGenerator columnGenerator = new JacobianColumnGenerator(stmGenerator, selected.getName());
propagator.addIntegrableGenerator(columnGenerator);
propagator.addIntegrableGenerator(stmGenerator);
SpacecraftState initialState = new SpacecraftState(initialOrbit);
initialState = stmGenerator.setInitialStateTransitionMatrix(initialState, null, orbitType, angleType);
initialState = columnGenerator.setInitialColumn(initialState, null, orbitType, angleType);
propagator.setInitialState(initialState);
PickUpHandler pickUp = new PickUpHandler(stmGenerator, orbitType, angleType, null, null, selected.getName());
PickUpHandler pickUp = new PickUpHandler(propagator, null, null, selected.getName());
propagator.setStepHandler(pickUp);
propagator.propagate(initialState.getDate().shiftedBy(dt));
RealMatrix dYdP = pickUp.getdYdP();
......
......@@ -16,21 +16,19 @@
*/
package org.orekit.propagation.numerical;
import java.util.Collections;
import org.hipparchus.linear.RealMatrix;
import org.junit.Assert;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.MatricesHarvester;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.integration.IntegrableGenerator;
import org.orekit.propagation.sampling.OrekitStepHandler;
import org.orekit.propagation.sampling.OrekitStepInterpolator;
import org.orekit.time.AbsoluteDate;
class PickUpHandler implements OrekitStepHandler {
private final StateTransitionMatrixGenerator stmGenerator;
private final MatricesMapper mapper;
private final NumericalPropagator propagator;
private final MatricesHarvester harvester;
private final AbsoluteDate pickUpDate;
private final String accParamName;
private final String columnName;
......@@ -38,19 +36,18 @@ class PickUpHandler implements OrekitStepHandler {
private RealMatrix dYdY0;
private RealMatrix dYdP;
private double[] accPartial;
private StateTransitionMatrixGenerator stmGenerator;
public PickUpHandler(final StateTransitionMatrixGenerator stmGenerator, final OrbitType orbitType,
final PositionAngle angleType, final AbsoluteDate pickUpDate,
public PickUpHandler(final NumericalPropagator propagator, final AbsoluteDate pickUpDate,
final String accParamName, final String columnName) {
this.stmGenerator = stmGenerator;
this.mapper = new MatricesMapper(stmGenerator.getName(),
columnName == null ? Collections.emptyList() : Collections.singletonList(columnName),
orbitType, angleType);
this.propagator = propagator;
this.harvester = propagator.getMatricesHarvester("stm", null, null);
this.pickUpDate = pickUpDate;
this.accParamName = accParamName;
this.columnName = columnName;
this.s0 = null;
this.accPartial = null;
Assert.assertTrue(harvester.getJacobiansColumnsNames().isEmpty());
}
public SpacecraftState getState() {
......@@ -69,6 +66,16 @@ class PickUpHandler implements OrekitStepHandler {
return accPartial.clone();
}
public void init(SpacecraftState s0, AbsoluteDate t) {
// as the generators are only created on the fly at propagation start
// we retrieve the STM generator here
for (final IntegrableGenerator generator : propagator.getIntegrableGenerators()) {
if (generator instanceof StateTransitionMatrixGenerator) {
stmGenerator = (StateTransitionMatrixGenerator) generator;
}
}
}
public void handleStep(OrekitStepInterpolator interpolator) {
if (pickUpDate != null) {
// we want to pick up some intermediate Jacobians
......@@ -91,13 +98,11 @@ class PickUpHandler implements OrekitStepHandler {
private void checkState(final SpacecraftState state) {
Assert.assertEquals(columnName == null ? 1 : 2, state.getAdditionalStatesValues().size());
dYdY0 = mapper.getStateTransitionMatrix(state);
dYdY0 = harvester.getStateTransitionMatrix(state);
if (accParamName != null) {
accPartial = stmGenerator.getAccelerationPartials(accParamName);
}
if (columnName != null) {
dYdP = mapper.getParametersJacobian(state);
}
dYdP = harvester.getParametersJacobian(state); // may be null
s0 = state;
}
......