use of org.orekit.orbits.Orbit in project Orekit by CS-SI.
the class KalmanEstimatorTest method testKeplerianRangeAzElAndRangeRate.
/**
* Perfect Range, Azel and range rate measurements with a biased start
* Start: position/velocity biased with: [+1000,0,0] m and [0,0,0.01] m/s
* Keplerian formalism
*/
@Test
public void testKeplerianRangeAzElAndRangeRate() 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 measPropagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart, minStep, maxStep, dP);
// Create perfect range measurements
final Propagator rangePropagator = EstimationTestUtils.createPropagator(context.initialOrbit, measPropagatorBuilder);
final List<ObservedMeasurement<?>> rangeMeasurements = EstimationTestUtils.createMeasurements(rangePropagator, new RangeMeasurementCreator(context), 0.0, 4.0, 300.0);
// Create perfect az/el measurements
final Propagator angularPropagator = EstimationTestUtils.createPropagator(context.initialOrbit, measPropagatorBuilder);
final List<ObservedMeasurement<?>> angularMeasurements = EstimationTestUtils.createMeasurements(angularPropagator, new AngularAzElMeasurementCreator(context), 0.0, 4.0, 500.0);
// Create perfect range rate measurements
final Propagator rangeRatePropagator = EstimationTestUtils.createPropagator(context.initialOrbit, measPropagatorBuilder);
final List<ObservedMeasurement<?>> rangeRateMeasurements = EstimationTestUtils.createMeasurements(rangeRatePropagator, new RangeRateMeasurementCreator(context, false), 0.0, 4.0, 700.0);
// Concatenate measurements
final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
measurements.addAll(rangeMeasurements);
measurements.addAll(angularMeasurements);
measurements.addAll(rangeRateMeasurements);
measurements.sort(new ChronologicalComparator());
// Reference propagator for estimation performances
final NumericalPropagator referencePropagator = measPropagatorBuilder.buildPropagator(measPropagatorBuilder.getSelectedNormalizedParameters());
// Reference position/velocity at last measurement date
final Orbit refOrbit = referencePropagator.propagate(measurements.get(measurements.size() - 1).getDate()).getOrbit();
// Biased propagator for the Kalman
final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, false, minStep, maxStep, dP);
// Cartesian covariance matrix initialization
// Initial sigmas: 1000m on position, 0.01m/s on velocity
final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e6, 1e6, 1e6, 1e-4, 1e-4, 1e-4 });
// 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, dYdC);
final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
// Orbital initial covariance matrix
final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
// Process noise matrix
final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10 });
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 = 2.91e-2;
final double expectedDeltaVel = 0.;
final double velEps = 5.52e-6;
final double[] expectedSigmasPos = { 1.747570, 0.666879, 1.696182 };
final double sigmaPosEps = 1e-6;
final double[] expectedSigmasVel = { 5.413666e-4, 4.088359e-4, 4.315316e-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.Orbit in project Orekit by CS-SI.
the class KalmanEstimatorTest method testKeplerianRangeWithOnBoardAntennaOffset.
/**
* Perfect range measurements with a biased start and an on-board antenna range offset
* Keplerian formalism
* @throws OrekitException
*/
@Test
public void testKeplerianRangeWithOnBoardAntennaOffset() 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);
propagatorBuilder.setAttitudeProvider(new LofOffset(propagatorBuilder.getFrame(), LOFType.LVLH));
// Antenna phase center definition
final Vector3D antennaPhaseCenter = new Vector3D(-1.2, 2.3, -0.7);
// Create perfect range measurements with antenna offset
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context, antennaPhaseCenter), 1.0, 3.0, 300.0);
// Add antenna offset to the measurements
final OnBoardAntennaRangeModifier obaModifier = new OnBoardAntennaRangeModifier(antennaPhaseCenter);
for (final ObservedMeasurement<?> range : measurements) {
((Range) range).addModifier(obaModifier);
}
// 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[] { 10., 10., 10., 1e-3, 1e-3, 1e-3 });
// Jacobian of the orbital parameters w/r to Cartesian
final Orbit initialOrbit = OrbitType.KEPLERIAN.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 = 4.57e-3;
final double expectedDeltaVel = 0.;
final double velEps = 7.29e-6;
final double[] expectedSigmasPos = { 1.105194, 0.930785, 1.254579 };
final double sigmaPosEps = 1e-6;
final double[] expectedSigmasVel = { 6.193718e-4, 4.088774e-4, 3.299135e-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.Orbit in project Orekit by CS-SI.
the class KalmanEstimatorTest method testKeplerianRangeAndRangeRate.
/**
* Perfect range and range rate measurements with a perfect start
* @throws OrekitException
*/
@Test
public void testKeplerianRangeAndRangeRate() 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 & range rate measurements
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
final List<ObservedMeasurement<?>> measurementsRange = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context), 1.0, 3.0, 300.0);
final List<ObservedMeasurement<?>> measurementsRangeRate = EstimationTestUtils.createMeasurements(propagator, new RangeRateMeasurementCreator(context, false), 1.0, 3.0, 300.0);
// Concatenate measurements
final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
measurements.addAll(measurementsRange);
measurements.addAll(measurementsRangeRate);
// 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-2, 1e-2, 1e-2, 1e-8, 1e-8, 1e-8 });
// 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
final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10 });
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 = 5.96e-3;
final double expectedDeltaVel = 0.;
final double velEps = 2.06e-6;
final double[] expectedSigmasPos = { 0.341538, 8.175281, 4.634384 };
final double sigmaPosEps = 1e-6;
final double[] expectedSigmasVel = { 1.167838e-3, 1.036437e-3, 2.834385e-3 };
final double sigmaVelEps = 1e-9;
EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedSigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}
use of org.orekit.orbits.Orbit in project Orekit by CS-SI.
the class KalmanEstimatorTest method testKeplerianPV.
/**
* Perfect PV measurements with a perfect start
* Keplerian formalism
* @throws OrekitException
*/
@Test
public void testKeplerianPV() 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 PV measurements
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new PVMeasurementCreator(), 0.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();
// Covariance matrix initialization
final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5 });
// Process noise matrix
RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8 });
// 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 = 5.80e-8;
final double expectedDeltaVel = 0.;
final double velEps = 2.28e-11;
final double[] expectedsigmasPos = { 0.998872, 0.933655, 0.997516 };
final double sigmaPosEps = 1e-6;
final double[] expectedSigmasVel = { 9.478853e-4, 9.910788e-4, 5.0438709e-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.Orbit in project Orekit by CS-SI.
the class KalmanOrbitDeterminationTest method testLageos2.
@Test
public // Orbit determination for Lageos2 based on SLR (range) measurements
void testLageos2() 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/Lageos2/od_test_Lageos2.in").toURI().getPath();
final File input = new File(inputPath);
// configure Orekit data acces
Utils.setDataRoot("orbit-determination/Lageos2: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[] { 1e4, 4e3, 1, 5e-3, 6e-5, 1e-4 });
// 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 });
// Initial measurement covariance matrix and process noise matrix
final RealMatrix measurementP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1., 1., 1., 1. });
final RealMatrix measurementQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e-6, 1e-6, 1e-6, 1e-6 });
// Kalman orbit determination run.
ResultKalman kalmanLageos2 = run(input, orbitType, print, cartesianOrbitalP, cartesianOrbitalQ, null, null, measurementP, measurementQ);
// Definition of the accuracy for the test
final double distanceAccuracy = 0.86;
final double velocityAccuracy = 4.12e-3;
// Tests
// Note: The reference initial orbit is the same as in the batch LS tests
// -----
// Number of measurements processed
final int numberOfMeas = 258;
Assert.assertEquals(numberOfMeas, kalmanLageos2.getNumberOfMeasurements());
// Estimated position and velocity
final Vector3D estimatedPos = kalmanLageos2.getEstimatedPV().getPosition();
final Vector3D estimatedVel = kalmanLageos2.getEstimatedPV().getVelocity();
// Reference position and velocity at initial date (same as in batch LS test)
final Vector3D refPos0 = new Vector3D(-5532131.956902, 10025696.592156, -3578940.040009);
final Vector3D refVel0 = new Vector3D(-3871.275109, -607.880985, 4280.972530);
// Run the reference until Kalman last date
final Orbit refOrbit = runReference(input, orbitType, refPos0, refVel0, null, kalmanLageos2.getEstimatedPV().getDate());
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);
Assert.assertEquals(0.0, dP, distanceAccuracy);
Assert.assertEquals(0.0, dV, velocityAccuracy);
// 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);
}
// Test on measurements parameters
final List<DelegatingDriver> list = new ArrayList<DelegatingDriver>();
list.addAll(kalmanLageos2.measurementsParameters.getDrivers());
sortParametersChanges(list);
// Batch LS values
// final double[] stationOffSet = { 1.659203, 0.861250, -0.885352 };
// final double rangeBias = -0.286275;
final double[] stationOffSet = { 0.298867, -0.137456, 0.013315 };
final double rangeBias = 0.002390;
Assert.assertEquals(stationOffSet[0], list.get(0).getValue(), distanceAccuracy);
Assert.assertEquals(stationOffSet[1], list.get(1).getValue(), distanceAccuracy);
Assert.assertEquals(stationOffSet[2], list.get(2).getValue(), distanceAccuracy);
Assert.assertEquals(rangeBias, list.get(3).getValue(), distanceAccuracy);
// test on statistic for the range residuals
final long nbRange = 258;
// Batch LS values
// final double[] RefStatRange = { -2.431135, 2.218644, 0.038483, 0.982017 };
final double[] RefStatRange = { -23.561314, 20.436464, 0.964164, 5.687187 };
Assert.assertEquals(nbRange, kalmanLageos2.getRangeStat().getN());
Assert.assertEquals(RefStatRange[0], kalmanLageos2.getRangeStat().getMin(), distanceAccuracy);
Assert.assertEquals(RefStatRange[1], kalmanLageos2.getRangeStat().getMax(), distanceAccuracy);
Assert.assertEquals(RefStatRange[2], kalmanLageos2.getRangeStat().getMean(), distanceAccuracy);
Assert.assertEquals(RefStatRange[3], kalmanLageos2.getRangeStat().getStandardDeviation(), distanceAccuracy);
}
Aggregations