use of org.orekit.orbits.OrbitType in project Orekit by CS-SI.
the class KalmanEstimatorTest method testCartesianRangeRate.
/**
* Perfect range rate measurements with a perfect start
* Cartesian formalism
* @throws OrekitException
*/
@Test
public void testCartesianRangeRate() throws OrekitException {
// Create context
Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
// Create initial orbit and propagator builder
final OrbitType orbitType = OrbitType.CARTESIAN;
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 perfect range measurements
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeRateMeasurementCreator(context, false), 1.0, 3.0, 300.0);
// Reference propagator for estimation performances
final NumericalPropagator 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[] { 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-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);
// Initial covariance matrix
final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
// Process noise matrix
final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12 });
final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
// Build the Kalman filter
final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
kalmanBuilder.builder(propagatorBuilder);
kalmanBuilder.estimatedMeasurementsParameters(new ParameterDriversList());
kalmanBuilder.initialCovarianceMatrix(initialP);
kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(Q));
final KalmanEstimator kalman = kalmanBuilder.build();
// Filter the measurements and check the results
final double expectedDeltaPos = 0.;
final double posEps = 9.50e-4;
final double expectedDeltaVel = 0.;
final double velEps = 3.49e-7;
final double[] expectedSigmasPos = { 0.324398, 1.347031, 1.743310 };
final double sigmaPosEps = 1e-6;
final double[] expectedSigmasVel = { 2.856883e-4, 5.765844e-4, 5.056186e-4 };
final double sigmaVelEps = 1e-10;
EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedSigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}
use of org.orekit.orbits.OrbitType in project Orekit by CS-SI.
the class KalmanEstimatorTest method testCircularAzimuthElevation.
/**
* Perfect azimuth/elevation measurements with a perfect start
* Circular formalism
* @throws OrekitException
*/
@Test
public void testCircularAzimuthElevation() throws OrekitException {
// Create context
Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
// Create initial orbit and propagator builder
final OrbitType orbitType = OrbitType.CIRCULAR;
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 perfect range measurements
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new AngularAzElMeasurementCreator(context), 1.0, 4.0, 60.0);
// Reference propagator for estimation performances
final NumericalPropagator 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
final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-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);
// Initial covariance matrix
final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
// Process noise matrix
final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12 });
final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
// Build the Kalman filter
final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
kalmanBuilder.builder(propagatorBuilder);
kalmanBuilder.estimatedMeasurementsParameters(new ParameterDriversList());
kalmanBuilder.initialCovarianceMatrix(initialP);
kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(Q));
final KalmanEstimator kalman = kalmanBuilder.build();
// Filter the measurements and check the results
final double expectedDeltaPos = 0.;
final double posEps = 4.78e-7;
final double expectedDeltaVel = 0.;
final double velEps = 1.54e-10;
final double[] expectedSigmasPos = { 0.356902, 1.297507, 1.798551 };
final double sigmaPosEps = 1e-6;
final double[] expectedSigmasVel = { 2.468745e-4, 5.810027e-4, 3.887394e-4 };
final double sigmaVelEps = 1e-10;
EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedSigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}
use of org.orekit.orbits.OrbitType in project Orekit by CS-SI.
the class KalmanEstimatorTest method testKeplerianRange.
/**
* Perfect range measurements with a biased start
* Keplerian formalism
* @throws OrekitException
*/
@Test
public void testKeplerianRange() throws OrekitException {
// 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 perfect range measurements
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context), 1.0, 4.0, 60.0);
// Reference propagator for estimation performances
final NumericalPropagator referencePropagator = propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
// Reference position/velocity at last measurement date
final Orbit refOrbit = referencePropagator.propagate(measurements.get(measurements.size() - 1).getDate()).getOrbit();
// Change semi-major axis of 1.2m as in the batch test
ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
aDriver.setValue(aDriver.getValue() + 1.2);
aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
// 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);
// Keplerian initial covariance matrix
final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
// Process noise matrix is set to 0 here
RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
// Build the Kalman filter
final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
kalmanBuilder.builder(propagatorBuilder);
kalmanBuilder.estimatedMeasurementsParameters(new ParameterDriversList());
kalmanBuilder.initialCovarianceMatrix(initialP);
kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(Q));
final KalmanEstimator kalman = kalmanBuilder.build();
// Filter the measurements and check the results
final double expectedDeltaPos = 0.;
final double posEps = 1.77e-4;
final double expectedDeltaVel = 0.;
final double velEps = 7.93e-8;
final double[] expectedSigmasPos = { 0.742488, 0.281910, 0.563217 };
final double sigmaPosEps = 1e-6;
final double[] expectedSigmasVel = { 2.206622e-4, 1.306669e-4, 1.293996e-4 };
final double sigmaVelEps = 1e-10;
EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedSigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}
use of org.orekit.orbits.OrbitType in project Orekit by CS-SI.
the class KalmanEstimatorTest method testWrappedException.
/**
* Test of a wrapped exception in a Kalman observer
* @throws OrekitException
*/
@Test
public void testWrappedException() throws OrekitException {
// 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 perfect range measurements
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context), 1.0, 3.0, 300.0);
// Build the Kalman filter
final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
kalmanBuilder.builder(propagatorBuilder);
kalmanBuilder.estimatedMeasurementsParameters(new ParameterDriversList());
kalmanBuilder.initialCovarianceMatrix(MatrixUtils.createRealMatrix(6, 6));
kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(MatrixUtils.createRealMatrix(6, 6)));
final KalmanEstimator kalman = kalmanBuilder.build();
kalman.setObserver(estimation -> {
throw new DummyException();
});
try {
// Filter the measurements and expect an exception to occur
EstimationTestUtils.checkKalmanFit(context, kalman, measurements, context.initialOrbit, positionAngle, 0., 0., 0., 0., new double[3], 0., new double[3], 0.);
} catch (DummyException de) {
// expected
}
}
use of org.orekit.orbits.OrbitType in project Orekit by CS-SI.
the class KalmanOrbitDeterminationTest method testW3B.
@Test
public // Orbit determination for range, azimuth elevation measurements
void testW3B() throws URISyntaxException, IllegalArgumentException, IOException, OrekitException, ParseException {
// Print results on console
final boolean print = false;
// Input in tutorial resources directory/output
final String inputPath = KalmanOrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/W3B/od_test_W3.in").toURI().getPath();
final File input = new File(inputPath);
// Configure Orekit data access
Utils.setDataRoot("orbit-determination/W3B:potential/icgem-format");
GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
// Choice of an orbit type to use
// Default for test is Cartesian
final OrbitType orbitType = OrbitType.CARTESIAN;
// Initial orbital Cartesian covariance matrix
// These covariances are derived from the deltas between initial and reference orbits
// So in a way they are "perfect"...
// Cartesian covariance matrix initialization
final RealMatrix cartesianOrbitalP = MatrixUtils.createRealDiagonalMatrix(new double[] { FastMath.pow(2.4e4, 2), FastMath.pow(1.e5, 2), FastMath.pow(4.e4, 2), FastMath.pow(3.5, 2), FastMath.pow(2., 2), FastMath.pow(0.6, 2) });
// Orbital Cartesian process noise matrix (Q)
final RealMatrix cartesianOrbitalQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10 });
// Propagation covariance and process noise matrices
final RealMatrix propagationP = MatrixUtils.createRealDiagonalMatrix(new double[] { // Cd
FastMath.pow(2., 2), // leak-X
FastMath.pow(5.7e-6, 2), // leak-X
FastMath.pow(1.1e-11, 2), // leak-Y
FastMath.pow(7.68e-7, 2), // leak-Y
FastMath.pow(1.26e-10, 2), // leak-Z
FastMath.pow(5.56e-6, 2), // leak-Z
FastMath.pow(2.79e-10, 2) });
final RealMatrix propagationQ = MatrixUtils.createRealDiagonalMatrix(new double[] { // Cd
FastMath.pow(1e-3, 2), // Leaks
0., // Leaks
0., // Leaks
0., // Leaks
0., // Leaks
0., // Leaks
0. });
// Measurement covariance and process noise matrices
// az/el bias sigma = 0.06deg
// range bias sigma = 100m
final double angularVariance = FastMath.pow(FastMath.toRadians(0.06), 2);
final double rangeVariance = FastMath.pow(500., 2);
final RealMatrix measurementP = MatrixUtils.createRealDiagonalMatrix(new double[] { angularVariance, angularVariance, rangeVariance, angularVariance, angularVariance, rangeVariance, angularVariance, angularVariance, rangeVariance, angularVariance, angularVariance, rangeVariance, angularVariance, angularVariance, rangeVariance });
// Process noise sigma: 1e-6 for all
final double measQ = FastMath.pow(1e-6, 2);
final RealMatrix measurementQ = MatrixUtils.createRealIdentityMatrix(measurementP.getRowDimension()).scalarMultiply(measQ);
// Kalman orbit determination run.
ResultKalman kalmanW3B = run(input, orbitType, print, cartesianOrbitalP, cartesianOrbitalQ, propagationP, propagationQ, measurementP, measurementQ);
// Tests
// -----
// Definition of the accuracy for the test
final double distanceAccuracy = 0.1;
// degrees
final double angleAccuracy = 1e-5;
// Number of measurements processed
final int numberOfMeas = 521;
Assert.assertEquals(numberOfMeas, kalmanW3B.getNumberOfMeasurements());
// Test on propagator parameters
// -----------------------------
// Batch LS result
// final double dragCoef = -0.2154;
final double dragCoef = 0.1931;
Assert.assertEquals(dragCoef, kalmanW3B.propagatorParameters.getDrivers().get(0).getValue(), 1e-3);
final Vector3D leakAcceleration0 = new Vector3D(kalmanW3B.propagatorParameters.getDrivers().get(1).getValue(), kalmanW3B.propagatorParameters.getDrivers().get(3).getValue(), kalmanW3B.propagatorParameters.getDrivers().get(5).getValue());
// Batch LS results
// Assert.assertEquals(8.002e-6, leakAcceleration0.getNorm(), 1.0e-8);
Assert.assertEquals(5.994e-6, leakAcceleration0.getNorm(), 1.0e-8);
final Vector3D leakAcceleration1 = new Vector3D(kalmanW3B.propagatorParameters.getDrivers().get(2).getValue(), kalmanW3B.propagatorParameters.getDrivers().get(4).getValue(), kalmanW3B.propagatorParameters.getDrivers().get(6).getValue());
// Batch LS results
// Assert.assertEquals(3.058e-10, leakAcceleration1.getNorm(), 1.0e-12);
Assert.assertEquals(1.831e-10, leakAcceleration1.getNorm(), 1.0e-12);
// Test on measurements parameters
// -------------------------------
final List<DelegatingDriver> list = new ArrayList<DelegatingDriver>();
list.addAll(kalmanW3B.measurementsParameters.getDrivers());
sortParametersChanges(list);
// Station CastleRock
// Batch LS results
// final double[] CastleAzElBias = { 0.062701342, -0.003613508 };
// final double CastleRangeBias = 11274.4677;
final double[] CastleAzElBias = { 0.062635, -0.003672 };
final double CastleRangeBias = 11289.3678;
Assert.assertEquals(CastleAzElBias[0], FastMath.toDegrees(list.get(0).getValue()), angleAccuracy);
Assert.assertEquals(CastleAzElBias[1], FastMath.toDegrees(list.get(1).getValue()), angleAccuracy);
Assert.assertEquals(CastleRangeBias, list.get(2).getValue(), distanceAccuracy);
// Station Fucino
// Batch LS results
// final double[] FucAzElBias = { -0.053526137, 0.075483886 };
// final double FucRangeBias = 13467.8256;
final double[] FucAzElBias = { -0.053298, 0.075589 };
final double FucRangeBias = 13482.0715;
Assert.assertEquals(FucAzElBias[0], FastMath.toDegrees(list.get(3).getValue()), angleAccuracy);
Assert.assertEquals(FucAzElBias[1], FastMath.toDegrees(list.get(4).getValue()), angleAccuracy);
Assert.assertEquals(FucRangeBias, list.get(5).getValue(), distanceAccuracy);
// Station Kumsan
// Batch LS results
// final double[] KumAzElBias = { -0.023574208, -0.054520756 };
// final double KumRangeBias = 13512.57594;
final double[] KumAzElBias = { -0.022805, -0.055057 };
final double KumRangeBias = 13502.7459;
Assert.assertEquals(KumAzElBias[0], FastMath.toDegrees(list.get(6).getValue()), angleAccuracy);
Assert.assertEquals(KumAzElBias[1], FastMath.toDegrees(list.get(7).getValue()), angleAccuracy);
Assert.assertEquals(KumRangeBias, list.get(8).getValue(), distanceAccuracy);
// Station Pretoria
// Batch LS results
// final double[] PreAzElBias = { 0.030201539, 0.009747877 };
// final double PreRangeBias = 13594.11889;
final double[] PreAzElBias = { 0.030353, 0.009658 };
final double PreRangeBias = 13609.2516;
Assert.assertEquals(PreAzElBias[0], FastMath.toDegrees(list.get(9).getValue()), angleAccuracy);
Assert.assertEquals(PreAzElBias[1], FastMath.toDegrees(list.get(10).getValue()), angleAccuracy);
Assert.assertEquals(PreRangeBias, list.get(11).getValue(), distanceAccuracy);
// Station Uralla
// Batch LS results
// final double[] UraAzElBias = { 0.167814449, -0.12305252 };
// final double UraRangeBias = 13450.26738;
final double[] UraAzElBias = { 0.167519, -0.122842 };
final double UraRangeBias = 13441.7019;
Assert.assertEquals(UraAzElBias[0], FastMath.toDegrees(list.get(12).getValue()), angleAccuracy);
Assert.assertEquals(UraAzElBias[1], FastMath.toDegrees(list.get(13).getValue()), angleAccuracy);
Assert.assertEquals(UraRangeBias, list.get(14).getValue(), distanceAccuracy);
// Test on statistic for the range residuals
final long nbRange = 182;
// statistics for the range residual (min, max, mean, std)
final double[] RefStatRange = { -12.981, 18.046, -1.133, 5.312 };
Assert.assertEquals(nbRange, kalmanW3B.getRangeStat().getN());
Assert.assertEquals(RefStatRange[0], kalmanW3B.getRangeStat().getMin(), distanceAccuracy);
Assert.assertEquals(RefStatRange[1], kalmanW3B.getRangeStat().getMax(), distanceAccuracy);
Assert.assertEquals(RefStatRange[2], kalmanW3B.getRangeStat().getMean(), distanceAccuracy);
Assert.assertEquals(RefStatRange[3], kalmanW3B.getRangeStat().getStandardDeviation(), distanceAccuracy);
// test on statistic for the azimuth residuals
final long nbAzi = 339;
// statistics for the azimuth residual (min, max, mean, std)
final double[] RefStatAzi = { -0.041441, 0.023473, -0.004426, 0.009911 };
Assert.assertEquals(nbAzi, kalmanW3B.getAzimStat().getN());
Assert.assertEquals(RefStatAzi[0], kalmanW3B.getAzimStat().getMin(), angleAccuracy);
Assert.assertEquals(RefStatAzi[1], kalmanW3B.getAzimStat().getMax(), angleAccuracy);
Assert.assertEquals(RefStatAzi[2], kalmanW3B.getAzimStat().getMean(), angleAccuracy);
Assert.assertEquals(RefStatAzi[3], kalmanW3B.getAzimStat().getStandardDeviation(), angleAccuracy);
// test on statistic for the elevation residuals
final long nbEle = 339;
final double[] RefStatEle = { -0.025399, 0.043345, 0.001011, 0.010636 };
Assert.assertEquals(nbEle, kalmanW3B.getElevStat().getN());
Assert.assertEquals(RefStatEle[0], kalmanW3B.getElevStat().getMin(), angleAccuracy);
Assert.assertEquals(RefStatEle[1], kalmanW3B.getElevStat().getMax(), angleAccuracy);
Assert.assertEquals(RefStatEle[2], kalmanW3B.getElevStat().getMean(), angleAccuracy);
Assert.assertEquals(RefStatEle[3], kalmanW3B.getElevStat().getStandardDeviation(), angleAccuracy);
RealMatrix covariances = kalmanW3B.getCovariances();
Assert.assertEquals(28, covariances.getRowDimension());
Assert.assertEquals(28, covariances.getColumnDimension());
// drag coefficient variance
Assert.assertEquals(0.016349, covariances.getEntry(6, 6), 1.0e-5);
// leak-X constant term variance
Assert.assertEquals(2.047303E-13, covariances.getEntry(7, 7), 1.0e-16);
// leak-Y constant term variance
Assert.assertEquals(5.462497E-13, covariances.getEntry(9, 9), 1.0e-15);
// leak-Z constant term variance
Assert.assertEquals(1.717781E-11, covariances.getEntry(11, 11), 1.0e-15);
// Test on orbital parameters
// Done at the end to avoid changing the estimated propagation parameters
// ----------------------------------------------------------------------
// Estimated position and velocity
final Vector3D estimatedPos = kalmanW3B.getEstimatedPV().getPosition();
final Vector3D estimatedVel = kalmanW3B.getEstimatedPV().getVelocity();
// Reference position and velocity at initial date (same as in batch LS test)
final Vector3D refPos0 = new Vector3D(-40541446.255, -9905357.41, 206777.413);
final Vector3D refVel0 = new Vector3D(759.0685, -1476.5156, 54.793);
// Gather the selected propagation parameters and initialize them to the values found
// with the batch LS method
final ParameterDriversList refPropagationParameters = kalmanW3B.propagatorParameters;
final double dragCoefRef = -0.215433133145843;
final double[] leakXRef = { +5.69040439901955E-06, 1.09710906802403E-11 };
final double[] leakYRef = { -7.66440256777678E-07, 1.25467464335066E-10 };
final double[] leakZRef = { -5.574055079952E-06, 2.78703463746911E-10 };
for (DelegatingDriver driver : refPropagationParameters.getDrivers()) {
switch(driver.getName()) {
case "drag coefficient":
driver.setValue(dragCoefRef);
break;
case "leak-X[0]":
driver.setValue(leakXRef[0]);
break;
case "leak-X[1]":
driver.setValue(leakXRef[1]);
break;
case "leak-Y[0]":
driver.setValue(leakYRef[0]);
break;
case "leak-Y[1]":
driver.setValue(leakYRef[1]);
break;
case "leak-Z[0]":
driver.setValue(leakZRef[0]);
break;
case "leak-Z[1]":
driver.setValue(leakZRef[1]);
break;
}
}
// Run the reference until Kalman last date
final Orbit refOrbit = runReference(input, orbitType, refPos0, refVel0, refPropagationParameters, kalmanW3B.getEstimatedPV().getDate());
// Test on last orbit
final Vector3D refPos = refOrbit.getPVCoordinates().getPosition();
final Vector3D refVel = refOrbit.getPVCoordinates().getVelocity();
// Check distances
final double dP = Vector3D.distance(refPos, estimatedPos);
final double dV = Vector3D.distance(refVel, estimatedVel);
// FIXME: debug - Comparison with batch LS is bad
final double debugDistanceAccuracy = 234.73;
final double debugVelocityAccuracy = 0.086;
Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), debugDistanceAccuracy);
Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), debugVelocityAccuracy);
// Print orbit deltas
if (print) {
System.out.println("Test performances:");
System.out.format("\t%-30s\n", "ΔEstimated / Reference");
System.out.format(Locale.US, "\t%-10s %20.6f\n", "ΔP [m]", dP);
System.out.format(Locale.US, "\t%-10s %20.6f\n", "ΔV [m/s]", dV);
}
}
Aggregations