Commit 8446d5ff authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

Added computation of state transition matrix using Brouwer-Lyddane.

parent dd524273
/* 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.List;
import org.hipparchus.analysis.differentiation.Gradient;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.utils.ParameterDriver;
/**
* Converter for Brouwer-Lyddane propagator.
*
* @author Bryan Cazabonne
* @since 11.1
*/
public class BrouwerLyddaneGradientConverter extends AbstractAnalyticalGradientConverter {
/** Fixed dimension of the state. */
public static final int FREE_STATE_PARAMETERS = 6;
/** Orbit propagator. */
private final BrouwerLyddanePropagator propagator;
/** Simple constructor.
* @param propagator orbit propagator used to access initial orbit
*/
BrouwerLyddaneGradientConverter(final BrouwerLyddanePropagator propagator) {
super(propagator, propagator.getMu(), FREE_STATE_PARAMETERS);
// Initialize fields
this.propagator = propagator;
}
/** {@inheritDoc} */
@Override
public FieldBrouwerLyddanePropagator<Gradient> getPropagator(final FieldSpacecraftState<Gradient> state,
final Gradient[] parameters) {
// Zero
final Gradient zero = state.getA().getField().getZero();
// Model parameters
final double[] ck0 = propagator.getCk0();
final double radius = propagator.getReferenceRadius();
final AttitudeProvider provider = propagator.getAttitudeProvider();
// Central attraction coefficient
final Gradient mu = zero.add(propagator.getMu());
// Return the "Field" propagator
return new FieldBrouwerLyddanePropagator<Gradient>(state.getOrbit(), provider, radius, mu,
ck0[2], ck0[3], ck0[4], ck0[5], parameters[0].getValue());
}
/** {@inheritDoc} */
@Override
public List<ParameterDriver> getParametersDrivers() {
return propagator.getParametersDrivers();
}
}
/* 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 org.hipparchus.linear.RealMatrix;
import org.orekit.utils.DoubleArrayDictionary;
/**
* Harvester between two-dimensional Jacobian matrices and
* one-dimensional {@link BrouwerLyddanePropagator}.
*
* @author Bryan Cazabonne
* @since 11.1
*/
public class BrouwerLyddaneHarvester extends AbstractAnalyticalMatricesHarvester {
/** Propagator bound to this harvester. */
private final BrouwerLyddanePropagator 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
*/
BrouwerLyddaneHarvester(final BrouwerLyddanePropagator propagator, final String stmName,
final RealMatrix initialStm, final DoubleArrayDictionary initialJacobianColumns) {
super(propagator, stmName, initialStm, initialJacobianColumns);
this.propagator = propagator;
}
/** {@inheritDoc} */
@Override
public AbstractAnalyticalGradientConverter getGradientConverter() {
return new BrouwerLyddaneGradientConverter(propagator);
}
}
......@@ -16,10 +16,12 @@
*/
package org.orekit.propagation.analytical;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.util.CombinatoricsUtils;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathUtils;
......@@ -35,10 +37,13 @@ import org.orekit.orbits.KeplerianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.AbstractMatricesHarvester;
import org.orekit.propagation.MatricesHarvester;
import org.orekit.propagation.PropagationType;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.analytical.tle.TLE;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.DoubleArrayDictionary;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeSpanMap;
......@@ -96,7 +101,7 @@ public class BrouwerLyddanePropagator extends AbstractAnalyticalPropagator {
* in the multiplications/divisions sequences.
* </p>
*/
private static final double SCALE = FastMath.scalb(1.0, -20);
private static final double SCALE = FastMath.scalb(1.0, -32);
/** Beta constant used by T2 function. */
private static final double BETA = FastMath.scalb(100, -11);
......@@ -607,6 +612,30 @@ public class BrouwerLyddanePropagator extends AbstractAnalyticalPropagator {
return M2Driver.getValue();
}
/**
* Get the central attraction coefficient μ.
* @return mu central attraction coefficient (m³/s²)
*/
public double getMu() {
return mu;
}
/**
* Get the un-normalized zonal coefficients.
* @return the un-normalized zonal coefficients
*/
public double[] getCk0() {
return ck0;
}
/**
* Get the reference radius of the central body attraction model.
* @return the reference radius in meters
*/
public double getReferenceRadius() {
return referenceRadius;
}
/**
* Get the parameters driver for propagation model.
* @return drivers for propagation model
......@@ -615,6 +644,28 @@ public class BrouwerLyddanePropagator extends AbstractAnalyticalPropagator {
return Collections.singletonList(M2Driver);
}
/** {@inheritDoc} */
@Override
protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
return new BrouwerLyddaneHarvester(this, stmName, initialStm, initialJacobianColumns);
}
/**
* 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
*/
protected List<String> getJacobiansColumnsNames() {
final List<String> columnsNames = new ArrayList<>();
for (final ParameterDriver driver : getParametersDrivers()) {
if (driver.isSelected() && !columnsNames.contains(driver.getName())) {
columnsNames.add(driver.getName());
}
}
Collections.sort(columnsNames);
return columnsNames;
}
/** Local class for Brouwer-Lyddane model. */
private class BLModel {
......
package org.orekit.propagation.analytical;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.RealMatrix;
import org.junit.Assert;
import org.junit.Before;
import org.junit.Test;
import org.orekit.Utils;
import org.orekit.forces.gravity.potential.GravityFieldFactory;
import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
import org.orekit.frames.FramesFactory;
import org.orekit.orbits.CartesianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.SpacecraftState;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversList;
public class BrouwerLyddaneParametersDerivativesTest {
/** Orbit propagator. */
private UnnormalizedSphericalHarmonicsProvider provider;
@Before
public void setUp() {
Utils.setDataRoot("regular-data:atmosphere:potential/icgem-format");
provider = GravityFieldFactory.getUnnormalizedProvider(5, 0);
}
@Test
public void testNoEstimatedParameters() {
// Definition of initial conditions with position and velocity
// ------------------------------------------------------------
// e = 0.04152500499523033 and i = 1.705015527659039
AbsoluteDate initDate = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
Vector3D position = new Vector3D(3220103., 69623., 6149822.);
Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
Orbit initialOrbit = new CartesianOrbit(new PVCoordinates(position, velocity),
FramesFactory.getEME2000(), initDate, provider.getMu());
// compute state Jacobian using PartialDerivatives
final BrouwerLyddanePropagator propagator = new BrouwerLyddanePropagator(initialOrbit, provider, 1.0e-14);
final SpacecraftState initialState = propagator.getInitialState();
final double[] stateVector = new double[6];
OrbitType.CARTESIAN.mapOrbitToArray(initialState.getOrbit(), PositionAngle.MEAN, stateVector, null);
BrouwerLyddaneHarvester harvester = (BrouwerLyddaneHarvester) propagator.setupMatricesComputation("stm", null, null);
harvester.freezeColumnsNames();
RealMatrix dYdP = harvester.getParametersJacobian(initialState);
Assert.assertNull(dYdP);
}
@Test
public void testParametersDerivatives() {
// Definition of initial conditions with position and velocity
// ------------------------------------------------------------
// e = 0.04152500499523033 and i = 1.705015527659039
AbsoluteDate initDate = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
Vector3D position = new Vector3D(3220103., 69623., 6149822.);
Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
Orbit initialOrbit = new CartesianOrbit(new PVCoordinates(position, velocity),
FramesFactory.getEME2000(), initDate, provider.getMu());
// Brouwer-Lyddane orbit propagator
final BrouwerLyddanePropagator propagator = new BrouwerLyddanePropagator(initialOrbit, provider, 1.0e-14);
// compute state Jacobian using PartialDerivatives
ParameterDriversList bound = new ParameterDriversList();
final ParameterDriver M2 = propagator.getParametersDrivers().get(0);
M2.setSelected(true);
bound.add(M2);
// Compute parameter Jacobian using Harvester
final SpacecraftState initialState = propagator.getInitialState();
final double[] stateVector = new double[6];
OrbitType.CARTESIAN.mapOrbitToArray(initialState.getOrbit(), PositionAngle.MEAN, stateVector, null);
final AbsoluteDate target = initialState.getDate().shiftedBy(initialState.getKeplerianPeriod());
BrouwerLyddaneHarvester harvester = (BrouwerLyddaneHarvester) propagator.setupMatricesComputation("stm", null, null);
harvester.freezeColumnsNames();
final SpacecraftState finalState = propagator.propagate(target);
RealMatrix dYdP = harvester.getParametersJacobian(finalState);
// compute reference Jacobian using finite differences
OrbitType orbitType = OrbitType.CARTESIAN;
BrouwerLyddanePropagator propagator2;
double[][] dYdPRef = new double[6][1];
ParameterDriver selected = bound.getDrivers().get(0);
double p0 = selected.getReferenceValue();
double h = selected.getScale();
selected.setValue(p0 - 4 * h);
propagator2 = new BrouwerLyddanePropagator(initialOrbit, provider, selected.getValue());
SpacecraftState sM4h = propagator2.propagate(target);
selected.setValue(p0 - 3 * h);
propagator2 = new BrouwerLyddanePropagator(initialOrbit, provider, selected.getValue());
SpacecraftState sM3h = propagator2.propagate(target);
selected.setValue(p0 - 2 * h);
propagator2 = new BrouwerLyddanePropagator(initialOrbit, provider, selected.getValue());
SpacecraftState sM2h = propagator2.propagate(target);
selected.setValue(p0 - 1 * h);
propagator2 = new BrouwerLyddanePropagator(initialOrbit, provider, selected.getValue());
SpacecraftState sM1h = propagator2.propagate(target);
selected.setValue(p0 + 1 * h);
propagator2 = new BrouwerLyddanePropagator(initialOrbit, provider, selected.getValue());
SpacecraftState sP1h = propagator2.propagate(target);
selected.setValue(p0 + 2 * h);
propagator2 = new BrouwerLyddanePropagator(initialOrbit, provider, selected.getValue());
SpacecraftState sP2h = propagator2.propagate(target);
selected.setValue(p0 + 3 * h);
propagator2 = new BrouwerLyddanePropagator(initialOrbit, provider, selected.getValue());
SpacecraftState sP3h = propagator2.propagate(target);
selected.setValue(p0 + 4 * h);
propagator2 = new BrouwerLyddanePropagator(initialOrbit, provider, selected.getValue());
SpacecraftState sP4h = propagator2.propagate(target);
fillJacobianColumn(dYdPRef, 0, orbitType, h,
sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
for (int i = 0; i < 6; ++i) {
Assert.assertEquals(0.0, (dYdPRef[i][0] - dYdP.getEntry(i, 0)) / dYdPRef[i][0], 8.49e-13);
}
}
private void fillJacobianColumn(double[][] jacobian, int column,
OrbitType orbitType, double h,
SpacecraftState sM4h, SpacecraftState sM3h,
SpacecraftState sM2h, SpacecraftState sM1h,
SpacecraftState sP1h, SpacecraftState sP2h,
SpacecraftState sP3h, SpacecraftState sP4h) {
double[] aM4h = stateToArray(sM4h, orbitType)[0];
double[] aM3h = stateToArray(sM3h, orbitType)[0];
double[] aM2h = stateToArray(sM2h, orbitType)[0];
double[] aM1h = stateToArray(sM1h, orbitType)[0];
double[] aP1h = stateToArray(sP1h, orbitType)[0];
double[] aP2h = stateToArray(sP2h, orbitType)[0];
double[] aP3h = stateToArray(sP3h, orbitType)[0];
double[] aP4h = stateToArray(sP4h, orbitType)[0];
for (int i = 0; i < jacobian.length; ++i) {
jacobian[i][column] = ( -3 * (aP4h[i] - aM4h[i]) +
32 * (aP3h[i] - aM3h[i]) -
168 * (aP2h[i] - aM2h[i]) +
672 * (aP1h[i] - aM1h[i])) / (840 * h);
}
}
private double[][] stateToArray(SpacecraftState state, OrbitType orbitType) {
double[][] array = new double[2][6];
orbitType.mapOrbitToArray(state.getOrbit(), PositionAngle.MEAN, array[0], array[1]);
return array;
}
}
package org.orekit.propagation.analytical;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.util.FastMath;
import org.junit.Assert;
import org.junit.Before;
import org.junit.Test;
import org.orekit.Utils;
import org.orekit.attitudes.Attitude;
import org.orekit.errors.OrekitException;
import org.orekit.forces.gravity.potential.GravityFieldFactory;
import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.orbits.CartesianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.MatricesHarvester;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinates;
public class BrouwerLyddaneStateTransitionMatrixTest {
/** Orbit propagator. */
private UnnormalizedSphericalHarmonicsProvider provider;
/** Drag parameter. */
private double M2;
@Before
public void setUp() {
Utils.setDataRoot("regular-data:atmosphere:potential/icgem-format");
provider = GravityFieldFactory.getUnnormalizedProvider(5, 0);
M2 = BrouwerLyddanePropagator.M2;
}
@Test(expected=OrekitException.class)
public void testNullStmName() {
// Definition of initial conditions with position and velocity
// ------------------------------------------------------------
// e = 0.04152500499523033 and i = 1.705015527659039
AbsoluteDate initDate = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
Vector3D position = new Vector3D(3220103., 69623., 6149822.);
Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
Orbit initialOrbit = new CartesianOrbit(new PVCoordinates(position, velocity),
FramesFactory.getEME2000(), initDate, provider.getMu());
final BrouwerLyddanePropagator propagator = new BrouwerLyddanePropagator(initialOrbit, provider, M2);
propagator.setupMatricesComputation(null, null, null);
}
@Test
public void testStateJacobian() {
// Definition of initial conditions with position and velocity
// ------------------------------------------------------------
// e = 0.04152500499523033 and i = 1.705015527659039
AbsoluteDate initDate = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
Vector3D position = new Vector3D(3220103., 69623., 6149822.);
Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
Orbit initialOrbit = new CartesianOrbit(new PVCoordinates(position, velocity),
FramesFactory.getEME2000(), initDate, provider.getMu());
// compute state Jacobian using PartialDerivatives
BrouwerLyddanePropagator propagator = new BrouwerLyddanePropagator(initialOrbit, provider, M2);
final SpacecraftState initialState = propagator.getInitialState();
final double[] stateVector = new double[6];
OrbitType.CARTESIAN.mapOrbitToArray(initialState.getOrbit(), PositionAngle.MEAN, stateVector, null);
final AbsoluteDate target = initialState.getDate().shiftedBy(initialState.getKeplerianPeriod());
MatricesHarvester harvester = propagator.setupMatricesComputation("stm", null, null);
final SpacecraftState finalState = propagator.propagate(target);
RealMatrix dYdY0 = harvester.getStateTransitionMatrix(finalState);
// compute reference state Jacobian using finite differences
double[][] dYdY0Ref = new double[6][6];
BrouwerLyddanePropagator propagator2;
double[] steps = NumericalPropagator.tolerances(10, initialState.getOrbit(), OrbitType.CARTESIAN)[0];
for (int i = 0; i < 6; ++i) {
propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, -4 * steps[i], i).getOrbit(), provider, M2);
SpacecraftState sM4h = propagator2.propagate(target);
propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, -3 * steps[i], i).getOrbit(), provider, M2);
SpacecraftState sM3h = propagator2.propagate(target);
propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, -2 * steps[i], i).getOrbit(), provider, M2);
SpacecraftState sM2h = propagator2.propagate(target);
propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, -1 * steps[i], i).getOrbit(), provider, M2);
SpacecraftState sM1h = propagator2.propagate(target);
propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, +1 * steps[i], i).getOrbit(), provider, M2);
SpacecraftState sP1h = propagator2.propagate(target);
propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, +2 * steps[i], i).getOrbit(), provider, M2);
SpacecraftState sP2h = propagator2.propagate(target);
propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, +3 * steps[i], i).getOrbit(), provider, M2);
SpacecraftState sP3h = propagator2.propagate(target);
propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, +4 * steps[i], i).getOrbit(), provider, M2);
SpacecraftState sP4h = propagator2.propagate(target);
fillJacobianColumn(dYdY0Ref, i, OrbitType.CARTESIAN, steps[i],
sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
}
// Verify
for (int i = 0; i < 6; ++i) {
for (int j = 0; j < 6; ++j) {
if (stateVector[i] != 0) {
double error = FastMath.abs((dYdY0.getEntry(i, j) - dYdY0Ref[i][j]) / stateVector[i]) * steps[j];
Assert.assertEquals(0, error, 5.15e-14);
}
}
}
}
private void fillJacobianColumn(double[][] jacobian, int column,
OrbitType orbitType, double h,
SpacecraftState sM4h, SpacecraftState sM3h,
SpacecraftState sM2h, SpacecraftState sM1h,
SpacecraftState sP1h, SpacecraftState sP2h,
SpacecraftState sP3h, SpacecraftState sP4h) {
double[] aM4h = stateToArray(sM4h, orbitType)[0];
double[] aM3h = stateToArray(sM3h, orbitType)[0];
double[] aM2h = stateToArray(sM2h, orbitType)[0];
double[] aM1h = stateToArray(sM1h, orbitType)[0];
double[] aP1h = stateToArray(sP1h, orbitType)[0];
double[] aP2h = stateToArray(sP2h, orbitType)[0];
double[] aP3h = stateToArray(sP3h, orbitType)[0];
double[] aP4h = stateToArray(sP4h, orbitType)[0];
for (int i = 0; i < jacobian.length; ++i) {
jacobian[i][column] = ( -3 * (aP4h[i] - aM4h[i]) +
32 * (aP3h[i] - aM3h[i]) -
168 * (aP2h[i] - aM2h[i]) +
672 * (aP1h[i] - aM1h[i])) / (840 * h);
}
}
private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType,
double delta, int column) {
double[][] array = stateToArray(state, orbitType);
array[0][column] += delta;
return arrayToState(array, state.getFrame(), state.getDate(),
state.getMu(), state.getAttitude());
}
private double[][] stateToArray(SpacecraftState state, OrbitType orbitType) {
double[][] array = new double[2][6];
orbitType.mapOrbitToArray(state.getOrbit(), PositionAngle.MEAN, array[0], array[1]);
return array;
}
private SpacecraftState arrayToState(double[][] array,
Frame frame, AbsoluteDate date, double mu,
Attitude attitude) {
CartesianOrbit orbit = (CartesianOrbit) OrbitType.CARTESIAN.mapArrayToOrbit(array[0], array[1], PositionAngle.MEAN, date, mu, frame);
return new SpacecraftState(orbit, attitude);