Commit 6364a7e3 authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

Improved test coverage.

parent b66ed411
Pipeline #1653 passed with stages
in 26 minutes and 21 seconds
......@@ -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;
}
}
}
......@@ -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. */
......
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;
@Override
public void evaluationPerformed(KalmanEstimation estimation) {
this.estimation = estimation;
}
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment