use of org.orekit.utils.TimeStampedPVCoordinates in project Orekit by CS-SI.
the class StreamingOemWriterTest method compareOemEphemerisBlocks.
private static void compareOemEphemerisBlocks(EphemeridesBlock block1, EphemeridesBlock block2, double p_tol, double v_tol) {
compareOemEphemerisBlocksMetadata(block1.getMetaData(), block2.getMetaData());
assertEquals(block1.getStart(), block2.getStart());
assertEquals(block1.getStop(), block2.getStop());
assertEquals(block1.getEphemeridesDataLines().size(), block2.getEphemeridesDataLines().size());
for (int i = 0; i < block1.getEphemeridesDataLines().size(); i++) {
TimeStampedPVCoordinates c1 = block1.getEphemeridesDataLines().get(i);
TimeStampedPVCoordinates c2 = block2.getEphemeridesDataLines().get(i);
assertEquals(c1.getDate(), c2.getDate());
assertEquals(c1.getPosition() + " -> " + c2.getPosition(), 0.0, Vector3D.distance(c1.getPosition(), c2.getPosition()), p_tol);
assertEquals(c1.getVelocity() + " -> " + c2.getVelocity(), 0.0, Vector3D.distance(c1.getVelocity(), c2.getVelocity()), v_tol);
}
}
use of org.orekit.utils.TimeStampedPVCoordinates in project Orekit by CS-SI.
the class BoxAndSolarArraySpacecraftTest method testNormalSunAlignedField.
@Test
public void testNormalSunAlignedField() throws OrekitException {
BoxAndSolarArraySpacecraft s = new BoxAndSolarArraySpacecraft(0, 0, 0, (date, frame) -> new TimeStampedPVCoordinates(date, new Vector3D(0, 1e6, 0), Vector3D.ZERO), 20.0, Vector3D.PLUS_J, 0.0, 1.0, 0.0);
Field<Decimal64> field = Decimal64Field.getInstance();
FieldVector3D<Decimal64> normal = s.getNormal(FieldAbsoluteDate.getJ2000Epoch(field), FramesFactory.getEME2000(), FieldVector3D.getZero(field), FieldRotation.getIdentity(field));
Assert.assertEquals(0, FieldVector3D.dotProduct(normal, Vector3D.PLUS_J).getReal(), 1.0e-16);
}
use of org.orekit.utils.TimeStampedPVCoordinates in project Orekit by CS-SI.
the class BatchLSEstimatorTest method testMultiSatWithParameters.
/**
* A modified version of the previous test with a selection of propagation drivers to estimate
* One common (µ)
* Some specifics for each satellite (Cr and Ca)
*
* @throws OrekitException
*/
@Test
public void testMultiSatWithParameters() throws OrekitException {
// Test: Set the propagator drivers to estimate for each satellite
final boolean muEstimated = true;
final boolean crEstimated1 = true;
final boolean caEstimated1 = true;
final boolean crEstimated2 = true;
final boolean caEstimated2 = false;
// Builder sat 1
final Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
final NumericalPropagatorBuilder propagatorBuilder1 = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 1.0, Force.POTENTIAL, Force.SOLAR_RADIATION_PRESSURE);
// Adding selection of parameters
String satName = "sat 1";
for (DelegatingDriver driver : propagatorBuilder1.getPropagationParametersDrivers().getDrivers()) {
if (driver.getName().equals("central attraction coefficient")) {
driver.setSelected(muEstimated);
}
if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
driver.setName(driver.getName() + " " + satName);
driver.setSelected(crEstimated1);
}
if (driver.getName().equals(RadiationSensitive.ABSORPTION_COEFFICIENT)) {
driver.setName(driver.getName() + " " + satName);
driver.setSelected(caEstimated1);
}
}
// Builder for sat 2
final Context context2 = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
final NumericalPropagatorBuilder propagatorBuilder2 = context2.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 1.0, Force.POTENTIAL, Force.SOLAR_RADIATION_PRESSURE);
// Adding selection of parameters
satName = "sat 2";
for (ParameterDriver driver : propagatorBuilder2.getPropagationParametersDrivers().getDrivers()) {
if (driver.getName().equals("central attraction coefficient")) {
driver.setSelected(muEstimated);
}
if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
driver.setName(driver.getName() + " " + satName);
driver.setSelected(crEstimated2);
}
if (driver.getName().equals(RadiationSensitive.ABSORPTION_COEFFICIENT)) {
driver.setName(driver.getName() + " " + satName);
driver.setSelected(caEstimated2);
}
}
// Create perfect inter-satellites range measurements
final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(), original.getPosition().add(new Vector3D(1000, 2000, 3000)), original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))), context.initialOrbit.getFrame(), context.initialOrbit.getMu());
final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit, propagatorBuilder2);
closePropagator.setEphemerisMode();
closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
final BoundedPropagator ephemeris = closePropagator.getGeneratedEphemeris();
Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder1);
final List<ObservedMeasurement<?>> r12 = EstimationTestUtils.createMeasurements(propagator1, new InterSatellitesRangeMeasurementCreator(ephemeris), 1.0, 3.0, 300.0);
// create perfect range measurements for first satellite
propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder1);
final List<ObservedMeasurement<?>> r1 = EstimationTestUtils.createMeasurements(propagator1, new RangeMeasurementCreator(context), 1.0, 3.0, 300.0);
// create orbit estimator
final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(), propagatorBuilder1, propagatorBuilder2);
for (final ObservedMeasurement<?> interSat : r12) {
estimator.addMeasurement(interSat);
}
for (final ObservedMeasurement<?> range : r1) {
estimator.addMeasurement(range);
}
estimator.setParametersConvergenceThreshold(1.0e-2);
estimator.setMaxIterations(10);
estimator.setMaxEvaluations(20);
estimator.setObserver(new BatchLSObserver() {
int lastIter = 0;
int lastEval = 0;
/**
* {@inheritDoc}
*/
@Override
public void evaluationPerformed(int iterationsCount, int evaluationscount, Orbit[] orbits, ParameterDriversList estimatedOrbitalParameters, ParameterDriversList estimatedPropagatorParameters, ParameterDriversList estimatedMeasurementsParameters, EstimationsProvider evaluationsProvider, Evaluation lspEvaluation) throws OrekitException {
if (iterationsCount == lastIter) {
Assert.assertEquals(lastEval + 1, evaluationscount);
} else {
Assert.assertEquals(lastIter + 1, iterationsCount);
}
lastIter = iterationsCount;
lastEval = evaluationscount;
AbsoluteDate previous = AbsoluteDate.PAST_INFINITY;
for (int i = 0; i < evaluationsProvider.getNumber(); ++i) {
AbsoluteDate current = evaluationsProvider.getEstimatedMeasurement(i).getDate();
Assert.assertTrue(current.compareTo(previous) >= 0);
previous = current;
}
}
});
List<DelegatingDriver> parameters = estimator.getOrbitalParametersDrivers(true).getDrivers();
ParameterDriver a0Driver = parameters.get(0);
Assert.assertEquals("a[0]", a0Driver.getName());
a0Driver.setValue(a0Driver.getValue() + 1.2);
a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
ParameterDriver a1Driver = parameters.get(6);
Assert.assertEquals("a[1]", a1Driver.getName());
a1Driver.setValue(a1Driver.getValue() - 5.4);
a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
final Orbit before = new KeplerianOrbit(parameters.get(6).getValue(), parameters.get(7).getValue(), parameters.get(8).getValue(), parameters.get(9).getValue(), parameters.get(10).getValue(), parameters.get(11).getValue(), PositionAngle.TRUE, closeOrbit.getFrame(), closeOrbit.getDate(), closeOrbit.getMu());
Assert.assertEquals(4.7246, Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(), before.getPVCoordinates().getPosition()), 1.0e-3);
Assert.assertEquals(0.0010514, Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(), before.getPVCoordinates().getVelocity()), 1.0e-6);
EstimationTestUtils.checkFit(context, estimator, 4, 5, 0.0, 6.0e-06, 0.0, 1.7e-05, 0.0, 4.4e-07, 0.0, 1.7e-10);
final Orbit determined = new KeplerianOrbit(parameters.get(6).getValue(), parameters.get(7).getValue(), parameters.get(8).getValue(), parameters.get(9).getValue(), parameters.get(10).getValue(), parameters.get(11).getValue(), PositionAngle.TRUE, closeOrbit.getFrame(), closeOrbit.getDate(), closeOrbit.getMu());
Assert.assertEquals(0.0, Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(), determined.getPVCoordinates().getPosition()), 5.8e-6);
Assert.assertEquals(0.0, Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(), determined.getPVCoordinates().getVelocity()), 3.5e-9);
// got a default one
for (final ParameterDriver driver : estimator.getOrbitalParametersDrivers(true).getDrivers()) {
if (driver.getName().startsWith("a[")) {
// user-specified reference date
Assert.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
} else {
// default reference date
Assert.assertEquals(0, driver.getReferenceDate().durationFrom(propagatorBuilder1.getInitialOrbitDate()), 1.0e-15);
}
}
}
use of org.orekit.utils.TimeStampedPVCoordinates in project Orekit by CS-SI.
the class RangeAnalytic method theoreticalEvaluationAnalytic.
/**
* Analytical version of the theoreticalEvaluation function in Range class
* The derivative structures are not used, an analytical computation is used instead.
* @param iteration current LS estimator iteration
* @param evaluation current LS estimator evaluation
* @param state spacecraft state. At measurement date on first iteration then close to emission date on further iterations
* @param interpolator Orekit step interpolator
* @return
* @throws OrekitException
*/
protected EstimatedMeasurement<Range> theoreticalEvaluationAnalytic(final int iteration, final int evaluation, final SpacecraftState state) throws OrekitException {
// Station attribute from parent Range class
final GroundStation groundStation = this.getStation();
// Station position at signal arrival
final AbsoluteDate downlinkDate = getDate();
final Transform topoToInertDownlink = groundStation.getOffsetToInertial(state.getFrame(), downlinkDate);
final TimeStampedPVCoordinates stationDownlink = topoToInertDownlink.transformPVCoordinates(new TimeStampedPVCoordinates(downlinkDate, PVCoordinates.ZERO));
// Take propagation time into account
// (if state has already been set up to pre-compensate propagation delay,
// we will have offset == downlinkDelay and transitState will be
// the same as state)
// Downlink time of flight
final double tauD = signalTimeOfFlight(state.getPVCoordinates(), stationDownlink.getPosition(), downlinkDate);
final double delta = downlinkDate.durationFrom(state.getDate());
final double dt = delta - tauD;
// Transit state position
final SpacecraftState transitState = state.shiftedBy(dt);
final AbsoluteDate transitDate = transitState.getDate();
final Vector3D transitP = transitState.getPVCoordinates().getPosition();
// Station position at transit state date
final Transform topoToInertAtTransitDate = groundStation.getOffsetToInertial(state.getFrame(), transitDate);
final TimeStampedPVCoordinates stationAtTransitDate = topoToInertAtTransitDate.transformPVCoordinates(new TimeStampedPVCoordinates(transitDate, PVCoordinates.ZERO));
// Uplink time of flight
final double tauU = signalTimeOfFlight(stationAtTransitDate, transitP, transitDate);
final double tau = tauD + tauU;
// Real date and position of station at signal departure
final AbsoluteDate uplinkDate = downlinkDate.shiftedBy(-tau);
final TimeStampedPVCoordinates stationUplink = topoToInertDownlink.shiftedBy(-tau).transformPVCoordinates(new TimeStampedPVCoordinates(uplinkDate, PVCoordinates.ZERO));
// Prepare the evaluation
final EstimatedMeasurement<Range> estimated = new EstimatedMeasurement<Range>(this, iteration, evaluation, new SpacecraftState[] { transitState }, new TimeStampedPVCoordinates[] { stationUplink, transitState.getPVCoordinates(), stationDownlink });
// Set range value
final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
estimated.setEstimatedValue(tau * cOver2);
// Partial derivatives with respect to state
// The formulas below take into account the fact the measurement is at fixed reception date.
// When spacecraft position is changed, the downlink delay is changed, and in order
// to still have the measurement arrive at exactly the same date on ground, we must
// take the spacecraft-station relative velocity into account.
final Vector3D v = state.getPVCoordinates().getVelocity();
final Vector3D qv = stationDownlink.getVelocity();
final Vector3D downInert = stationDownlink.getPosition().subtract(transitP);
final double dDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tauD - Vector3D.dotProduct(downInert, v);
final Vector3D upInert = transitP.subtract(stationUplink.getPosition());
// test
// final double dUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tauU -
// Vector3D.dotProduct(upInert, qv);
// test
final double dUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tauU - Vector3D.dotProduct(upInert, stationUplink.getVelocity());
// derivatives of the downlink time of flight
final double dTauDdPx = -downInert.getX() / dDown;
final double dTauDdPy = -downInert.getY() / dDown;
final double dTauDdPz = -downInert.getZ() / dDown;
// Derivatives of the uplink time of flight
final double dTauUdPx = 1. / dUp * upInert.dotProduct(Vector3D.PLUS_I.add((qv.subtract(v)).scalarMultiply(dTauDdPx)));
final double dTauUdPy = 1. / dUp * upInert.dotProduct(Vector3D.PLUS_J.add((qv.subtract(v)).scalarMultiply(dTauDdPy)));
final double dTauUdPz = 1. / dUp * upInert.dotProduct(Vector3D.PLUS_K.add((qv.subtract(v)).scalarMultiply(dTauDdPz)));
// derivatives of the range measurement
final double dRdPx = (dTauDdPx + dTauUdPx) * cOver2;
final double dRdPy = (dTauDdPy + dTauUdPy) * cOver2;
final double dRdPz = (dTauDdPz + dTauUdPz) * cOver2;
estimated.setStateDerivatives(0, new double[] { dRdPx, dRdPy, dRdPz, dRdPx * dt, dRdPy * dt, dRdPz * dt });
if (groundStation.getEastOffsetDriver().isSelected() || groundStation.getNorthOffsetDriver().isSelected() || groundStation.getZenithOffsetDriver().isSelected()) {
// Downlink tme of flight derivatives / station position in topocentric frame
final AngularCoordinates ac = topoToInertDownlink.getAngular().revert();
// final Rotation rotTopoToInert = ac.getRotation();
final Vector3D omega = ac.getRotationRate();
// Inertial frame
final double dTauDdQIx = downInert.getX() / dDown;
final double dTauDdQIy = downInert.getY() / dDown;
final double dTauDdQIz = downInert.getZ() / dDown;
// Uplink tme of flight derivatives / station position in topocentric frame
// Inertial frame
final double dTauUdQIx = 1 / dUp * upInert.dotProduct(Vector3D.MINUS_I.add((qv.subtract(v)).scalarMultiply(dTauDdQIx)).subtract(Vector3D.PLUS_I.crossProduct(omega).scalarMultiply(tau)));
final double dTauUdQIy = 1 / dUp * upInert.dotProduct(Vector3D.MINUS_J.add((qv.subtract(v)).scalarMultiply(dTauDdQIy)).subtract(Vector3D.PLUS_J.crossProduct(omega).scalarMultiply(tau)));
final double dTauUdQIz = 1 / dUp * upInert.dotProduct(Vector3D.MINUS_K.add((qv.subtract(v)).scalarMultiply(dTauDdQIz)).subtract(Vector3D.PLUS_K.crossProduct(omega).scalarMultiply(tau)));
// Range partial derivatives
// with respect to station position in inertial frame
final Vector3D dRdQI = new Vector3D((dTauDdQIx + dTauUdQIx) * cOver2, (dTauDdQIy + dTauUdQIy) * cOver2, (dTauDdQIz + dTauUdQIz) * cOver2);
// convert to topocentric frame, as the station position
// offset parameter is expressed in this frame
final Vector3D dRdQT = ac.getRotation().applyTo(dRdQI);
if (groundStation.getEastOffsetDriver().isSelected()) {
estimated.setParameterDerivatives(groundStation.getEastOffsetDriver(), dRdQT.getX());
}
if (groundStation.getNorthOffsetDriver().isSelected()) {
estimated.setParameterDerivatives(groundStation.getNorthOffsetDriver(), dRdQT.getY());
}
if (groundStation.getZenithOffsetDriver().isSelected()) {
estimated.setParameterDerivatives(groundStation.getZenithOffsetDriver(), dRdQT.getZ());
}
}
return estimated;
}
use of org.orekit.utils.TimeStampedPVCoordinates in project Orekit by CS-SI.
the class RangeTest method genericTestValues.
/**
* Generic test function for values of the range
* @param printResults Print the results ?
* @throws OrekitException
*/
void genericTestValues(final boolean printResults) throws OrekitException {
Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 0.001);
// 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);
// Lists for results' storage - Used only for derivatives with respect to state
// "final" value to be seen by "handleStep" function of the propagator
final List<Double> absoluteErrors = new ArrayList<Double>();
final List<Double> relativeErrors = new ArrayList<Double>();
// Set master mode
// Use a lambda function to implement "handleStep" function
propagator.setMasterMode((OrekitStepInterpolator interpolator, boolean isLast) -> {
for (final ObservedMeasurement<?> measurement : measurements) {
// Play test if the measurement date is between interpolator previous and current date
if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) && (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
// We intentionally propagate to a date which is close to the
// real spacecraft state but is *not* the accurate date, by
// compensating only part of the downlink delay. This is done
// in order to validate the partial derivatives with respect
// to velocity. If we had chosen the proper state date, the
// range would have depended only on the current position but
// not on the current velocity.
final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
final SpacecraftState state = interpolator.getInterpolatedState(date);
// Values of the Range & errors
final double RangeObserved = measurement.getObservedValue()[0];
final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
Assert.assertEquals(3, participants.length);
Assert.assertEquals(0.5 * Constants.SPEED_OF_LIGHT * participants[2].getDate().durationFrom(participants[0].getDate()), estimated.getEstimatedValue()[0], 2.0e-8);
// the real state used for estimation is adjusted according to downlink delay
double adjustment = state.getDate().durationFrom(estimated.getStates()[0].getDate());
Assert.assertTrue(adjustment > 0.006);
Assert.assertTrue(adjustment < 0.010);
final double RangeEstimated = estimated.getEstimatedValue()[0];
final double absoluteError = RangeEstimated - RangeObserved;
absoluteErrors.add(absoluteError);
relativeErrors.add(FastMath.abs(absoluteError) / FastMath.abs(RangeObserved));
// Print results on console ?
if (printResults) {
final AbsoluteDate measurementDate = measurement.getDate();
String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
System.out.format(Locale.US, "%-15s %-23s %-23s %19.6f %19.6f %13.6e %13.6e%n", stationName, measurementDate, date, RangeObserved, RangeEstimated, FastMath.abs(RangeEstimated - RangeObserved), FastMath.abs((RangeEstimated - RangeObserved) / RangeObserved));
}
}
// End if measurement date between previous and current interpolator step
}
// End for loop on the measurements
});
// Print results on console ? Header
if (printResults) {
System.out.format(Locale.US, "%-15s %-23s %-23s %19s %19s %13s %13s%n", "Station", "Measurement Date", "State Date", "Range observed [m]", "Range estimated [m]", "ΔRange [m]", "rel ΔRange");
}
// Rewind the propagator to initial date
propagator.propagate(context.initialOrbit.getDate());
// Sort measurements chronologically
measurements.sort(new ChronologicalComparator());
// Propagate to final measurement's date
propagator.propagate(measurements.get(measurements.size() - 1).getDate());
// Convert lists to double array
final double[] absErrors = absoluteErrors.stream().mapToDouble(Double::doubleValue).toArray();
final double[] relErrors = relativeErrors.stream().mapToDouble(Double::doubleValue).toArray();
// Statistics' assertion
final double absErrorsMedian = new Median().evaluate(absErrors);
final double absErrorsMin = new Min().evaluate(absErrors);
final double absErrorsMax = new Max().evaluate(absErrors);
final double relErrorsMedian = new Median().evaluate(relErrors);
final double relErrorsMax = new Max().evaluate(relErrors);
// Print the results on console ? Final results
if (printResults) {
System.out.println();
System.out.println("Absolute errors median: " + absErrorsMedian);
System.out.println("Absolute errors min : " + absErrorsMin);
System.out.println("Absolute errors max : " + absErrorsMax);
System.out.println("Relative errors median: " + relErrorsMedian);
System.out.println("Relative errors max : " + relErrorsMax);
}
Assert.assertEquals(0.0, absErrorsMedian, 4.9e-8);
Assert.assertEquals(0.0, absErrorsMin, 2.2e-7);
Assert.assertEquals(0.0, absErrorsMax, 2.1e-7);
Assert.assertEquals(0.0, relErrorsMedian, 1.0e-14);
Assert.assertEquals(0.0, relErrorsMax, 2.6e-14);
}
Aggregations