DSSTHarvester.java
/* Copyright 2002-2023 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.semianalytical.dsst;
import java.util.ArrayList;
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.OrbitType;
import org.orekit.orbits.PositionAngleType;
import org.orekit.propagation.AbstractMatricesHarvester;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.PropagationType;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
import org.orekit.propagation.semianalytical.dsst.forces.FieldShortPeriodTerms;
import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
import org.orekit.utils.DoubleArrayDictionary;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeSpanMap;
import org.orekit.utils.TimeSpanMap.Span;
/** Harvester between two-dimensional Jacobian matrices and one-dimensional {@link
* SpacecraftState#getAdditionalState(String) additional state arrays}.
* @author Luc Maisonobe
* @author Bryan Cazabonne
* @since 11.1
*/
public class DSSTHarvester extends AbstractMatricesHarvester {
/** Retrograde factor I.
* <p>
* DSST model needs equinoctial orbit as internal representation.
* Classical equinoctial elements have discontinuities when inclination
* is close to zero. In this representation, I = +1. <br>
* To avoid this discontinuity, another representation exists and equinoctial
* elements can be expressed in a different way, called "retrograde" orbit.
* This implies I = -1. <br>
* As Orekit doesn't implement the retrograde orbit, I is always set to +1.
* But for the sake of consistency with the theory, the retrograde factor
* has been kept in the formulas.
* </p>
*/
private static final int I = 1;
/** Propagator bound to this harvester. */
private final DSSTPropagator propagator;
/** Derivatives of the short period terms that apply to State Transition Matrix.*/
private final double[][] shortPeriodDerivativesStm;
/** Derivatives of the short period terms that apply to Jacobians columns. */
private final DoubleArrayDictionary shortPeriodDerivativesJacobianColumns;
/** Columns names for parameters. */
private List<String> columnsNames;
/** Field short periodic terms. */
private List<FieldShortPeriodTerms<Gradient>> fieldShortPeriodTerms;
/** Simple constructor.
* <p>
* The arguments for initial matrices <em>must</em> be compatible with the
* {@link org.orekit.orbits.OrbitType#EQUINOCTIAL equinoctial orbit type}
* and {@link PositionAngleType 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
*/
DSSTHarvester(final DSSTPropagator propagator, final String stmName,
final RealMatrix initialStm, final DoubleArrayDictionary initialJacobianColumns) {
super(stmName, initialStm, initialJacobianColumns);
this.propagator = propagator;
this.shortPeriodDerivativesStm = new double[STATE_DIMENSION][STATE_DIMENSION];
this.shortPeriodDerivativesJacobianColumns = new DoubleArrayDictionary();
this.fieldShortPeriodTerms = new ArrayList<>();
}
/** {@inheritDoc} */
@Override
public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {
final RealMatrix stm = super.getStateTransitionMatrix(state);
if (propagator.getPropagationType() == PropagationType.OSCULATING) {
// add the short period terms
for (int i = 0; i < STATE_DIMENSION; i++) {
for (int j = 0; j < STATE_DIMENSION; j++) {
stm.addToEntry(i, j, shortPeriodDerivativesStm[i][j]);
}
}
}
return stm;
}
/** {@inheritDoc} */
@Override
public RealMatrix getParametersJacobian(final SpacecraftState state) {
final RealMatrix jacobian = super.getParametersJacobian(state);
if (jacobian != null && propagator.getPropagationType() == PropagationType.OSCULATING) {
// add the short period terms
final List<String> names = getJacobiansColumnsNames();
for (int j = 0; j < names.size(); ++j) {
final double[] column = shortPeriodDerivativesJacobianColumns.get(names.get(j));
for (int i = 0; i < STATE_DIMENSION; i++) {
jacobian.addToEntry(i, j, column[i]);
}
}
}
return jacobian;
}
/** Get the Jacobian matrix B1 (B1 = ∂εη/∂Y).
* <p>
* B1 represents the partial derivatives of the short period motion
* with respect to the mean equinoctial elements.
* </p>
* @return the B1 jacobian matrix
*/
public RealMatrix getB1() {
// Initialize B1
final RealMatrix B1 = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
// add the short period terms
for (int i = 0; i < STATE_DIMENSION; i++) {
for (int j = 0; j < STATE_DIMENSION; j++) {
B1.addToEntry(i, j, shortPeriodDerivativesStm[i][j]);
}
}
// Return B1
return B1;
}
/** Get the Jacobian matrix B2 (B2 = ∂Y/∂Y₀).
* <p>
* B2 represents the partial derivatives of the mean equinoctial elements
* with respect to the initial ones.
* </p>
* @param state spacecraft state
* @return the B2 jacobian matrix
*/
public RealMatrix getB2(final SpacecraftState state) {
return super.getStateTransitionMatrix(state);
}
/** Get the Jacobian matrix B3 (B3 = ∂Y/∂P).
* <p>
* B3 represents the partial derivatives of the mean equinoctial elements
* with respect to the estimated propagation parameters.
* </p>
* @param state spacecraft state
* @return the B3 jacobian matrix
*/
public RealMatrix getB3(final SpacecraftState state) {
return super.getParametersJacobian(state);
}
/** Get the Jacobian matrix B4 (B4 = ∂εη/∂c).
* <p>
* B4 represents the partial derivatives of the short period motion
* with respect to the estimated propagation parameters.
* </p>
* @return the B4 jacobian matrix
*/
public RealMatrix getB4() {
// Initialize B4
final List<String> names = getJacobiansColumnsNames();
final RealMatrix B4 = MatrixUtils.createRealMatrix(STATE_DIMENSION, names.size());
// add the short period terms
for (int j = 0; j < names.size(); ++j) {
final double[] column = shortPeriodDerivativesJacobianColumns.get(names.get(j));
for (int i = 0; i < STATE_DIMENSION; i++) {
B4.addToEntry(i, j, column[i]);
}
}
// Return B4
return B4;
}
/** Freeze the names of the Jacobian columns.
* <p>
* This method is called when proagation starts, i.e. when configuration is completed
* </p>
*/
public void freezeColumnsNames() {
columnsNames = getJacobiansColumnsNames();
}
/** {@inheritDoc} */
@Override
public List<String> getJacobiansColumnsNames() {
return columnsNames == null ? propagator.getJacobiansColumnsNames() : columnsNames;
}
/** Initialize the short periodic terms for the "field" elements.
* @param reference current mean spacecraft state
*/
public void initializeFieldShortPeriodTerms(final SpacecraftState reference) {
// Converter
final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());
// Loop on force models
for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {
// Convert to Gradient
final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
final Gradient[] dsParameters = converter.getParametersAtStateDate(dsState, forceModel);
final FieldAuxiliaryElements<Gradient> auxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), I);
// Initialize the "Field" short periodic terms in OSCULATING mode
fieldShortPeriodTerms.addAll(forceModel.initializeShortPeriodTerms(auxiliaryElements, PropagationType.OSCULATING, dsParameters));
}
}
/** Update the short periodic terms for the "field" elements.
* @param reference current mean spacecraft state
*/
@SuppressWarnings("unchecked")
public void updateFieldShortPeriodTerms(final SpacecraftState reference) {
// Converter
final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());
// Loop on force models
for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {
// Convert to Gradient
final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
final Gradient[] dsParameters = converter.getParameters(dsState, forceModel);
// Update the short periodic terms for the current force model
forceModel.updateShortPeriodTerms(dsParameters, dsState);
}
}
/** {@inheritDoc} */
@Override
public void setReferenceState(final SpacecraftState reference) {
// reset derivatives to zero
for (final double[] row : shortPeriodDerivativesStm) {
Arrays.fill(row, 0.0);
}
shortPeriodDerivativesJacobianColumns.clear();
final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());
// Compute Jacobian
for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {
final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
final Gradient zero = dsState.getDate().getField().getZero();
final Gradient[] shortPeriod = new Gradient[6];
Arrays.fill(shortPeriod, zero);
for (final FieldShortPeriodTerms<Gradient> spt : fieldShortPeriodTerms) {
final Gradient[] spVariation = spt.value(dsState.getOrbit());
for (int i = 0; i < spVariation .length; i++) {
shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
}
}
final double[] derivativesASP = shortPeriod[0].getGradient();
final double[] derivativesExSP = shortPeriod[1].getGradient();
final double[] derivativesEySP = shortPeriod[2].getGradient();
final double[] derivativesHxSP = shortPeriod[3].getGradient();
final double[] derivativesHySP = shortPeriod[4].getGradient();
final double[] derivativesLSP = shortPeriod[5].getGradient();
// update Jacobian with respect to state
addToRow(derivativesASP, 0);
addToRow(derivativesExSP, 1);
addToRow(derivativesEySP, 2);
addToRow(derivativesHxSP, 3);
addToRow(derivativesHySP, 4);
addToRow(derivativesLSP, 5);
int paramsIndex = converter.getFreeStateParameters();
for (ParameterDriver driver : forceModel.getParametersDrivers()) {
if (driver.isSelected()) {
final TimeSpanMap<String> driverNameSpanMap = driver.getNamesSpanMap();
// for each span (for each estimated value) corresponding name is added
for (Span<String> span = driverNameSpanMap.getFirstSpan(); span != null; span = span.next()) {
// get the partials derivatives for this driver
DoubleArrayDictionary.Entry entry = shortPeriodDerivativesJacobianColumns.getEntry(span.getData());
if (entry == null) {
// create an entry filled with zeroes
shortPeriodDerivativesJacobianColumns.put(span.getData(), new double[STATE_DIMENSION]);
entry = shortPeriodDerivativesJacobianColumns.getEntry(span.getData());
}
// add the contribution of the current force model
entry.increment(new double[] {
derivativesASP[paramsIndex], derivativesExSP[paramsIndex], derivativesEySP[paramsIndex],
derivativesHxSP[paramsIndex], derivativesHySP[paramsIndex], derivativesLSP[paramsIndex]
});
++paramsIndex;
}
}
}
}
}
/** Fill State Transition Matrix rows.
* @param derivatives derivatives of a component
* @param index component index (0 for a, 1 for ex, 2 for ey, 3 for hx, 4 for hy, 5 for l)
*/
private void addToRow(final double[] derivatives, final int index) {
for (int i = 0; i < 6; i++) {
shortPeriodDerivativesStm[index][i] += derivatives[i];
}
}
/** {@inheritDoc} */
@Override
public OrbitType getOrbitType() {
return propagator.getOrbitType();
}
/** {@inheritDoc} */
@Override
public PositionAngleType getPositionAngleType() {
return propagator.getPositionAngleType();
}
}