Improved test coverage.

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

@@ -159,6 +159,14 @@ public class ExtendedSemiAnalyticalKalmanFilterTest {
         Assert.assertEquals(0.0, statY.getMax(),  0.029);
         Assert.assertEquals(0.0, statX.getMax(),  0.033);
 
+        // Check that "physical" matrices are null
+        final KalmanEstimation estimation = observer.getEstimation();
+        Assert.assertNotNull(estimation.getPhysicalEstimatedState());
+        Assert.assertNotNull(estimation.getPhysicalInnovationCovarianceMatrix());
+        Assert.assertNotNull(estimation.getPhysicalKalmanGain());
+        Assert.assertNotNull(estimation.getPhysicalMeasurementJacobian());
+        Assert.assertNotNull(estimation.getPhysicalStateTransitionMatrix());
+
     }
 
     /**
@@ -474,6 +482,9 @@ public class ExtendedSemiAnalyticalKalmanFilterTest {
         private StreamingStatistics statY;
         private StreamingStatistics statZ;
 
+        /** Kalman estimation. */
+        private KalmanEstimation estimation;
+
         /**
          * Constructor.
          * @param startEpoch start epoch
@@ -526,8 +537,14 @@ public class ExtendedSemiAnalyticalKalmanFilterTest {
 
             }
 
+            this.estimation = estimation;
+
         }
 
+        /**
+         * Get the statistics on the X coordinate residuals.
+         * @return the statistics on the X coordinate residuals
+         */
         public StreamingStatistics getXStatistics() {
         	if (print) {
         		System.out.println("Min X res (m): " + statX.getMin() + " Max X res (m): " + statX.getMax() + " Mean X res (m): " + statX.getMean() + " STD: " + statX.getStandardDeviation());
@@ -535,6 +552,10 @@ public class ExtendedSemiAnalyticalKalmanFilterTest {
         	return statX;
         }
 
+        /**
+         * Get the statistics on the Y coordinate residuals.
+         * @return the statistics on the Y coordinate residuals
+         */
         public StreamingStatistics getYStatistics() {
         	if (print) {
         		System.out.println("Min Y res (m): " + statY.getMin() + " Max Y res (m): " + statY.getMax() + " Mean Y res (m): " + statY.getMean() + " STD: " + statY.getStandardDeviation());
@@ -542,6 +563,10 @@ public class ExtendedSemiAnalyticalKalmanFilterTest {
         	return statY;
         }
 
+        /**
+         * Get the statistics on the Z coordinate residuals.
+         * @return the statistics on the Z coordinate residuals
+         */
         public StreamingStatistics getZStatistics() {
         	if (print) {
         		System.out.println("Min Z res (m): " + statZ.getMin() + " Max Z res (m): " + statZ.getMax() + " Mean Z res (m): " + statZ.getMean() + " STD: " + statZ.getStandardDeviation());
@@ -549,6 +574,14 @@ public class ExtendedSemiAnalyticalKalmanFilterTest {
         	return statZ;
         }
 
+        /**
+         * Get the Kalman estimation.
+         * @return the Kalman estimation
+         */
+        public KalmanEstimation getEstimation() {
+            return estimation;
+        }
+
     }
 
 }

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

@@ -18,7 +18,10 @@ package org.orekit.estimation.sequential;
 
 import java.util.List;
 
+import org.hipparchus.exception.LocalizedCoreFormats;
+import org.hipparchus.exception.MathRuntimeException;
 import org.hipparchus.linear.MatrixUtils;
+import org.hipparchus.linear.QRDecomposer;
 import org.hipparchus.linear.RealMatrix;
 import org.hipparchus.stat.descriptive.StreamingStatistics;
 import org.hipparchus.util.FastMath;
@@ -43,6 +46,7 @@ import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
 import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
 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;
 
@@ -58,6 +62,64 @@ public class SemiAnalyticalKalmanEstimatorTest {
         }
     }
 
+    @Test
+    public void testMathRuntimeException() {
+
+        // Create context
+        DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
+        // Create initial orbit and DSST propagator builder
+        final OrbitType     orbitType     = OrbitType.EQUINOCTIAL;
+        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);
+
+        // Create perfect range measurements
+        final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
+        final List<ObservedMeasurement<?>> measurements =
+                        DSSTEstimationTestUtils.createMeasurements(propagator,
+                                                                   new DSSTRangeMeasurementCreator(context),
+                                                                   0.0, 6.0, 60.0);
+        // DSST propagator builder (used for orbit determination)
+        final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
+
+        // Equinictial covariance matrix initialization
+        final RealMatrix equinoctialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
+            0., 0., 0., 0., 0., 0.
+        });
+
+        // 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.MEAN, dYdC);
+        final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
+
+        // Equinoctial initial covariance matrix
+        final RealMatrix initialP = Jac.multiply(equinoctialP.multiply(Jac.transpose()));
+
+        // Process noise matrix is set to 0 here
+        RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
+
+        // Build the Kalman filter
+        final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
+                        addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
+                        decomposer(new QRDecomposer(1.0e-15)).
+                        build();
+        kalman.setObserver(estimation -> {
+            throw new MathRuntimeException(LocalizedCoreFormats.INTERNAL_ERROR, "me");
+        });
+
+        try {
+            kalman.processMeasurements(measurements);
+            Assert.fail("an exception should have been thrown");
+        } catch (OrekitException oe) {
+            Assert.assertEquals(LocalizedCoreFormats.INTERNAL_ERROR, oe.getSpecifier());
+        }
+    }
+
     /**
      * Perfect range measurements.
      * Only the Newtonian Attraction is used.
@@ -85,6 +147,7 @@ public class SemiAnalyticalKalmanEstimatorTest {
                         DSSTEstimationTestUtils.createMeasurements(propagator,
                                                                    new DSSTRangeMeasurementCreator(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);
@@ -132,6 +195,14 @@ public class SemiAnalyticalKalmanEstimatorTest {
                                            expectedDeltaVel, velEps);
 
         Assert.assertEquals(0.0, observer.getMeanResidual(), 4.98e-8);
+        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(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());
     }
 
     /**
@@ -162,6 +233,7 @@ public class SemiAnalyticalKalmanEstimatorTest {
                         DSSTEstimationTestUtils.createMeasurements(propagator,
                                                                    new DSSTRangeMeasurementCreator(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);
@@ -214,6 +286,14 @@ public class SemiAnalyticalKalmanEstimatorTest {
                                            expectedDeltaVel, velEps);
 
         Assert.assertEquals(0.0, observer.getMeanResidual(), 8.51-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(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());
     }
 
     /**
@@ -250,6 +330,7 @@ public class SemiAnalyticalKalmanEstimatorTest {
                         DSSTEstimationTestUtils.createMeasurements(propagator,
                                                                    new DSSTRangeMeasurementCreator(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);
@@ -306,6 +387,14 @@ public class SemiAnalyticalKalmanEstimatorTest {
                                            expectedDeltaVel, velEps);
 
         Assert.assertEquals(0.0, observer.getMeanResidual(), 8.81e-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(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());
     }
 
     /** Observer for Kalman estimation. */

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

