Skip to content

GitLab

Commit 4ff58edd authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

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

parent 8be4fba3
Hide whitespace changes
Inline Side-by-side
src/test/java/org/orekit/estimation/sequential/ExtendedSemiAnalyticalKalmanFilterTest.java
View file @ 4ff58edd
......@@ -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
View file @ 4ff58edd
......@@ -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());
}
}
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