Improved test coverage of the extended semi-analytical Kalman filter.

src/test/java/org/orekit/estimation/sequential/ExtendedSemiAnalyticalKalmanFilterTest.java

@@ -71,7 +71,12 @@ import org.orekit.utils.IERSConventions;
 import org.orekit.utils.ParameterDriver;
 import org.orekit.utils.TimeStampedPVCoordinates;
 
-
+/**
+ * Validation against real data of the ESKF. This test is a short version of the one presented in:
+ * "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
+ *  Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
+ *  Specialist Conference, Big Sky, August 2021."
+ */
 public class ExtendedSemiAnalyticalKalmanFilterTest {
 
 	/** Print. */

src/test/java/org/orekit/estimation/sequential/SemiAnalyticalKalmanEstimatorTest.java

@@ -32,9 +32,11 @@ import org.orekit.errors.OrekitMessages;
 import org.orekit.estimation.DSSTContext;
 import org.orekit.estimation.DSSTEstimationTestUtils;
 import org.orekit.estimation.measurements.EstimatedMeasurement;
+import org.orekit.estimation.measurements.GroundStation;
 import org.orekit.estimation.measurements.ObservedMeasurement;
 import org.orekit.estimation.measurements.Range;
 import org.orekit.estimation.measurements.RangeMeasurementCreator;
+import org.orekit.estimation.measurements.modifiers.DynamicOutlierFilter;
 import org.orekit.forces.gravity.potential.GravityFieldFactory;
 import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
 import org.orekit.orbits.Orbit;
@@ -44,11 +46,13 @@ import org.orekit.propagation.PropagationType;
 import org.orekit.propagation.Propagator;
 import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
 import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
+import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
 import org.orekit.propagation.semianalytical.dsst.forces.DSSTTesseral;
 import org.orekit.propagation.semianalytical.dsst.forces.DSSTZonal;
 import org.orekit.time.AbsoluteDate;
 import org.orekit.utils.Constants;
 import org.orekit.utils.ParameterDriver;
+import org.orekit.utils.ParameterDriversList;
 
 public class SemiAnalyticalKalmanEstimatorTest {
 
@@ -123,6 +127,9 @@ public class SemiAnalyticalKalmanEstimatorTest {
     /**
      * Perfect range measurements.
      * Only the Newtonian Attraction is used.
+     * Case 1 of : "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
+     *              Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
+     *              Specialist Conference, Big Sky, August 2021."
      */
     @Test
     public void testKeplerianRange() {
@@ -208,6 +215,9 @@ public class SemiAnalyticalKalmanEstimatorTest {
     /**
      * Perfect range measurements.
      * J20 is added to the perturbation model compare to the previous test
+     * Case 2 of : "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
+     *              Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
+     *              Specialist Conference, Big Sky, August 2021."
      */
     @Test
     public void testRangeWithZonal() {
@@ -285,7 +295,7 @@ public class SemiAnalyticalKalmanEstimatorTest {
                                            expectedDeltaPos, posEps,
                                            expectedDeltaVel, velEps);
 
-        Assert.assertEquals(0.0, observer.getMeanResidual(), 8.51-3);
+        Assert.assertEquals(0.0, observer.getMeanResidual(), 8.51e-3);
         Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
         Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
         Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
@@ -300,6 +310,9 @@ public class SemiAnalyticalKalmanEstimatorTest {
      * Perfect range measurements.
      * J20 is added to the perturbation model
      * In addition, J21 and J22 are also added
+     * Case 3 of : "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
+     *              Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
+     *              Specialist Conference, Big Sky, August 2021."
      */
     @Test
     public void testRangeWithTesseral() {
@@ -435,4 +448,208 @@ public class SemiAnalyticalKalmanEstimatorTest {
 
     }
 
+    @Test
+    public void testWithEstimatedPropagationParameters() {
+
+        // Create context
+        DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
+
+        // Create initial orbit and propagator builder
+        final OrbitType     orbitType     = OrbitType.EQUINOCTIAL;
+        final PositionAngle positionAngle = PositionAngle.MEAN;
+        final boolean       perfectStart  = true;
+        final double        minStep       = 120.0;
+        final double        maxStep       = 1200.0;
+        final double        dP            = 1.;
+
+        // Propagator builder for measurement generation
+        final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
+        final DSSTForceModel zonal = new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0));
+        zonal.getParametersDrivers().get(0).setSelected(true);
+        builder.addForceModel(zonal);
+
+        // Create perfect range measurements
+        final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
+        final List<ObservedMeasurement<?>> measurements =
+                        DSSTEstimationTestUtils.createMeasurements(propagator,
+                                                                   new RangeMeasurementCreator(context),
+                                                                   0.0, 6.0, 60.0);
+        final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
+
+        // DSST propagator builder (used for orbit determination)
+        final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
+        propagatorBuilder.addForceModel(zonal);
+
+        // Reference propagator for estimation performances
+        final DSSTPropagator referencePropagator = propagatorBuilder.
+                        buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
+        
+        // Reference position/velocity at last measurement date
+        final Orbit refOrbit = referencePropagator.
+                        propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
+
+        // Cartesian covariance matrix initialization
+        // 100m on position / 1e-2m/s on velocity 
+        final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
+            100., 100., 100., 1e-2, 1e-2, 1e-2
+        });
+
+        // Covariance matrix on propagation parameters
+        final RealMatrix propagationP = MatrixUtils.createRealDiagonalMatrix(new double [] {
+            1.0e-10
+        });
+
+        // Jacobian of the orbital parameters w/r to Cartesian
+        final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
+        final double[][] dYdC = new double[6][6];
+        initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
+        final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
+        final RealMatrix orbitalP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
+        
+        // Keplerian initial covariance matrix
+        final RealMatrix initialP = MatrixUtils.createRealMatrix(7, 7);
+        initialP.setSubMatrix(orbitalP.getData(), 0, 0);
+        initialP.setSubMatrix(propagationP.getData(), 6, 6);
+
+        // Process noise matrix is set to 0 here
+        RealMatrix Q = MatrixUtils.createRealMatrix(7, 7);
+
+        // Build the Kalman filter
+        final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
+                        addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
+                        build();
+        final Observer observer = new Observer();
+        kalman.setObserver(observer);
+
+        // Filter the measurements and check the results
+        final double   expectedDeltaPos  = 0.;
+        final double   posEps            = 4.9e-2;
+        final double   expectedDeltaVel  = 0.;
+        final double   velEps            = 1.6e-5;
+        DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
+                                           refOrbit, positionAngle,
+                                           expectedDeltaPos, posEps,
+                                           expectedDeltaVel, velEps);
+
+        Assert.assertEquals(0.0, observer.getMeanResidual(), 1.79e-3);
+        Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
+        Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
+        Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
+        Assert.assertEquals(1, kalman.getPropagationParametersDrivers(true).getNbParams());
+        Assert.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
+        Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
+        Assert.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
+        Assert.assertNotNull(kalman.getPhysicalEstimatedState());
+    }
+
+    @Test
+    public void testWithEstimatedMeasurementParameters() {
+
+        // Create context
+        DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
+
+        // Create initial orbit and propagator builder
+        final OrbitType     orbitType     = OrbitType.EQUINOCTIAL;
+        final PositionAngle positionAngle = PositionAngle.MEAN;
+        final boolean       perfectStart  = true;
+        final double        minStep       = 120.0;
+        final double        maxStep       = 1200.0;
+        final double        dP            = 1.;
+
+        // Propagator builder for measurement generation
+        final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
+        final DSSTForceModel zonal = new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0));
+        builder.addForceModel(zonal);
+
+        // Create perfect range measurements
+        final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
+        final ParameterDriversList estimatedDrivers = new ParameterDriversList();
+        final double groundClockDrift =  4.8e-9;
+        for (final GroundStation station : context.stations) {
+            station.getClockOffsetDriver().setValue(groundClockDrift);
+            station.getClockOffsetDriver().setSelected(true);
+            estimatedDrivers.add(station.getClockOffsetDriver());
+        }
+        final List<ObservedMeasurement<?>> measurements =
+                        DSSTEstimationTestUtils.createMeasurements(propagator,
+                                                                   new RangeMeasurementCreator(context),
+                                                                   0.0, 6.0, 60.0);
+        final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
+
+        // Create outlier filter
+        final DynamicOutlierFilter<Range> filter = new DynamicOutlierFilter<>(10, 1.0);
+        for (ObservedMeasurement<?> measurement : measurements) {
+            Range range = (Range) measurement;
+            range.addModifier(filter);
+        }
+
+        // DSST propagator builder (used for orbit determination)
+        final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
+        propagatorBuilder.addForceModel(zonal);
+
+        // Reference propagator for estimation performances
+        final DSSTPropagator referencePropagator = propagatorBuilder.
+                        buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
+        
+        // Reference position/velocity at last measurement date
+        final Orbit refOrbit = referencePropagator.
+                        propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
+
+        // Cartesian covariance matrix initialization
+        // 100m on position / 1e-2m/s on velocity 
+        final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
+            100., 100., 100., 1e-2, 1e-2, 1e-2
+        });
+
+        // Jacobian of the orbital parameters w/r to Cartesian
+        final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
+        final double[][] dYdC = new double[6][6];
+        initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
+        final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
+        final RealMatrix orbitalP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
+        
+        // Keplerian initial covariance matrix
+        final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
+        initialP.setSubMatrix(orbitalP.getData(), 0, 0);
+
+        // Process noise matrix is set to 0 here
+        RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
+
+        // Initial measurement covariance matrix and process noise matrix
+        final RealMatrix measurementP = MatrixUtils.createRealDiagonalMatrix(new double [] {
+           1.0e-15, 1.0e-15
+        });
+        final RealMatrix measurementQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
+            1.0e-25, 1.0e-25
+        });
+
+        // Build the Kalman filter
+        final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
+                        addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
+                        estimatedMeasurementsParameters(estimatedDrivers, new ConstantProcessNoise(measurementP, measurementQ)).
+                        build();
+        final Observer observer = new Observer();
+        kalman.setObserver(observer);
+
+        // Filter the measurements and check the results
+        final double   expectedDeltaPos  = 0.;
+        final double   posEps            = 4.9e-2;
+        final double   expectedDeltaVel  = 0.;
+        final double   velEps            = 1.6e-5;
+        DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
+                                           refOrbit, positionAngle,
+                                           expectedDeltaPos, posEps,
+                                           expectedDeltaVel, velEps);
+
+        Assert.assertEquals(0.0, observer.getMeanResidual(), 1.79e-3);
+        Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
+        Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
+        Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
+        Assert.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
+        Assert.assertEquals(2, kalman.getEstimatedMeasurementsParameters().getNbParams());
+        Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
+        Assert.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
+        Assert.assertNotNull(kalman.getPhysicalEstimatedState());
+    }
+
 }