diff --git a/src/changes/changes.xml b/src/changes/changes.xml
index 1cedfe1a28156002a80b80fc6b3f1f76228702e6..13257e2ff907e2d3051b0854e06f63d3b6278f6e 100644
--- a/src/changes/changes.xml
+++ b/src/changes/changes.xml
@@ -21,6 +21,10 @@
</properties>
<body>
<release version="10.0" date="TBD" description="TBD">
+ <action dev="maxime" type="add" issue="403">
+ Added tests for class UnivariateProcessNoise.
+ Working tests for non-Cartesian orbit propagation are still needed.
+ </action>
<action dev="maxime" type="fix" issue="514">
Deprecated unused DerivativeStructure acceleration computation methods.
In interfaces radiationPressureAcceleration and dragAcceleration, and all their implementations and their tests.
diff --git a/src/main/java/org/orekit/estimation/sequential/UnivariateProcessNoise.java b/src/main/java/org/orekit/estimation/sequential/UnivariateProcessNoise.java
index a5c3d7d895441469e601338777b8b6bd6c246199..def7b1cfd56b60321df567f6b12efd19449b8655 100644
--- a/src/main/java/org/orekit/estimation/sequential/UnivariateProcessNoise.java
+++ b/src/main/java/org/orekit/estimation/sequential/UnivariateProcessNoise.java
@@ -24,6 +24,7 @@ import org.orekit.errors.OrekitException;
import org.orekit.frames.LOFType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.SpacecraftState;
+import org.orekit.utils.CartesianDerivativesFilter;
/** Provider for a temporal evolution of the process noise matrix.
* All parameters (orbital or propagation) are time dependent and provided as {@link UnivariateFunction}.
@@ -184,40 +185,25 @@ public class UnivariateProcessNoise extends AbstractCovarianceMatrixProvider {
// Form the diagonal matrix in LOF frame and Cartesian formalism
final RealMatrix lofCartesianProcessNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(lofOrbitalProcessNoiseValues);
- // Get the rotation matrix from LOF to inertial frame
- final double[][] lofToInertialRotation = lofType.rotationFromInertial(current.getPVCoordinates()).
- revert().getMatrix();
-
- // Jacobian from LOF to inertial frame
- final RealMatrix jacLofToInertial = MatrixUtils.createRealMatrix(6, 6);
- jacLofToInertial.setSubMatrix(lofToInertialRotation, 0, 0);
- jacLofToInertial.setSubMatrix(lofToInertialRotation, 3, 3);
-
- // FIXME: Trying to fix the transform from LOF to inertial
- //final Transform lofToInertial = lofType.transformFromInertial(current.getDate(), current.getPVCoordinates()).getInverse();
- //final Vector3D OM = lofToInertial.getRotationRate().negate();
- //final double[][] MOM = new double[3][3];
- //MOM[0][1] = -OM.getZ();
- //MOM[0][2] = OM.getY();
- //MOM[1][0] = OM.getZ();
- //MOM[1][2] = -OM.getX();
- //MOM[2][0] = -OM.getY();
- //MOM[2][1] = OM.getX();
- //jacLofToInertial.setSubMatrix(MOM, 3, 0);
- //debug
+ // Get the Jacobian from LOF to Inertial
+ final double[][] dLofdInertial = new double[6][6];
+ lofType.transformFromInertial(current.getDate(),
+ current.getPVCoordinates()).getInverse().getJacobian(CartesianDerivativesFilter.USE_PV,
+ dLofdInertial);
+ final RealMatrix jacLofToInertial = MatrixUtils.createRealMatrix(dLofdInertial);
// Jacobian of orbit parameters with respect to Cartesian parameters
final double[][] dYdC = new double[6][6];
current.getOrbit().getJacobianWrtCartesian(positionAngle, dYdC);
- final RealMatrix jacParametersWrtCartesian = MatrixUtils.createRealMatrix(dYdC);
+ final RealMatrix jacOrbitWrtCartesian = MatrixUtils.createRealMatrix(dYdC);
// Complete Jacobian of the transformation
- final RealMatrix jacobian = jacParametersWrtCartesian.multiply(jacLofToInertial);
+ final RealMatrix jacobian = jacOrbitWrtCartesian.multiply(jacLofToInertial);
// Return the orbital process noise matrix in inertial frame and proper orbit type
final RealMatrix inertialOrbitalProcessNoiseMatrix = jacobian.
multiply(lofCartesianProcessNoiseMatrix).
- multiply(jacobian.transpose());
+ multiplyTransposed(jacobian);
return inertialOrbitalProcessNoiseMatrix;
}
diff --git a/src/test/java/org/orekit/estimation/sequential/UnivariateprocessNoiseTest.java b/src/test/java/org/orekit/estimation/sequential/UnivariateprocessNoiseTest.java
index 7f07dd170fe723246114c078f9cb2c2ee2cd3eae..e97bbf3641dda6f48dae3fb8f2e673718eb17365 100644
--- a/src/test/java/org/orekit/estimation/sequential/UnivariateprocessNoiseTest.java
+++ b/src/test/java/org/orekit/estimation/sequential/UnivariateprocessNoiseTest.java
@@ -3,21 +3,27 @@ package org.orekit.estimation.sequential;
import static org.junit.Assert.assertArrayEquals;
import java.util.Arrays;
+import java.util.Locale;
import org.hipparchus.analysis.UnivariateFunction;
import org.hipparchus.analysis.polynomials.PolynomialFunction;
-import org.hipparchus.geometry.euclidean.threed.Vector3D;
+import org.hipparchus.linear.Array2DRowRealMatrix;
+import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
+import org.hipparchus.linear.RealVector;
import org.hipparchus.random.CorrelatedRandomVectorGenerator;
import org.hipparchus.random.GaussianRandomGenerator;
import org.hipparchus.random.Well1024a;
import org.hipparchus.stat.descriptive.StreamingStatistics;
import org.hipparchus.util.FastMath;
+import org.junit.Assert;
import org.junit.Ignore;
import org.junit.Test;
+import org.orekit.errors.OrekitIllegalArgumentException;
import org.orekit.estimation.Context;
import org.orekit.estimation.EstimationTestUtils;
+import org.orekit.estimation.Force;
import org.orekit.frames.LOFType;
import org.orekit.frames.Transform;
import org.orekit.orbits.OrbitType;
@@ -25,17 +31,66 @@ import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
+import org.orekit.utils.CartesianDerivativesFilter;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
public class UnivariateprocessNoiseTest {
- /** Test different functions and check the conversion from LOF to ECI (and back) of the covariances. */
+ /** Basic test for getters. */
@Test
- public void testUnivariateProcessNoise() {
+ public void testUnivariateProcessNoiseGetters() {
+
+ Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
+
+ // Define the univariate functions for the standard deviations
+ final UnivariateFunction[] lofCartesianOrbitalParametersEvolution = new UnivariateFunction[6];
+ // Evolution for position error
+ lofCartesianOrbitalParametersEvolution[0] = new PolynomialFunction(new double[] {100., 0., 1e-4});
+ lofCartesianOrbitalParametersEvolution[1] = new PolynomialFunction(new double[] {100., 1e-1, 0.});
+ lofCartesianOrbitalParametersEvolution[2] = new PolynomialFunction(new double[] {100., 0., 0.});
+ // Evolution for velocity error
+ lofCartesianOrbitalParametersEvolution[3] = new PolynomialFunction(new double[] {1., 0., 1.e-6});
+ lofCartesianOrbitalParametersEvolution[4] = new PolynomialFunction(new double[] {1., 1e-3, 0.});
+ lofCartesianOrbitalParametersEvolution[5] = new PolynomialFunction(new double[] {1., 0., 0.});
+ final UnivariateFunction[] propagationParametersEvolution =
+ new UnivariateFunction[] {new PolynomialFunction(new double[] {10, 1., 1e-4}),
+ new PolynomialFunction(new double[] {1000., 0., 0.})};
+
+ // Create a dummy initial covariance matrix
+ final RealMatrix initialCovarianceMatrix = MatrixUtils.createRealIdentityMatrix(7);
+
+ // Set the process noise object
+ // Define input LOF and output position angle
+ final LOFType lofType = LOFType.TNW;
+ final UnivariateProcessNoise processNoise = new UnivariateProcessNoise(initialCovarianceMatrix,
+ lofType,
+ PositionAngle.TRUE,
+ lofCartesianOrbitalParametersEvolution,
+ propagationParametersEvolution);
+
+ Assert.assertEquals(LOFType.TNW, processNoise.getLofType());
+ Assert.assertEquals(PositionAngle.TRUE, processNoise.getPositionAngle());
+ Assert.assertEquals(initialCovarianceMatrix,
+ processNoise.getInitialCovarianceMatrix(new SpacecraftState(context.initialOrbit)));
+ Assert.assertArrayEquals(lofCartesianOrbitalParametersEvolution, processNoise.getLofCartesianOrbitalParametersEvolution());
+ Assert.assertArrayEquals(propagationParametersEvolution, processNoise.getPropagationParametersEvolution());
+ }
+
+ /** Test UnivariateProcessNoise class.
+ * - Initialize with different univariate functions for orbital/propagation parameters variation
+ * - Check that the inertial process noise covariance matrix is consistent with the inputs
+ * - Propagation in Cartesian formalism
+ */
+ @Test
+ public void testProcessNoiseMatrixCartesian() {
+
// Create context
Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
+
+ // Print result on console ?
+ final boolean print = false;
// Create initial orbit and propagator builder
final OrbitType orbitType = OrbitType.CARTESIAN;
@@ -45,8 +100,10 @@ public class UnivariateprocessNoiseTest {
final double maxStep = 60.;
final double dP = 1.;
final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart,
- minStep, maxStep, dP);
-
+ minStep, maxStep, dP,
+ Force.POTENTIAL, Force.THIRD_BODY_MOON,
+ Force.THIRD_BODY_SUN,
+ Force.SOLAR_RADIATION_PRESSURE);
// Create a propagator
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
propagatorBuilder);
@@ -61,7 +118,8 @@ public class UnivariateprocessNoiseTest {
lofCartesianOrbitalParametersEvolution[3] = new PolynomialFunction(new double[] {1., 0., 1.e-6});
lofCartesianOrbitalParametersEvolution[4] = new PolynomialFunction(new double[] {1., 1e-3, 0.});
lofCartesianOrbitalParametersEvolution[5] = new PolynomialFunction(new double[] {1., 0., 0.});
-
+
+ // Propagation parameters' evolution
final UnivariateFunction[] propagationParametersEvolution =
new UnivariateFunction[] {new PolynomialFunction(new double[] {10, 1., 1e-4}),
new PolynomialFunction(new double[] {1000., 0., 0.})};
@@ -77,30 +135,129 @@ public class UnivariateprocessNoiseTest {
positionAngle,
lofCartesianOrbitalParametersEvolution,
propagationParametersEvolution);
- // Test on initial value, after 1 min and after a
+ // Test on initial value, after 1 hour and after one orbit
final SpacecraftState state0 = propagator.getInitialState();
- final SpacecraftState state1 = propagator.propagate(context.initialOrbit.getDate().shiftedBy(60.));
+ final SpacecraftState state1 = propagator.propagate(context.initialOrbit.getDate().shiftedBy(3600.));
final SpacecraftState state2 = propagator.propagate(context.initialOrbit.getDate()
- .shiftedBy(context.initialOrbit.getKeplerianPeriod()/4.));
+ .shiftedBy(2*context.initialOrbit.getKeplerianPeriod()));
// Number of samples for the statistics
final int sampleNumber = 10000;
- // Relative tolerance on final standard deviations observed (< 2% here)
- final double relativeTolerance = 1.28e-2;;
+ // Relative tolerance on final standard deviations observed (< 1.5% for 10000 samples)
+ final double relativeTolerance = 1.5e-2;
- checkCovarianceValue(state0, state0, processNoise, sampleNumber, relativeTolerance);
- checkCovarianceValue(state0, state1, processNoise, sampleNumber, relativeTolerance);
- checkCovarianceValue(state0, state2, processNoise, sampleNumber, relativeTolerance);
+ if (print) {
+ System.out.println("Orbit Type : " + orbitType);
+ System.out.println("Position Angle: " + positionAngle + "\n");
+ }
+ checkCovarianceValue(print, state0, state0, processNoise, sampleNumber, relativeTolerance);
+ checkCovarianceValue(print, state0, state1, processNoise, sampleNumber, relativeTolerance);
+ checkCovarianceValue(print, state0, state2, processNoise, sampleNumber, relativeTolerance);
}
+ /** Test process noise matrix computation.
+ * - Initialize with different univariate functions for orbital/propagation parameters variation
+ * - Check that the inertial process noise covariance matrix is consistent with the inputs
+ * - Propagation in Non-Cartesian formalism (Keplerian, Circular or Equinoctial)
+ * TO DO: Find out why position in LOF frame is off after one hour of propagation
+ */
+ @Ignore
+ @Test
+ public void testProcessNoiseMatrixNonCartesian() {
+
+ // Create context
+ Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
+
+ // Print result on console ?
+ final boolean print = true;
+
+ // Create initial orbit and propagator builder
+ final OrbitType orbitType = OrbitType.KEPLERIAN;
+ final PositionAngle positionAngle = PositionAngle.TRUE;
+ final boolean perfectStart = true;
+ final double minStep = 1.e-6;
+ final double maxStep = 60.;
+ final double dP = 1.;
+ final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart,
+ minStep, maxStep, dP);//
+// Force.POTENTIAL, Force.THIRD_BODY_MOON,
+// Force.THIRD_BODY_SUN,
+// Force.SOLAR_RADIATION_PRESSURE);
+
+ // Create a propagator
+ final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
+ propagatorBuilder);
+
+ // Define the univariate functions for the standard deviations
+ final UnivariateFunction[] lofCartesianOrbitalParametersEvolution = new UnivariateFunction[6];
+ // Evolution for position error
+ lofCartesianOrbitalParametersEvolution[0] = new PolynomialFunction(new double[] {100., 0., 1e-4});
+ lofCartesianOrbitalParametersEvolution[1] = new PolynomialFunction(new double[] {100., 1e-1, 0.});
+ lofCartesianOrbitalParametersEvolution[2] = new PolynomialFunction(new double[] {100., 0., 0.});
+ // Evolution for velocity error
+ lofCartesianOrbitalParametersEvolution[3] = new PolynomialFunction(new double[] {1., 0., 1.e-6});
+ lofCartesianOrbitalParametersEvolution[4] = new PolynomialFunction(new double[] {1., 1e-3, 0.});
+ lofCartesianOrbitalParametersEvolution[5] = new PolynomialFunction(new double[] {1., 0., 0.});
+
+ final UnivariateFunction[] propagationParametersEvolution =
+ new UnivariateFunction[] {new PolynomialFunction(new double[] {10, 1., 1e-4}),
+ new PolynomialFunction(new double[] {1000., 0., 0.})};
+
+ // Create a dummy initial covariance matrix
+ final RealMatrix initialCovarianceMatrix = MatrixUtils.createRealIdentityMatrix(7);
+
+ // Set the process noise object
+ // Define input LOF and output position angle
+ final LOFType lofType = LOFType.TNW;
+ final UnivariateProcessNoise processNoise = new UnivariateProcessNoise(initialCovarianceMatrix,
+ lofType,
+ positionAngle,
+ lofCartesianOrbitalParametersEvolution,
+ propagationParametersEvolution);
+ // Test on initial value, after 1 hour and after 2 orbits
+ final SpacecraftState state0 = propagator.getInitialState();
+ final SpacecraftState state1 = propagator.propagate(context.initialOrbit.getDate().shiftedBy(3600.));
+// final SpacecraftState state2 = propagator.propagate(context.initialOrbit.getDate()
+// .shiftedBy(2*context.initialOrbit.getKeplerianPeriod()));
+
+ // Number of samples for the statistics
+ final int sampleNumber = 10000;
+
+ // Relative tolerance on final standard deviations observed
+ final double relativeTolerance = 0.02;
+
+ if (print) {
+ System.out.println("Orbit Type : " + orbitType);
+ System.out.println("Position Angle: " + positionAngle + "\n");
+ }
+ checkCovarianceValue(print, state0, state0, processNoise, sampleNumber, relativeTolerance);
+ checkCovarianceValue(print, state0, state1, processNoise, sampleNumber, relativeTolerance);
+
+ // Orbit too far off after 2 orbital periods
+ // It becomes inconsistent when applying random vector values
+ //checkCovarianceValue(print, state0, state2, processNoise, sampleNumber, relativeTolerance);
+ }
- private static void checkCovarianceValue(final SpacecraftState previous,
- final SpacecraftState current,
- final UnivariateProcessNoise univariateProcessNoise,
- final int sampleNumber,
- final double relativeTolerance) {
+ /** Check the values of the covariance given in inertial frame by the UnivariateProcessNoise object.
+ * - Get the process noise covariance matrix P in inertial frame
+ * - Use a random vector generator based on P to produce a set of N error vectors in inertial frame
+ * - Compare the standard deviations of the noisy data to the univariate functions given in input of the UnivariateProcessNoise class.
+ * - For orbit parameters this involve converting the inertial orbital vector to LOF frame and Cartesian formalism first
+ * @param print print results in console ?
+ * @param previous previous state
+ * @param current current state
+ * @param univariateProcessNoise UnivariateProcessNoise object to test
+ * @param sampleNumber number of sample vectors for the statistics
+ * @param relativeTolerance relative tolerance on the standard deviations for the tests
+ */
+ private void checkCovarianceValue(final boolean print,
+ final SpacecraftState previous,
+ final SpacecraftState current,
+ final UnivariateProcessNoise univariateProcessNoise,
+ final int sampleNumber,
+ final double relativeTolerance) {
// Get the process noise matrix
final RealMatrix processNoiseMatrix = univariateProcessNoise.getProcessNoiseMatrix(previous, current);
@@ -138,8 +295,8 @@ public class UnivariateprocessNoiseTest {
null);
// Transform from inertial to current spacecraft LOF frame
final Transform inertialToLof = univariateProcessNoise.getLofType().transformFromInertial(current.getDate(),
- current.getOrbit().getPVCoordinates());
- // Create the vectors and compute the stats
+ current.getOrbit().getPVCoordinates());
+ // Create the vectors and compute the states
for (int i = 0; i < sampleNumber; i++) {
// Create a random vector
@@ -148,22 +305,31 @@ public class UnivariateprocessNoiseTest {
// Orbital parameters values
// -------------------------
- // Get the full inertial orbit by adding up the values of current orbit and orbit error (first 6 cop. of random vector)
+ // Get the full inertial orbit by adding up the values of current orbit and orbit error (first 6 components of random vector)
final double[] modifiedOrbitArray = new double[6];
for (int j = 0; j < 6; j++) {
modifiedOrbitArray[j] = currentOrbitArray[j] + randomVector[j];
}
// Get the corresponding PV coordinates
- final TimeStampedPVCoordinates inertialPV =
- current.getOrbit().getType().mapArrayToOrbit(modifiedOrbitArray,
- null,
- univariateProcessNoise.getPositionAngle(),
- current.getDate(),
- current.getMu(),
- current.getFrame())
- .getPVCoordinates();
-
+ TimeStampedPVCoordinates inertialPV = null;
+ try {
+ inertialPV = current.getOrbit().getType().mapArrayToOrbit(modifiedOrbitArray,
+ null,
+ univariateProcessNoise.getPositionAngle(),
+ current.getDate(),
+ current.getMu(),
+ current.getFrame())
+ .getPVCoordinates();
+ } catch (OrekitIllegalArgumentException e) {
+ // If orbit becomes inconsistent due to errors that are too large, print orbital values
+ System.out.println("i = " + i + " / Inconsistent Orbit");
+ System.out.println("\tCurrent Orbit = " + Arrays.toString(currentOrbitArray));
+ System.out.println("\tModified Orbit = " + Arrays.toString(modifiedOrbitArray));
+ System.out.println("\tDelta Orbit = " + Arrays.toString(randomVector));
+ e.printStackTrace();
+ System.err.println(e.getMessage());
+ }
// Transform from inertial to current LOF
// The value obtained is the Cartesian error vector in LOF (w/r to current spacecraft position)
@@ -225,6 +391,18 @@ public class UnivariateprocessNoiseTest {
}
}
+ // Print values
+ if (print) {
+ System.out.println("\tdt = " + dt + " / N = " + sampleNumber + "\n");
+ System.out.println("\tσ orbit ref = " + Arrays.toString(orbitalValues));
+ System.out.println("\tσ orbit stat = " + Arrays.toString(orbitalStatisticsValues));
+ System.out.println("\tσ orbit % = " + Arrays.toString(Arrays.stream(orbitalRelativeValues).map(i -> i*100.).toArray()) + "\n");
+
+ System.out.println("\tσ propag ref = " + Arrays.toString(propagationValues));
+ System.out.println("\tσ propag stat = " + Arrays.toString(propagationStatisticsValues));
+ System.out.println("\tσ propag % = " + Arrays.toString(Arrays.stream(propagationRelativeValues).map(i -> i*100.).toArray()) + "\n");
+ }
+
// Test the values
assertArrayEquals(new double[6],
orbitalRelativeValues,
@@ -238,7 +416,7 @@ public class UnivariateprocessNoiseTest {
* @param covarianceMatrix input covariance matrix
* @return correlated gaussian random vectors generator
*/
- private static CorrelatedRandomVectorGenerator createSampler(final RealMatrix covarianceMatrix) {
+ private CorrelatedRandomVectorGenerator createSampler(final RealMatrix covarianceMatrix) {
double small = 10e-20 * covarianceMatrix.getNorm();
return new CorrelatedRandomVectorGenerator(
covarianceMatrix,
@@ -246,25 +424,33 @@ public class UnivariateprocessNoiseTest {
new GaussianRandomGenerator(new Well1024a(0x366a26b94e520f41l)));
}
- /** Bug on Vy when dt gets large (here 1/2 Torb is enough to make it crash).
- * Other components are ok.
+ /** Test LOF orbital covariance matrix value against reference.
+ * 1. Initialize with different univariate functions for orbital/propagation parameters variation
+ * 2. Get the inertial process noise covariance matrix
+ * 3. Convert the inertial covariance from 2 in Cartesian and LOF
+ * 4. Compare values of 3 with reference values from 1
*/
- @Ignore
@Test
- public void testBugVy() {
+ public void testLofCartesianOrbitalCovarianceFormal() {
// Create context
Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
+ // Print result on console ?
+ final boolean print = false;
+
// Create initial orbit and propagator builder
- final OrbitType orbitType = OrbitType.CARTESIAN;
- final PositionAngle positionAngle = PositionAngle.TRUE; // Not used here
+ final OrbitType orbitType = OrbitType.KEPLERIAN;
+ final PositionAngle positionAngle = PositionAngle.MEAN;
final boolean perfectStart = true;
final double minStep = 1.e-6;
final double maxStep = 60.;
final double dP = 1.;
final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart,
- minStep, maxStep, dP);
+ minStep, maxStep, dP,
+ Force.POTENTIAL, Force.THIRD_BODY_MOON,
+ Force.THIRD_BODY_SUN,
+ Force.SOLAR_RADIATION_PRESSURE);
// Create a propagator
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
@@ -296,274 +482,96 @@ public class UnivariateprocessNoiseTest {
positionAngle,
lofCartesianOrbitalParametersEvolution,
propagationParametersEvolution);
- // Test on initial value, after 1 min and after a
+ // Initial value
final SpacecraftState state0 = propagator.getInitialState();
- // dt = 1/2 Torb
+ // 1 orbit
final SpacecraftState state1 = propagator.propagate(context.initialOrbit.getDate()
- .shiftedBy(context.initialOrbit.getKeplerianPeriod()/2.));
- // dt = 1 Torb
- final SpacecraftState state2 = propagator.propagate(context.initialOrbit.getDate()
.shiftedBy(context.initialOrbit.getKeplerianPeriod()));
+ // 1 day
+ final SpacecraftState state2 = propagator.propagate(context.initialOrbit.getDate().shiftedBy(86400.));
- // dt = 2 Torb
- final SpacecraftState state3 = propagator.propagate(context.initialOrbit.getDate()
- .shiftedBy(2. * context.initialOrbit.getKeplerianPeriod()));
-
- // Number of samples for the statistics
- final int sampleNumber = 10000;
-
- // FIXME: Relative tolerance = 87% so that the first two tests pass
- final double relativeTolerance = 87e-2;;
-
- // dt = 1/2 Torb, error on Vy is 2.63%
- checkBugCovarianceValue(false, state0, state1, processNoise, sampleNumber, relativeTolerance, false);
- // dt = 1 Torb, error on Vy is 26%
- checkBugCovarianceValue(false, state0, state2, processNoise, sampleNumber, relativeTolerance, false);
- // dt = 2Torb, with a wrong velocity composition, it works well, error on Vy is about 0.26%
- checkBugCovarianceValue(false, state0, state3, processNoise, sampleNumber, relativeTolerance, true);
- // dt = 2Torb, error on Vy is 87%
- checkBugCovarianceValue(false, state0, state3, processNoise, sampleNumber, relativeTolerance, false);
-
- }
-
- /** Bugs with Keplerian formalism, all components are out. */
- @Ignore
- @Test
- public void testBugKeplerian() {
-
- // Create context
- Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
-
- // Create initial orbit and propagator builder
- final OrbitType orbitType = OrbitType.KEPLERIAN;
- final PositionAngle positionAngle = PositionAngle.TRUE;
- final boolean perfectStart = true;
- final double minStep = 1.e-6;
- final double maxStep = 60.;
- final double dP = 1.;
- final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart,
- minStep, maxStep, dP);
-
- // Create a propagator
- final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
- propagatorBuilder);
-
- // Define the univariate functions for the standard deviations
- final UnivariateFunction[] lofCartesianOrbitalParametersEvolution = new UnivariateFunction[6];
- // Evolution for position error
- lofCartesianOrbitalParametersEvolution[0] = new PolynomialFunction(new double[] {100., 0., 1e-4});
- lofCartesianOrbitalParametersEvolution[1] = new PolynomialFunction(new double[] {100., 1e-1, 0.});
- lofCartesianOrbitalParametersEvolution[2] = new PolynomialFunction(new double[] {100., 0., 0.});
- // Evolution for velocity error
- lofCartesianOrbitalParametersEvolution[3] = new PolynomialFunction(new double[] {1., 0., 1.e-6});
- lofCartesianOrbitalParametersEvolution[4] = new PolynomialFunction(new double[] {1., 1e-3, 0.});
- lofCartesianOrbitalParametersEvolution[5] = new PolynomialFunction(new double[] {1., 0., 0.});
-
- final UnivariateFunction[] propagationParametersEvolution =
- new UnivariateFunction[] {new PolynomialFunction(new double[] {10, 1., 1e-4}),
- new PolynomialFunction(new double[] {1000., 0., 0.})};
-
- // Create a dummy initial covariance
- final RealMatrix initialCovarianceMatrix = MatrixUtils.createRealIdentityMatrix(7);
-
- // Set the process noise object
- // Define input LOF and output position angle
- final LOFType lofType = LOFType.TNW;
- final UnivariateProcessNoise processNoise = new UnivariateProcessNoise(initialCovarianceMatrix,
- lofType,
- positionAngle,
- lofCartesianOrbitalParametersEvolution,
- propagationParametersEvolution);
- // Test on initial value, after 1 min and after 1 hour
- final SpacecraftState state0 = propagator.getInitialState();
- final SpacecraftState state1 = propagator.propagate(context.initialOrbit.getDate()
- .shiftedBy(context.initialOrbit.getKeplerianPeriod()/4.));
-
-// //FIXME: debug
-// final Orbit initialOrbit = propagator.getInitialState().getOrbit();
-// final double posNorm = initialOrbit.getPVCoordinates().getPosition().getNorm();
-// final double velNorm = initialOrbit.getPVCoordinates().getVelocity().getNorm();
-// final Orbit orbit1 = new CartesianOrbit(new PVCoordinates(new Vector3D(posNorm, 0, 0),
-// new Vector3D(0, 0, velNorm)),
-// initialOrbit.getFrame(),
-// initialOrbit.getDate(), initialOrbit.getMu());
-// final SpacecraftState state1 = new SpacecraftState(orbitType.convertType(orbit1));
-// final SpacecraftState state0 = state1.shiftedBy(-initialOrbit.getKeplerianPeriod()/4.);
-// // debug
-
- // Number of samples for the statistics
- final int sampleNumber = 10000;
-
- // FIXME: Relative tolerance = 134% so that the first two tests pass
- final double relativeTolerance = 134e-2;;
+ if (print) {
+ System.out.println("Orbit Type : " + orbitType);
+ System.out.println("Position Angle: " + positionAngle);
+ }
- checkBugCovarianceValue(false, state0, state0, processNoise, sampleNumber, relativeTolerance, false);
- checkBugCovarianceValue(false, state0, state1, processNoise, sampleNumber, relativeTolerance, false);
- checkBugCovarianceValue(false, state0, state1, processNoise, sampleNumber, relativeTolerance, true);
+ // Worst case, KEPLERIAN MEAN - 1.62e-11
+ checkLofOrbitalCovarianceMatrix(print, state0, state0, processNoise, 1.62e-11);
+ // Worst case, CIRCULAR ECCENTRIC - 2.78e-10
+ checkLofOrbitalCovarianceMatrix(print, state0, state1, processNoise, 2.78e-10);
+ // Worst case, CIRCULAR TRUE - 9.15e-9
+ checkLofOrbitalCovarianceMatrix(print, state0, state2, processNoise, 9.15e-9);
}
- private static void checkBugCovarianceValue(final boolean print,
- final SpacecraftState previous,
- final SpacecraftState current,
- final UnivariateProcessNoise univariateProcessNoise,
- final int sampleNumber,
- final double relativeTolerance,
- final boolean wrongVelocityComposition) {
-
- // Get the process noise matrix
- final RealMatrix processNoiseMatrix = univariateProcessNoise.getProcessNoiseMatrix(previous, current);
-
- // Initialize a random vector generator
- final CorrelatedRandomVectorGenerator randomVectorGenerator = createSampler(processNoiseMatrix);
-
- // Propagation parameters length
- int propagationParametersLength = processNoiseMatrix.getColumnDimension() - 6;
- if ( propagationParametersLength < 0) {
- propagationParametersLength = 0;
- }
-
- // Prepare the statistics
- final StreamingStatistics[] orbitalStatistics = new StreamingStatistics[6];
+ /** Check orbital covariance matrix in LOF wrt reference.
+ * Here we do the reverse calculcation
+ */
+ private void checkLofOrbitalCovarianceMatrix(final boolean print,
+ final SpacecraftState previous,
+ final SpacecraftState current,
+ final UnivariateProcessNoise univariateProcessNoise,
+ final double relativeTolerance) {
+
+ // Get the process noise matrix in inertial frame
+ final RealMatrix inertialQ = univariateProcessNoise.getProcessNoiseMatrix(previous, current);
+
+ // Extract the orbit part
+ final RealMatrix inertialOrbitalQ = inertialQ.getSubMatrix(0, 5, 0, 5);
+
+ // Jacobian from parameters to Cartesian
+ final double[][] dCdY = new double[6][6];
+ current.getOrbit().getJacobianWrtParameters(univariateProcessNoise.getPositionAngle(), dCdY);
+ final RealMatrix jacOrbToCar = new Array2DRowRealMatrix(dCdY, false);
+
+ // Jacobian from inertial to LOF
+ final double[][] dLOFdI = new double[6][6];
+ univariateProcessNoise.getLofType().transformFromInertial(current.getDate(),
+ current.getOrbit().getPVCoordinates()).getJacobian(CartesianDerivativesFilter.USE_PV, dLOFdI);
+ final RealMatrix jacIToLOF = new Array2DRowRealMatrix(dLOFdI, false);
+
+ // Complete Jacobian
+ final RealMatrix jac = jacIToLOF.multiply(jacOrbToCar);
+
+ // Q in LOF and Cartesian
+ final RealMatrix lofQ = jac.multiply(inertialOrbitalQ).multiplyTransposed(jac);
+
+ // Build reference LOF Cartesian orbital sigmas
+ final RealVector refLofSig = new ArrayRealVector(6);
+ double dt = current.getDate().durationFrom(previous.getDate());
for (int i = 0; i < 6; i++) {
- orbitalStatistics[i] = new StreamingStatistics();
- }
- StreamingStatistics[] propagationStatistics;
- if (propagationParametersLength > 0) {
- propagationStatistics = new StreamingStatistics[propagationParametersLength];
- for (int i = 0; i < propagationParametersLength; i++) {
- propagationStatistics[i] = new StreamingStatistics();
- }
- } else {
- propagationStatistics = null;
+ refLofSig.setEntry(i, univariateProcessNoise.getLofCartesianOrbitalParametersEvolution()[i].value(dt));
}
-
- // Current orbit stored in an array
- // With the position angle defined in the univariate process noise function
- final double[] currentOrbitArray = new double[6];
- current.getOrbit().getType().mapOrbitToArray(current.getOrbit(),
- univariateProcessNoise.getPositionAngle(),
- currentOrbitArray,
- null);
- // Transform from inertial to current spacecraft LOF frame
- final Transform inertialToLof = univariateProcessNoise.getLofType().transformFromInertial(current.getDate(),
- current.getOrbit().getPVCoordinates());
- // Create the vectors and compute the stats
- for (int i = 0; i < sampleNumber; i++) {
-
- // Create a random vector
- final double[] randomVector = randomVectorGenerator.nextVector();
-
- // Orbital parameters values
- // -------------------------
-
- // Get the full inertial orbit by adding up the values of current orbit and orbit error (first 6 components of random vector)
- final double[] modifiedOrbitArray = new double[6];
+
+ // Compare diagonal values with reference (relative difference) and suppress them from the matrix
+ // Ensure non-diagonal values are into numerical noise sensitivity
+ RealVector dLofDiag = new ArrayRealVector(6);
+ for (int i = 0; i < 6; i++) {
for (int j = 0; j < 6; j++) {
- modifiedOrbitArray[j] = currentOrbitArray[j] + randomVector[j];
- }
-
- // Get the corresponding PV coordinates
- final TimeStampedPVCoordinates inertialPV =
- current.getOrbit().getType().mapArrayToOrbit(modifiedOrbitArray,
- null,
- univariateProcessNoise.getPositionAngle(),
- current.getDate(),
- current.getMu(),
- current.getFrame())
- .getPVCoordinates();
-
-
- // Transform from inertial to current LOF
- // The values obtained should be the Cartesian error vector in LOF (w/r to current spacecraft position)
- final PVCoordinates lofPV;
- if (!wrongVelocityComposition) {
- // Proper velocity composition
- lofPV = inertialToLof.transformPVCoordinates(inertialPV);
- } else {
- // Wrong velocity composition, ignoring orbital angular velocity
- final Vector3D pos = inertialToLof.transformPosition(inertialPV.getPosition());
- final Vector3D vel = inertialToLof.transformVector(inertialPV.getVelocity());
- lofPV = new PVCoordinates(pos, vel);
- }
-
- // Store the LOF PV values in the statistics summaries
- orbitalStatistics[0].addValue(lofPV.getPosition().getX());
- orbitalStatistics[1].addValue(lofPV.getPosition().getY());
- orbitalStatistics[2].addValue(lofPV.getPosition().getZ());
- orbitalStatistics[3].addValue(lofPV.getVelocity().getX());
- orbitalStatistics[4].addValue(lofPV.getVelocity().getY());
- orbitalStatistics[5].addValue(lofPV.getVelocity().getZ());
-
- // Propagation parameters
- // ----------------------
-
- // Extract the propagation parameters random vector and store them in the statistics
- if (propagationParametersLength > 0) {
- for (int j = 6; j < randomVector.length; j++) {
- propagationStatistics[j - 6].addValue(randomVector[j]);
+ double refI = refLofSig.getEntry(i);
+ if (i == j) {
+ // Diagonal term
+ dLofDiag.setEntry(i, FastMath.abs(1. - lofQ.getEntry(i, i) / refI / refI));
+ lofQ.setEntry(i, i, 0.);
+ } else {
+ // Non-diagonal term, ref is 0.
+ lofQ.setEntry(i, j, FastMath.abs(lofQ.getEntry(i,j) / refI / refLofSig.getEntry(j)));
}
- }
- }
-
- // Get the real values and the statistics
- // --------------------------------------
-
- // DT
- final double dt = current.getDate().durationFrom(previous.getDate());
-
- // Get the values of the orbital functions and the statistics
- final double[] orbitalValues = new double[6];
- final double[] orbitalStatisticsValues = new double[6];
- final double[] orbitalRelativeValues = new double[6];
-
- for (int i = 0; i < 6; i++) {
- orbitalValues[i] = FastMath.abs(univariateProcessNoise.getLofCartesianOrbitalParametersEvolution()[i].value(dt));
- orbitalStatisticsValues[i] = orbitalStatistics[i].getStandardDeviation();
-
- if (FastMath.abs(orbitalValues[i]) > 1e-15) {
- orbitalRelativeValues[i] = FastMath.abs(1. - orbitalStatisticsValues[i]/orbitalValues[i]);
- } else {
- orbitalRelativeValues[i] = orbitalStatisticsValues[i];
- }
- }
-
- // Get the values of the propagation functions and statistics
- final double[] propagationValues = new double[propagationParametersLength];
- final double[] propagationStatisticsValues = new double[propagationParametersLength];
- final double[] propagationRelativeValues = new double[propagationParametersLength];
- for (int i = 0; i < propagationParametersLength; i++) {
- propagationValues[i] = FastMath.abs(univariateProcessNoise.getPropagationParametersEvolution()[i].value(dt));
- propagationStatisticsValues[i] = propagationStatistics[i].getStandardDeviation();
- if (FastMath.abs(propagationValues[i]) > 1e-15) {
- propagationRelativeValues[i] = FastMath.abs(1. - propagationStatisticsValues[i]/propagationValues[i]);
- } else {
- propagationRelativeValues[i] = propagationStatisticsValues[i];
}
+
}
-
+
+ // Norm of diagonal and non-diagonal
+ double dDiag = dLofDiag.getNorm();
+ double dNonDiag = lofQ.getNorm();
+
+ // Print values ?
if (print) {
- // FIXME: Test the values
- System.out.println("dt = " + dt + " / N = " + sampleNumber + "\n");
- if (wrongVelocityComposition) {
- System.out.println("Using wrong velocity computation here" + "\n");
- }
-
- System.out.println("σ orbit ref = " + Arrays.toString(orbitalValues));
- System.out.println("σ orbit stat = " + Arrays.toString(orbitalStatisticsValues));
- System.out.println("σ orbit % = " + Arrays.toString(Arrays.stream(orbitalRelativeValues).map(i -> i*100.).toArray()) + "\n");
-
- System.out.println("σ propag ref = " + Arrays.toString(propagationValues));
- System.out.println("σ propag stat = " + Arrays.toString(propagationStatisticsValues));
- System.out.println("σ propag % = " + Arrays.toString(Arrays.stream(propagationRelativeValues).map(i -> i*100.).toArray()) + "\n");
- //debug
+ System.out.println("\tdt = " + dt);
+ System.out.format(Locale.US, "\tΔDiagonal norm in Cartesian LOF = %10.4e%n", dDiag);
+ System.out.format(Locale.US, "\tΔNon-Diagonal norm in Cartesian LOF = %10.4e%n%n", dNonDiag);
}
-
- assertArrayEquals(new double[6],
- orbitalRelativeValues,
- relativeTolerance);
- assertArrayEquals(new double[propagationParametersLength],
- propagationRelativeValues,
- relativeTolerance);
+ Assert.assertEquals(0., dDiag , relativeTolerance);
+ Assert.assertEquals(0., dNonDiag, relativeTolerance);
}
}