+package org.orekit.estimation.sequential;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.hipparchus.linear.MatrixUtils;
+import org.hipparchus.linear.RealMatrix;
+import org.hipparchus.linear.RealVector;
+import org.junit.Assert;
+import org.junit.Before;
+import org.junit.Test;
+import org.orekit.estimation.DSSTContext;
+import org.orekit.estimation.DSSTEstimationTestUtils;
+import org.orekit.estimation.DSSTForce;
+import org.orekit.estimation.measurements.GroundStation;
+import org.orekit.estimation.measurements.ObservableSatellite;
+import org.orekit.estimation.measurements.Range;
+import org.orekit.estimation.measurements.modifiers.Bias;
+import org.orekit.forces.radiation.RadiationSensitive;
+import org.orekit.orbits.Orbit;
+import org.orekit.orbits.OrbitType;
+import org.orekit.orbits.PositionAngle;
+import org.orekit.propagation.PropagationType;
+import org.orekit.propagation.SpacecraftState;
+import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
+import org.orekit.time.AbsoluteDate;
+import org.orekit.utils.ParameterDriver;
+import org.orekit.utils.ParameterDriversList;
+
+public class SemiAnalyticalKalmanModelTest {
+
+    /** Orbit type for propagation. */
+    private final OrbitType orbitType = OrbitType.EQUINOCTIAL;
+    
+    /** Position angle for propagation. */
+    private final PositionAngle positionAngle = PositionAngle.MEAN;
+    
+    /** Initial orbit. */
+    private Orbit orbit0;
+    
+    /** Propagator builder. */
+    private DSSTPropagatorBuilder propagatorBuilder;
+    
+    /** Covariance matrix provider. */
+    private CovarianceMatrixProvider covMatrixProvider;
+    
+    /** Estimated measurement parameters list. */
+    private ParameterDriversList estimatedMeasurementsParameters;
+    
+    /** Kalman extended estimator containing models. */
+    private SemiAnalyticalKalmanEstimator kalman;
+    
+    /** Kalman observer. */
+    private ModelLogger modelLogger;
+    
+    /** State size. */
+    private int M;
+
+    /** Range after t0. */
+    private Range range;
+
+    /** Driver for satellite range bias. */
+    private ParameterDriver satRangeBiasDriver;
+    
+    /** Driver for SRP coefficient. */
+    private ParameterDriver srpCoefDriver;
+    
+    /** Tolerance for the test. */
+    private final double tol = 1e-16;
+    
+    @Before
+    public void setup() {
+
+        // Create context
+        DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
+        
+        // Initial orbit and date
+        this.orbit0 = context.initialOrbit;
+        ObservableSatellite sat = new ObservableSatellite(0);
+        
+        // Create propagator builder
+        this.propagatorBuilder = context.createBuilder(PropagationType.MEAN, PropagationType.OSCULATING, true,
+                                                       1.0e-6, 60.0, 10., DSSTForce.SOLAR_RADIATION_PRESSURE);
+
+        // Create PV at t0
+        final AbsoluteDate date0 = context.initialOrbit.getDate();
+        
+        // Create one 0m range measurement at t0 + 10s
+        final AbsoluteDate date  = date0.shiftedBy(10.);
+        final GroundStation station = context.stations.get(0);
+        this.range = new Range(station, true, date, 18616150., 10., 1., sat);
+        // Exact range value is 1.8616150246470984E7 m
+
+        // Add sat range bias to PV and select it
+        final Bias<Range> satRangeBias = new Bias<Range>(new String[] {"sat range bias"},
+                                                         new double[] {100.},
+                                                         new double[] {10.},
+                                                         new double[] {0.},
+                                                         new double[] {100.});
+        this.satRangeBiasDriver = satRangeBias.getParametersDrivers().get(0);
+        satRangeBiasDriver.setSelected(true);
+        satRangeBiasDriver.setReferenceDate(date);
+        range.addModifier(satRangeBias);
+        for (ParameterDriver driver : range.getParametersDrivers()) {
+            driver.setReferenceDate(date);
+        }
+
+        // Gather list of meas parameters (only sat range bias here)
+        this.estimatedMeasurementsParameters = new ParameterDriversList();
+        for (final ParameterDriver driver : range.getParametersDrivers()) {
+            if (driver.isSelected()) {
+                estimatedMeasurementsParameters.add(driver);
+            }
+        }
+        // Select SRP coefficient
+        this.srpCoefDriver = propagatorBuilder.getPropagationParametersDrivers().
+                        findByName(RadiationSensitive.REFLECTION_COEFFICIENT);
+        srpCoefDriver.setReferenceDate(date);
+        srpCoefDriver.setSelected(true);
+        
+        // Create a covariance matrix using the scales of the estimated parameters
+        final double[] scales = getParametersScale(propagatorBuilder, estimatedMeasurementsParameters);
+        this.M = scales.length;
+        this.covMatrixProvider = setInitialCovarianceMatrix(scales);
+
+        // Initialize Kalman
+        final SemiAnalyticalKalmanEstimatorBuilder kalmanBuilder = new SemiAnalyticalKalmanEstimatorBuilder();
+        kalmanBuilder.addPropagationConfiguration(propagatorBuilder, covMatrixProvider);
+        kalmanBuilder.estimatedMeasurementsParameters(estimatedMeasurementsParameters, null);
+        this.kalman = kalmanBuilder.build();
+        this.modelLogger = new ModelLogger();
+        kalman.setObserver(modelLogger);
+    }
+
+    @Test
+    public void ModelPhysicalOutputsTest() {
+        
+        // Check model at t0 before any measurement is added
+        // -------------------------------------------------
+        checkModelAtT0();
+
+    }
+
+    /** Check the model physical outputs at t0 before any measurement is added. */
+    private void checkModelAtT0() {
+
+        // Instantiate a Model from attributes
+        final SemiAnalyticalKalmanModel model = new SemiAnalyticalKalmanModel(propagatorBuilder,
+                                                  covMatrixProvider,
+                                                  estimatedMeasurementsParameters,
+                                                  null);
+
+        // Evaluate at t0
+        // --------------
+        
+        // Time
+        Assert.assertEquals(0., model.getEstimate().getTime(), 0.);
+        Assert.assertEquals(0., model.getCurrentDate().durationFrom(orbit0.getDate()), 0.);
+        
+        // Measurement number
+        Assert.assertEquals(0, model.getCurrentMeasurementNumber());
+        
+        // Normalized state - is zeros
+        final RealVector stateN = model.getEstimate().getState();
+        Assert.assertArrayEquals(new double[M], stateN.toArray(), tol);
+        
+        // Physical state - = initialized
+        final RealVector x = model.getPhysicalEstimatedState();
+        final RealVector expX = MatrixUtils.createRealVector(M);
+        final double[] orbitState0 = new double[6];
+        orbitType.mapOrbitToArray(orbit0, positionAngle, orbitState0, null);
+        expX.setSubVector(0, MatrixUtils.createRealVector(orbitState0));
+        expX.setEntry(6, srpCoefDriver.getReferenceValue());
+        expX.setEntry(7, satRangeBiasDriver.getReferenceValue());
+        final double[] dX = x.subtract(expX).toArray();
+        Assert.assertArrayEquals(new double[M], dX, tol);
+        
+        // Normalized covariance - filled with 1
+        final double[][] Pn = model.getEstimate().getCovariance().getData();
+        final double[][] expPn = new double[M][M];
+        for (int i = 0; i < M; i++) {
+            for (int j = 0; j < M; j++) {
+                expPn[i][j] = 1.;
+            }
+            Assert.assertArrayEquals("Failed on line " + i, expPn[i], Pn[i], tol);
+        }
+        
+        // Physical covariance = initialized
+        final RealMatrix P   = model.getPhysicalEstimatedCovarianceMatrix();
+        final RealMatrix expP = covMatrixProvider.getInitialCovarianceMatrix(new SpacecraftState(orbit0));
+        final double[][] dP = P.subtract(expP).getData();
+        for (int i = 0; i < M; i++) {
+            Assert.assertArrayEquals("Failed on line " + i, new double[M], dP[i], tol);
+        }
+        
+        // Check that other "physical" matrices are null
+        Assert.assertNull(model.getEstimate().getInnovationCovariance());
+        Assert.assertNull(model.getPhysicalInnovationCovarianceMatrix());
+        Assert.assertNull(model.getEstimate().getKalmanGain());
+        Assert.assertNull(model.getPhysicalKalmanGain());
+        Assert.assertNull(model.getEstimate().getMeasurementJacobian());
+        Assert.assertNull(model.getPhysicalMeasurementJacobian());
+        Assert.assertNull(model.getEstimate().getStateTransitionMatrix());
+        Assert.assertNull(model.getPhysicalStateTransitionMatrix());
+    }
+    
+    /** Get an array of the scales of the estimated parameters.
+     * @param builder propagator builder
+     * @param estimatedMeasurementsParameters estimated measurements parameters
+     * @return array containing the scales of the estimated parameter
+     */
+    private double[] getParametersScale(final DSSTPropagatorBuilder builder,
+                                        final ParameterDriversList estimatedMeasurementsParameters) {
+        final List<Double> scaleList = new ArrayList<>();
+        
+        // Orbital parameters
+        for (ParameterDriver driver : builder.getOrbitalParametersDrivers().getDrivers()) {
+            if (driver.isSelected()) {
+                scaleList.add(driver.getScale());
+            }
+        }
+        
+        // Propagation parameters
+        for (ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
+            if (driver.isSelected()) {
+                scaleList.add(driver.getScale());
+            }
+        }
+        
+        // Measurement parameters
+        for (ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
+            if (driver.isSelected()) {
+                scaleList.add(driver.getScale());
+            }
+        }
+        
+        final double[] scales = new double[scaleList.size()];
+        for (int i = 0; i < scaleList.size(); i++) {
+            scales[i] = scaleList.get(i);
+        }
+        return scales;
+    }
+    
+    /** Create a covariance matrix provider with initial and process noise matrix constant and identical.
+     * Each element Pij of the initial covariance matrix equals:
+     * Pij = scales[i]*scales[j]
+     * @param scales scales of the estimated parameters
+     * @return covariance matrix provider
+     */
+    private CovarianceMatrixProvider setInitialCovarianceMatrix(final double[] scales) {
+
+        final int n = scales.length;
+        final RealMatrix cov = MatrixUtils.createRealMatrix(n, n);
+        for (int i = 0; i < n; i++) {
+            for (int j = 0; j < n; j++) {
+                cov.setEntry(i, j, scales[i] * scales[j]);
+            }
+        }
+        return new ConstantProcessNoise(cov); 
+    }
+    
+    
+    /** Observer allowing to get Kalman model after a measurement was processed in the Kalman filter. */
+    public class ModelLogger implements KalmanObserver {
+        KalmanEstimation estimation;
+