Skip to content
Snippets Groups Projects
Commit a932005e authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

Prepared computation of STM for all analytical orbit propagators.

parent 04d98077
No related branches found
No related tags found
No related merge requests found
/* 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 {
}
}
/**
* Get the names of the parameters in the matrix returned by {@link MatricesHarvester#getParametersJacobian}.
* @return names of the parameters (i.e. columns) of the Jacobian matrix
* @since 11.1
*/
protected List<String> getJacobiansColumnsNames() {
return new ArrayList<>();
}
/** Internal PVCoordinatesProvider for attitude computation. */
private class LocalPVProvider implements PVCoordinatesProvider {
......
......@@ -16,26 +16,15 @@
*/
package org.orekit.propagation.analytical.tle;
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.frames.Frame;
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.propagation.analytical.AbstractAnalyticalGradientConverter;
import org.orekit.time.TimeScale;
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 TLE propagator.
* @author Luc Maisonobe
......@@ -43,7 +32,7 @@ import org.orekit.utils.TimeStampedFieldPVCoordinates;
* @author Thomas Paulet
* @since 11.0
*/
class TLEGradientConverter extends AbstractGradientConverter {
class TLEGradientConverter extends AbstractAnalyticalGradientConverter {
/** Fixed dimension of the state. */
public static final int FREE_STATE_PARAMETERS = 6;
......@@ -60,139 +49,20 @@ class TLEGradientConverter extends AbstractGradientConverter {
/** Attitude provider. */
private final AttitudeProvider provider;
/** States with various number of additional propagation parameters. */
private final List<FieldSpacecraftState<Gradient>> gStates;
/** Simple constructor.
* @param propagator TLE propagator used to access initial orbit
*/
TLEGradientConverter(final TLEPropagator propagator) {
super(FREE_STATE_PARAMETERS);
super(propagator, TLEConstants.MU, FREE_STATE_PARAMETERS);
// TLE and related parameters
this.tle = propagator.getTLE();
this.teme = propagator.getFrame();
this.utc = tle.getUtc();
// Attitude provider
this.tle = propagator.getTLE();
this.teme = propagator.getFrame();
this.utc = tle.getUtc();
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(FREE_STATE_PARAMETERS, 0, pos.getX()),
Gradient.variable(FREE_STATE_PARAMETERS, 1, pos.getY()),
Gradient.variable(FREE_STATE_PARAMETERS, 2, pos.getZ()));
// Velocity may have derivatives or not
final Vector3D vel = state.getPVCoordinates().getVelocity();
final FieldVector3D<Gradient> velG = new FieldVector3D<>(Gradient.variable(FREE_STATE_PARAMETERS, 3, vel.getX()),
Gradient.variable(FREE_STATE_PARAMETERS, 4, vel.getY()),
Gradient.variable(FREE_STATE_PARAMETERS, 5, vel.getZ()));
// Acceleration never has derivatives
final Vector3D acc = state.getPVCoordinates().getAcceleration();
final FieldVector3D<Gradient> accG = new FieldVector3D<>(Gradient.constant(FREE_STATE_PARAMETERS, acc.getX()),
Gradient.constant(FREE_STATE_PARAMETERS, acc.getY()),
Gradient.constant(FREE_STATE_PARAMETERS, acc.getZ()));
// Mass never has derivatives
final Gradient gM = Gradient.constant(FREE_STATE_PARAMETERS, state.getMass());
final Gradient gMu = Gradient.constant(FREE_STATE_PARAMETERS, TLEPropagator.getMU());
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 TLE model.
* @return state with the number of parameters consistent with TLE model
*/
public FieldSpacecraftState<Gradient> getState() {
// Count the required number of parameters
int nbParams = 0;
for (final ParameterDriver driver : tle.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 = FREE_STATE_PARAMETERS + 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(TLE)}
* @return TLE model parameters
*/
public Gradient[] getParameters(final FieldSpacecraftState<Gradient> state) {
final int freeParameters = state.getMass().getFreeParameters();
final List<ParameterDriver> drivers = tle.getParametersDrivers();
final Gradient[] parameters = new Gradient[drivers.size()];
int index = FREE_STATE_PARAMETERS;
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 converted TLE propagator.
* @param state state as returned by {@link #getState(TLE)}
* @param parameters model parameters as returned by {@link #getParameters(FieldSpacecraftState, TLE)}
* @return the converted propagator
*/
/** {@inheritDoc} */
@Override
public FieldTLEPropagator<Gradient> getPropagator(final FieldSpacecraftState<Gradient> state,
final Gradient[] parameters) {
......@@ -224,4 +94,10 @@ class TLEGradientConverter extends AbstractGradientConverter {
}
/** {@inheritDoc} */
@Override
public List<ParameterDriver> getParametersDrivers() {
return tle.getParametersDrivers();
}
}
......@@ -16,41 +16,18 @@
*/
package org.orekit.propagation.analytical.tle;
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.propagation.analytical.AbstractAnalyticalGradientConverter;
import org.orekit.propagation.analytical.AbstractAnalyticalMatricesHarvester;
import org.orekit.utils.DoubleArrayDictionary;
import org.orekit.utils.FieldPVCoordinates;
import org.orekit.utils.ParameterDriver;
/** Harvester between two-dimensional Jacobian matrices and one-dimensional {@link
* SpacecraftState#getAdditionalState(String) additional state arrays}.
/** Harvester between two-dimensional Jacobian matrices and
* one-dimensional {@link TLEPropagator}.
* @author Thomas Paulet
* @author Bryan Cazabonne
* @since 11.1
*/
class TLEHarvester 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;
class TLEHarvester extends AbstractAnalyticalMatricesHarvester {
/** Propagator bound to this harvester. */
private final TLEPropagator propagator;
......@@ -71,146 +48,13 @@ class TLEHarvester extends AbstractMatricesHarvester {
*/
TLEHarvester(final TLEPropagator 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;
super(propagator, stmName, initialStm, initialJacobianColumns);
this.propagator = propagator;
}
/** {@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 TLEGradientConverter converter = new TLEGradientConverter(propagator);
final FieldSpacecraftState<Gradient> gState = converter.getState();
final Gradient[] gParameters = converter.getParameters(gState);
final FieldTLEPropagator<Gradient> gPropagator = converter.getPropagator(gState, gParameters);
// Compute Jacobian
final AbsoluteDate target = reference.getDate();
final FieldAbsoluteDate<Gradient> start = gPropagator.getTLE().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 : propagator.getTLE().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];
}
public AbstractAnalyticalGradientConverter getGradientConverter() {
return new TLEGradientConverter(propagator);
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment