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

Use the new STM and Jacobians API in DSST.

This is a partial implementation for issue #855.
parent 590c03fd
......@@ -70,8 +70,8 @@
<action dev="luc" type="add" issue="856">
Added a new and simpler API for State Transition Matrix and Jacobian
matrix computation. This new API is for now only used with NumericalPropagator
(both in batch least squares and in Kalman filter), but it is expected to be
generalized to semi-analytical and analytical propagators as well when it is
and DSSTPropagator (both in batch least squares and in Kalman filter), but it
is expected to be generalized to analytical propagators as well when it is
stabilized.
</action>
<action dev="luc" type="add" >
......
......@@ -73,6 +73,12 @@ public class DSSTBatchLSModel extends AbstractBatchLSModel {
this.stateType = stateType;
}
/** {@inheritDoc} */
@Override
protected MatricesHarvester configureHarvester(final Propagator propagator) {
return ((DSSTPropagator) propagator).setupMatricesComputation(STM_NAME, null, null);
}
/** {@inheritDoc} */
@Override
@Deprecated
......
......@@ -21,10 +21,8 @@ import java.util.List;
import org.orekit.propagation.MatricesHarvester;
import org.orekit.propagation.PropagationType;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.conversion.OrbitDeterminationPropagatorBuilder;
import org.orekit.propagation.semianalytical.dsst.DSSTJacobiansMapper;
import org.orekit.propagation.semianalytical.dsst.DSSTPartialDerivativesEquations;
import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
import org.orekit.utils.ParameterDriversList;
......@@ -75,13 +73,7 @@ public class DSSTKalmanModel 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;
final DSSTPartialDerivativesEquations pde = new DSSTPartialDerivativesEquations(equationName, (DSSTPropagator) getReferenceTrajectories()[k], pType);
// Reset the Jacobians
final SpacecraftState rawState = getReferenceTrajectories()[k].getInitialState();
final SpacecraftState stateWithDerivatives = pde.setInitialJacobians(rawState);
((DSSTPropagator) getReferenceTrajectories()[k]).setInitialState(stateWithDerivatives, sType);
harvesters[k] = pde.getMapper();
harvesters[k] = ((DSSTPropagator) getReferenceTrajectories()[k]).setupMatricesComputation(equationName, null, null);
}
// Update Jacobian harvesters
......
/* Copyright 2002-2021 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;
import java.util.List;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.orekit.utils.DoubleArrayDictionary;
/** Base harvester between two-dimensional Jacobian matrices and one-dimensional {@link
* SpacecraftState#getAdditionalState(String) additional state arrays}.
* @author Luc Maisonobe
* @since 11.1
*/
public abstract class AbstractMatricesHarvester implements MatricesHarvester {
/** State dimension, fixed to 6. */
public static final int STATE_DIMENSION = 6;
/** Identity conversion matrix. */
private static final double[][] IDENTITY = {
{ 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
{ 0.0, 1.0, 0.0, 0.0, 0.0, 0.0 },
{ 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 },
{ 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 },
{ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 },
{ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 }
};
/** Initial State Transition Matrix. */
private final RealMatrix initialStm;
/** Initial columns of the Jacobians matrix with respect to parameters. */
private final DoubleArrayDictionary initialJacobianColumns;
/** State Transition Matrix state name. */
private final String stmName;
/** 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 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 AbstractMatricesHarvester(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;
}
/** Get the State Transition Matrix state name.
* @return State Transition Matrix state name
*/
public String getStmName() {
return stmName;
}
/** Get the initial State Transition Matrix.
* @return initial State Transition Matrix
*/
public 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
*/
public 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.
* <p>
* The base implementation returns identity, which is suitable for DSST and TLE propagators,
* as state parameters and parameters used for derivatives are the same.
* </p>
* <p>
* For Numerical propagator, parameters used for derivatives are Cartesian
* and they can be different from state parameters because the numerical propagator can accept different type
* of orbits, so the method is overridden in derived classes.
* </p>
* @param state spacecraft state
* @return conversion Jacobian
*/
protected double[][] getConversionJacobian(final SpacecraftState state) {
return IDENTITY;
}
/** {@inheritDoc} */
@Override
public void setReferenceState(final SpacecraftState reference) {
// nothing to do
}
/** {@inheritDoc} */
@Override
public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {
if (!state.hasAdditionalState(stmName)) {
return null;
}
// get the conversion Jacobian
final double[][] dYdC = getConversionJacobian(state);
// extract the additional state
final double[] p = state.getAdditionalState(stmName);
// compute dYdY0 = dYdC * dCdY0
final RealMatrix dYdY0 = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
for (int i = 0; i < STATE_DIMENSION; i++) {
final double[] rowC = dYdC[i];
for (int j = 0; j < STATE_DIMENSION; ++j) {
double sum = 0;
int pIndex = j;
for (int k = 0; k < STATE_DIMENSION; ++k) {
sum += rowC[k] * p[pIndex];
pIndex += STATE_DIMENSION;
}
dYdY0.setEntry(i, j, sum);
}
}
return dYdY0;
}
/** {@inheritDoc} */
@Override
public RealMatrix getParametersJacobian(final SpacecraftState state) {
final List<String> columnsNames = getJacobiansColumnsNames();
if (columnsNames == null || columnsNames.isEmpty()) {
return null;
}
// get the conversion Jacobian
final double[][] dYdC = getConversionJacobian(state);
// compute dYdP = dYdC * dCdP
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;
for (int k = 0; k < STATE_DIMENSION; ++k) {
sum += dYdCi[k] * p[k];
}
dYdP.setEntry(i, j, sum);
}
}
return dYdP;
}
/** Freeze the names of the Jacobian columns.
* <p>
* This method is called when propagation starts, i.e. when configuration is completed
* </p>
*/
public abstract void freezeColumnsNames();
}
/* Copyright 2002-2021 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;
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;
/** Base generator for one column of a Jacobian matrix.
* <p>
* Specialized versions of this class are used when estimating
* parameters (for example propagation parameters like drag coefficient,
* thrust or maneuver start date) in orbit determination or maneuver
* optimization.
* </p>
* <p>
* Some implementations may be based on closed-form expressions and
* will implement the {@link AdditionalStateProvider} interface whereas
* otherimplementations may be based on differential equations and will
* implement the {@link
* org.orekit.propagation.integration.AdditionalDerivativesProvider
* AdditionalDerivativesProvider} interface). This base class therefore
* does not specify how the column is generated and put into the state.
* </p>
* @author Luc Maisonobe
* @since 11.1
*/
public class BaseJacobianColumnGenerator {
/** Space dimension. */
private static final int SPACE_DIMENSION = 3;
/** State dimension. */
private static final int STATE_DIMENSION = 2 * SPACE_DIMENSION;
/** Threshold for matrix solving. */
private static final double THRESHOLD = Precision.SAFE_MIN;
/** Name of the parameter corresponding to the column. */
private final String columnName;
/** Simple constructor.
* @param columnName name of the parameter corresponding to the column
*/
protected BaseJacobianColumnGenerator(final String columnName) {
this.columnName = columnName;
}
/** Get the name of the additional state.
* @return name of the additional state (names containing "orekit"
* with any case are reserved for the library internal use)
*/
public String getName() {
return columnName;
}
/** Get the dimension of the generated column.
* @return dimension of the generated column
*/
public int getDimension() {
return STATE_DIMENSION;
}
/** Set the initial value of the column.
* <p>
* The returned state must be added to the propagator.
* </p>
* @param state initial state (must already contain the Cartesian State Transition Matrix)
* @param dYdQ column of the Jacobian ∂Y/∂qₘ for the {@link #getName()} named parameter},
* if null (which is the most frequent case), assumed to be 0
* @param orbitType orbit type used for states Y and Y₀ in {@code dYdQ}
* @param positionAngle position angle used states Y and Y₀ in {@code dYdQ}
* @return state with Jacobian column (converted to Cartesian ∂C/∂Q) added
*/
public SpacecraftState setInitialColumn(final SpacecraftState state, final double[] dYdQ,
final OrbitType orbitType, final PositionAngle positionAngle) {
final double[] column;
if (dYdQ == null) {
// initial Jacobian is null (this is the most frequent case)
column = new double[STATE_DIMENSION];
} else {
if (dYdQ.length != STATE_DIMENSION) {
throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
dYdQ.length, STATE_DIMENSION);
}
// convert to Cartesian Jacobian
final double[][] dYdC = new double[STATE_DIMENSION][STATE_DIMENSION];
orbitType.convertType(state.getOrbit()).getJacobianWrtCartesian(positionAngle, dYdC);
column = new QRDecomposition(MatrixUtils.createRealMatrix(dYdC), THRESHOLD).
getSolver().
solve(MatrixUtils.createRealVector(dYdQ)).
toArray();
}
// set additional state
return state.addAdditionalState(columnName, column);
}
}
......@@ -73,7 +73,6 @@ import org.orekit.forces.maneuvers.trigger.ManeuverTriggersResetter;
* @since 11.1
*/
public class TriggerDateJacobianColumnGenerator
extends BaseJacobianColumnGenerator
implements AdditionalStateProvider, ManeuverTriggersResetter {
/** Dimension of the state. */
......@@ -88,6 +87,9 @@ public class TriggerDateJacobianColumnGenerator
/** Name of the state for State Transition Matrix. */
private final String stmName;
/** Name of the parameter corresponding to the column. */
private final String columnName;
/** Start/stop management flag. */
private boolean manageStart;
......@@ -116,14 +118,20 @@ public class TriggerDateJacobianColumnGenerator
public TriggerDateJacobianColumnGenerator(final String stmName, final String columnName,
final boolean manageStart, final Maneuver maneuver,
final double threshold) {
super(columnName);
this.stmName = stmName;
this.columnName = columnName;
this.manageStart = manageStart;
this.maneuver = maneuver;
this.sign = manageStart ? -1 : +1;
this.threshold = threshold;
}
/** {@inheritDoc} */
@Override
public String getName() {
return columnName;
}
/** {@inheritDoc}
* <p>
* The column state can be computed only if the State Transition Matrix state is available.
......
......@@ -27,6 +27,7 @@ import java.util.Map;
import java.util.Queue;
import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.ode.DenseOutputModel;
import org.hipparchus.ode.ExpandableODE;
import org.hipparchus.ode.ODEIntegrator;
......@@ -48,9 +49,11 @@ import org.orekit.errors.OrekitMessages;
import org.orekit.frames.Frame;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.AbstractMatricesHarvester;
import org.orekit.propagation.AbstractPropagator;
import org.orekit.propagation.BoundedPropagator;
import org.orekit.propagation.EphemerisGenerator;
import org.orekit.propagation.MatricesHarvester;
import org.orekit.propagation.PropagationType;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.events.EventDetector;
......@@ -108,6 +111,9 @@ public abstract class AbstractIntegratedPropagator extends AbstractPropagator {
*/
private PropagationType propagationType;
/** Harvester for State Transition Matrix and Jacobian matrix. */
private AbstractMatricesHarvester harvester;
/** Build a new instance.
* @param integrator numerical integrator to use for propagation.
* @param propagationType type of orbit to output (mean or osculating).
......@@ -120,6 +126,7 @@ public abstract class AbstractIntegratedPropagator extends AbstractPropagator {
this.integrator = integrator;
this.propagationType = propagationType;
this.resetAtEnd = true;
this.harvester = null;
}
/** Allow/disallow resetting the initial state at end of propagation.
......@@ -174,8 +181,18 @@ public abstract class AbstractIntegratedPropagator extends AbstractPropagator {
/** Check if only the mean elements should be used in a semianalitical propagation.
* @return {@link PropagationType MEAN} if only mean elements have to be used or
* {@link PropagationType OSCULATING} if osculating elements have to be also used.
* @deprecated as of 11.1, replaced by {@link #getPropagationType()}
*/
@Deprecated
protected PropagationType isMeanOrbit() {
return getPropagationType();
}
/** Get the propagation type.
* @return propagation type.
* @since 11.1
*/
public PropagationType getPropagationType() {
return propagationType;
}
......@@ -296,6 +313,37 @@ public abstract class AbstractIntegratedPropagator extends AbstractPropagator {
return Collections.unmodifiableList(additionalDerivativesProviders);
}
/** {@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 abstract AbstractMatricesHarvester createHarvester(String stmName, RealMatrix initialStm,
DoubleArrayDictionary initialJacobianColumns);
/** Get the harvester.
* @return harvester, or null if it was not created
*/
protected AbstractMatricesHarvester getHarvester() {
return harvester;
}
/** {@inheritDoc} */
public void addEventDetector(final EventDetector detector) {
detectors.add(detector);
......
......@@ -19,6 +19,7 @@ package org.orekit.propagation.numerical;
import java.util.Arrays;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.ode.ODEIntegrator;
import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaIntegrator;
import org.hipparchus.util.FastMath;
......@@ -34,6 +35,7 @@ import org.orekit.orbits.CartesianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.AbstractMatricesHarvester;
import org.orekit.propagation.PropagationType;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.analytical.gnss.data.GLONASSOrbitalElements;
......@@ -44,6 +46,7 @@ import org.orekit.time.AbsoluteDate;
import org.orekit.time.GLONASSDate;
import org.orekit.utils.AbsolutePVCoordinates;
import org.orekit.utils.Constants;
import org.orekit.utils.DoubleArrayDictionary;
import org.orekit.utils.IERSConventions;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
......@@ -779,4 +782,12 @@ public class GLONASSNumericalPropagator extends AbstractIntegratedPropagator {
}
/** {@inheritDoc} */
@Override
protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
// FIXME: not implemented as of 11.1
throw new UnsupportedOperationException();
}
}
......@@ -16,7 +16,6 @@
*/
package org.orekit.propagation.numerical;
import org.orekit.propagation.BaseJacobianColumnGenerator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.integration.AdditionalDerivativesProvider;
......@@ -31,12 +30,14 @@ import org.orekit.propagation.integration.AdditionalDerivativesProvider;
* @since 11.1
*/
class IntegrableJacobianColumnGenerator
extends BaseJacobianColumnGenerator
implements AdditionalDerivativesProvider, StateTransitionMatrixGenerator.PartialsObserver {
/** Name of the state for State Transition Matrix. */
private final String stmName;
/** Name of the parameter corresponding to the column. */
private final String columnName;
/** Last value computed for the partial derivatives. */
private final double[] pDot;
......@@ -50,12 +51,25 @@ class IntegrableJacobianColumnGenerator
* @param columnName name of the parameter corresponding to the column
*/
IntegrableJacobianColumnGenerator(final StateTransitionMatrixGenerator stmGenerator, final String columnName) {
super(columnName);
this.stmName = stmGenerator.getName();
this.columnName = columnName;
this.pDot = new double[getDimension()];
stmGenerator.addObserver(columnName, this);
}
/** {@inheritDoc} */
@Override
public String getName() {
return columnName;
}
/** Get the dimension of the generated column.
* @return dimension of the generated column
*/
public int getDimension() {
return 6;
}
/** {@inheritDoc}
* <p>
* The column derivative can be computed only if the State Transition Matrix derivatives
......