Commit c41574b8 authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

Merge branch 'analytical-od' into 'develop'

Analytical Orbit Determination

See merge request !230
parents 04d98077 aa1cbbb7
Pipeline #1671 failed with stages
in 29 minutes and 12 seconds
......@@ -23,7 +23,7 @@
<release version="11.1" date="TBD" description="TBD">
<action dev="bryan" type="add">
Added a new and simpler API for State Transition Matrix and Jacobian
matrix computation for TLEPropagator.
matrix computation for analytical orbit propagators.
</action>
<action dev="bryan" type="fix" issue="878">
Fixed writing of ITRF frames before 2000 when generating CCSDS files.
......
......@@ -56,7 +56,7 @@ public class BatchLSModel extends AbstractBatchLSModel {
/** {@inheritDoc} */
@Override
protected MatricesHarvester configureHarvester(final Propagator propagator) {
return ((NumericalPropagator) propagator).setupMatricesComputation(STM_NAME, null, null);
return propagator.setupMatricesComputation(STM_NAME, null, null);
}
/** {@inheritDoc} */
......
......@@ -75,7 +75,7 @@ public class DSSTBatchLSModel extends AbstractBatchLSModel {
/** {@inheritDoc} */
@Override
protected MatricesHarvester configureHarvester(final Propagator propagator) {
return ((DSSTPropagator) propagator).setupMatricesComputation(STM_NAME, null, null);
return propagator.setupMatricesComputation(STM_NAME, null, null);
}
/** {@inheritDoc} */
......
......@@ -39,8 +39,9 @@ import org.orekit.utils.ParameterDriversList;
* @author Bryan Cazabonne
* @author Thomas Paulet
* @since 11.0
*
* @deprecated as of 11.1, replaced by {@link AnalyticalBatchLSModel}
*/
@Deprecated
public class TLEBatchLSModel extends AbstractBatchLSModel {
/** Name of the State Transition Matrix state. */
......
......@@ -23,7 +23,6 @@ import org.orekit.propagation.PropagationType;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.conversion.OrbitDeterminationPropagatorBuilder;
import org.orekit.propagation.numerical.JacobiansMapper;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.utils.ParameterDriversList;
/** Class defining the process model dynamics to use with a {@link KalmanEstimator}.
......@@ -63,7 +62,7 @@ public class KalmanModel extends AbstractKalmanModel {
for (int k = 0; k < propagators.length; ++k) {
// Link the partial derivatives to this new propagator
final String equationName = KalmanEstimator.class.getName() + "-derivatives-" + k;
harvesters[k] = ((NumericalPropagator) getReferenceTrajectories()[k]).setupMatricesComputation(equationName, null, null);
harvesters[k] = getReferenceTrajectories()[k].setupMatricesComputation(equationName, null, null);
}
// Update Jacobian harvesters
......
......@@ -37,7 +37,9 @@ import org.orekit.utils.ParameterDriversList;
* @author Bryan Cazabonne
* @author Thomas Paulet
* @since 11.0
* @deprecated as of 11.1, replaced by {@link KalmanModel}
*/
@Deprecated
public class TLEKalmanModel extends AbstractKalmanModel {
/** Kalman process model constructor (package private).
......
......@@ -24,6 +24,7 @@ import java.util.List;
import java.util.Map;
import java.util.Queue;
import org.hipparchus.linear.RealMatrix;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
......@@ -62,12 +63,16 @@ public abstract class AbstractPropagator implements Propagator {
/** Initial state. */
private SpacecraftState initialState;
/** Harvester for State Transition Matrix and Jacobian matrix. */
private AbstractMatricesHarvester harvester;
/** Build a new instance.
*/
protected AbstractPropagator() {
multiplexer = new StepHandlerMultiplexer();
multiplexer = new StepHandlerMultiplexer();
additionalStateProviders = new ArrayList<>();
unmanagedStates = new HashMap<>();
unmanagedStates = new HashMap<>();
harvester = null;
}
/** Set a start date.
......@@ -137,6 +142,41 @@ public abstract class AbstractPropagator implements Propagator {
return Collections.unmodifiableList(additionalStateProviders);
}
/** {@inheritDoc} */
@Override
public MatricesHarvester setupMatricesComputation(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
if (stmName == null) {
throw new OrekitException(OrekitMessages.NULL_ARGUMENT, "stmName");
}
harvester = createHarvester(stmName, initialStm, initialJacobianColumns);
return harvester;
}
/** Create the harvester suitable for propagator.
* @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
*/
protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
// FIXME: not implemented as of 11.1
throw new UnsupportedOperationException();
}
/** Get the harvester.
* @return harvester, or null if it was not created
* @since 11.1
*/
protected AbstractMatricesHarvester getHarvester() {
return harvester;
}
/** Update state by adding unmanaged states.
* @param original original state
* @return updated state, with unmanaged states included
......
/* Copyright 2002-2022 CS GROUP
* 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.propagation.analytical;
import java.util.ArrayList;
import java.util.List;
import org.hipparchus.analysis.differentiation.Gradient;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.FieldAttitude;
import org.orekit.orbits.FieldCartesianOrbit;
import org.orekit.orbits.FieldOrbit;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.integration.AbstractGradientConverter;
import org.orekit.utils.FieldAngularCoordinates;
import org.orekit.utils.FieldPVCoordinates;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
/**
* Converter for analytical orbit propagator.
*
* @author Bryan Cazabonne
* @since 11.1
*/
public abstract class AbstractAnalyticalGradientConverter extends AbstractGradientConverter {
/** Attitude provider. */
private final AttitudeProvider provider;
/** States with various number of additional propagation parameters. */
private final List<FieldSpacecraftState<Gradient>> gStates;
/**
* Constructor.
* @param propagator analytical orbit propagator
* @param mu central attraction coefficient
* @param freeStateParameters number of free parameters
*/
protected AbstractAnalyticalGradientConverter(final AbstractAnalyticalPropagator propagator,
final double mu,
final int freeStateParameters) {
super(freeStateParameters);
// Attitude provider
this.provider = propagator.getAttitudeProvider();
// Spacecraft state
final SpacecraftState state = propagator.getInitialState();
// Position always has derivatives
final Vector3D pos = state.getPVCoordinates().getPosition();
final FieldVector3D<Gradient> posG = new FieldVector3D<>(Gradient.variable(freeStateParameters, 0, pos.getX()),
Gradient.variable(freeStateParameters, 1, pos.getY()),
Gradient.variable(freeStateParameters, 2, pos.getZ()));
// Velocity may have derivatives or not
final Vector3D vel = state.getPVCoordinates().getVelocity();
final FieldVector3D<Gradient> velG = new FieldVector3D<>(Gradient.variable(freeStateParameters, 3, vel.getX()),
Gradient.variable(freeStateParameters, 4, vel.getY()),
Gradient.variable(freeStateParameters, 5, vel.getZ()));
// Acceleration never has derivatives
final Vector3D acc = state.getPVCoordinates().getAcceleration();
final FieldVector3D<Gradient> accG = new FieldVector3D<>(Gradient.constant(freeStateParameters, acc.getX()),
Gradient.constant(freeStateParameters, acc.getY()),
Gradient.constant(freeStateParameters, acc.getZ()));
// Mass never has derivatives
final Gradient gM = Gradient.constant(freeStateParameters, state.getMass());
final Gradient gMu = Gradient.constant(freeStateParameters, mu);
final FieldOrbit<Gradient> gOrbit =
new FieldCartesianOrbit<>(new TimeStampedFieldPVCoordinates<>(state.getDate(), posG, velG, accG),
state.getFrame(), gMu);
// Attitude
final FieldAttitude<Gradient> gAttitude = provider.getAttitude(gOrbit, gOrbit.getDate(), gOrbit.getFrame());
// Initialize the list with the state having 0 force model parameters
gStates = new ArrayList<>();
gStates.add(new FieldSpacecraftState<>(gOrbit, gAttitude, gM));
}
/** Get the state with the number of parameters consistent with the propagation model.
* @return state with the number of parameters consistent with the propagation model
*/
public FieldSpacecraftState<Gradient> getState() {
// Count the required number of parameters
int nbParams = 0;
for (final ParameterDriver driver : getParametersDrivers()) {
if (driver.isSelected()) {
++nbParams;
}
}
// Fill in intermediate slots
while (gStates.size() < nbParams + 1) {
gStates.add(null);
}
if (gStates.get(nbParams) == null) {
// It is the first time we need this number of parameters
// We need to create the state
final int freeParameters = getFreeStateParameters() + nbParams;
final FieldSpacecraftState<Gradient> s0 = gStates.get(0);
// Orbit
final FieldPVCoordinates<Gradient> pv0 = s0.getPVCoordinates();
final FieldOrbit<Gradient> gOrbit =
new FieldCartesianOrbit<>(new TimeStampedFieldPVCoordinates<>(s0.getDate().toAbsoluteDate(),
extend(pv0.getPosition(), freeParameters),
extend(pv0.getVelocity(), freeParameters),
extend(pv0.getAcceleration(), freeParameters)),
s0.getFrame(),
extend(s0.getMu(), freeParameters));
// Attitude
final FieldAngularCoordinates<Gradient> ac0 = s0.getAttitude().getOrientation();
final FieldAttitude<Gradient> gAttitude =
new FieldAttitude<>(s0.getAttitude().getReferenceFrame(),
new TimeStampedFieldAngularCoordinates<>(gOrbit.getDate(),
extend(ac0.getRotation(), freeParameters),
extend(ac0.getRotationRate(), freeParameters),
extend(ac0.getRotationAcceleration(), freeParameters)));
// Mass
final Gradient gM = extend(s0.getMass(), freeParameters);
gStates.set(nbParams, new FieldSpacecraftState<>(gOrbit, gAttitude, gM));
}
return gStates.get(nbParams);
}
/** Get the model parameters.
* @param state state as returned by {@link #getState()}
* @return the model parameters
*/
public Gradient[] getParameters(final FieldSpacecraftState<Gradient> state) {
final int freeParameters = state.getMass().getFreeParameters();
final List<ParameterDriver> drivers = getParametersDrivers();
final Gradient[] parameters = new Gradient[drivers.size()];
int index = getFreeStateParameters();
int i = 0;
for (ParameterDriver driver : drivers) {
parameters[i++] = driver.isSelected() ?
Gradient.variable(freeParameters, index++, driver.getValue()) :
Gradient.constant(freeParameters, driver.getValue());
}
return parameters;
}
/**
* Get the parameter drivers related to the analytical propagation model.
* @return a list of parameter drivers
*/
public abstract List<ParameterDriver> getParametersDrivers();
/**
* Get the converted analytical orbit propagator.
* @param state state as returned by {@link #getState()}
* @param parameters model parameters as returned by {@link #getParameters(FieldSpacecraftState)}
* @return the converted analytical orbit propagator
*/
public abstract FieldAbstractAnalyticalPropagator<Gradient> getPropagator(FieldSpacecraftState<Gradient> state,
Gradient[] parameters);
}
/* Copyright 2002-2022 CS GROUP
* 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.propagation.analytical;
import java.util.Arrays;
import java.util.List;
import org.hipparchus.analysis.differentiation.Gradient;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.orekit.orbits.FieldOrbit;
import org.orekit.propagation.AbstractMatricesHarvester;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.SpacecraftState;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.DoubleArrayDictionary;
import org.orekit.utils.FieldPVCoordinates;
import org.orekit.utils.ParameterDriver;
/**
* Base class harvester between two-dimensional Jacobian
* matrices and analytical orbit propagator.
* @author Thomas Paulet
* @author Bryan Cazabonne
* @since 11.1
*/
public abstract class AbstractAnalyticalMatricesHarvester extends AbstractMatricesHarvester {
/** Columns names for parameters. */
private List<String> columnsNames;
/** Epoch of the last computed state transition matrix. */
private AbsoluteDate epoch;
/** Analytical derivatives that apply to State Transition Matrix. */
private final double[][] analyticalDerivativesStm;
/** Analytical derivatives that apply to Jacobians columns. */
private final DoubleArrayDictionary analyticalDerivativesJacobianColumns;
/** Propagator bound to this harvester. */
private final AbstractAnalyticalPropagator propagator;
/** Simple constructor.
* <p>
* The arguments for initial matrices <em>must</em> be compatible with the
* {@link org.orekit.orbits.OrbitType orbit type}
* and {@link org.orekit.orbits.PositionAngle position angle} that will be used by propagator
* </p>
* @param propagator propagator bound to this harvester
* @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
*/
protected AbstractAnalyticalMatricesHarvester(final AbstractAnalyticalPropagator propagator, final String stmName,
final RealMatrix initialStm, final DoubleArrayDictionary initialJacobianColumns) {
super(stmName, initialStm, initialJacobianColumns);
this.propagator = propagator;
this.epoch = propagator.getInitialState().getDate();
this.columnsNames = null;
this.analyticalDerivativesStm = getInitialStateTransitionMatrix().getData();
this.analyticalDerivativesJacobianColumns = new DoubleArrayDictionary();
}
/** {@inheritDoc} */
@Override
public List<String> getJacobiansColumnsNames() {
return columnsNames == null ? propagator.getJacobiansColumnsNames() : columnsNames;
}
/** {@inheritDoc} */
@Override
public void freezeColumnsNames() {
columnsNames = getJacobiansColumnsNames();
}
/** {@inheritDoc} */
@Override
public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {
// Update the partial derivatives if needed
updateDerivativesIfNeeded(state);
//return the state transition matrix
return MatrixUtils.createRealMatrix(analyticalDerivativesStm);
}
/** {@inheritDoc} */
@Override
public RealMatrix getParametersJacobian(final SpacecraftState state) {
// Update the partial derivatives if needed
updateDerivativesIfNeeded(state);
// Estimated parameters
final List<String> names = getJacobiansColumnsNames();
if (names == null || names.isEmpty()) {
return null;
}
// Initialize Jacobian
final RealMatrix dYdP = MatrixUtils.createRealMatrix(STATE_DIMENSION, names.size());
// Add the derivatives
for (int j = 0; j < names.size(); ++j) {
final double[] column = analyticalDerivativesJacobianColumns.get(names.get(j));
if (column != null) {
for (int i = 0; i < STATE_DIMENSION; i++) {
dYdP.addToEntry(i, j, column[i]);
}
}
}
// Return
return dYdP;
}
/** {@inheritDoc} */
@Override
public void setReferenceState(final SpacecraftState reference) {
// reset derivatives to zero
for (final double[] row : analyticalDerivativesStm) {
Arrays.fill(row, 0.0);
}
analyticalDerivativesJacobianColumns.clear();
final AbstractAnalyticalGradientConverter converter = getGradientConverter();
final FieldSpacecraftState<Gradient> gState = converter.getState();
final Gradient[] gParameters = converter.getParameters(gState);
final FieldAbstractAnalyticalPropagator<Gradient> gPropagator = converter.getPropagator(gState, gParameters);
// Compute Jacobian
final AbsoluteDate target = reference.getDate();
final FieldAbsoluteDate<Gradient> start = gPropagator.getInitialState().getDate();
final double dt = target.durationFrom(start.toAbsoluteDate());
final FieldOrbit<Gradient> gOrbit = gPropagator.propagateOrbit(start.shiftedBy(dt), gParameters);
final FieldPVCoordinates<Gradient> gPv = gOrbit.getPVCoordinates();
final double[] derivativesX = gPv.getPosition().getX().getGradient();
final double[] derivativesY = gPv.getPosition().getY().getGradient();
final double[] derivativesZ = gPv.getPosition().getZ().getGradient();
final double[] derivativesVx = gPv.getVelocity().getX().getGradient();
final double[] derivativesVy = gPv.getVelocity().getY().getGradient();
final double[] derivativesVz = gPv.getVelocity().getZ().getGradient();
// Update Jacobian with respect to state
addToRow(derivativesX, 0);
addToRow(derivativesY, 1);
addToRow(derivativesZ, 2);
addToRow(derivativesVx, 3);
addToRow(derivativesVy, 4);
addToRow(derivativesVz, 5);
// Partial derivatives of the state with respect to propagation parameters
int paramsIndex = converter.getFreeStateParameters();
for (ParameterDriver driver : converter.getParametersDrivers()) {
if (driver.isSelected()) {
// get the partials derivatives for this driver
DoubleArrayDictionary.Entry entry = analyticalDerivativesJacobianColumns.getEntry(driver.getName());
if (entry == null) {
// create an entry filled with zeroes
analyticalDerivativesJacobianColumns.put(driver.getName(), new double[STATE_DIMENSION]);
entry = analyticalDerivativesJacobianColumns.getEntry(driver.getName());
}
// add the contribution of the current force model
entry.increment(new double[] {
derivativesX[paramsIndex], derivativesY[paramsIndex], derivativesZ[paramsIndex],
derivativesVx[paramsIndex], derivativesVy[paramsIndex], derivativesVz[paramsIndex]
});
++paramsIndex;
}
}
// Update the epoch of the last computed partial derivatives
epoch = target;
}
/** Update the partial derivatives (if needed).
* @param state current spacecraft state
*/
private void updateDerivativesIfNeeded(final SpacecraftState state) {
if (!state.getDate().isEqualTo(epoch)) {
setReferenceState(state);
}
}
/** Fill State Transition Matrix rows.
* @param derivatives derivatives of a component
* @param index component index
*/
private void addToRow(final double[] derivatives, final int index) {
for (int i = 0; i < 6; i++) {
analyticalDerivativesStm[index][i] += derivatives[i];
}
}
/**
* Get the gradient converter related to the analytical orbit propagator.
* @return the gradient converter
*/
public abstract AbstractAnalyticalGradientConverter getGradientConverter();
}
......@@ -36,6 +36,7 @@ import org.orekit.propagation.AbstractPropagator;
import org.orekit.propagation.AdditionalStateProvider;
import org.orekit.propagation.BoundedPropagator;
import org.orekit.propagation.EphemerisGenerator;
import org.orekit.propagation.MatricesHarvester;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.events.EventDetector;
import org.orekit.propagation.events.EventState;
......@@ -378,6 +379,15 @@ public abstract class AbstractAnalyticalPropagator extends AbstractPropagator {
}
}
<