Examples with ChronologicalComparator org.orekit.time.ChronologicalComparator used on opensource projects

Example 1 with ChronologicalComparator

use of org.orekit.time.ChronologicalComparator in project Orekit by CS-SI.

the class InterSatellitesRangeTest method genericTestStateDerivatives.

void genericTestStateDerivatives(final boolean printResults, final int index, final double refErrorsPMedian, final double refErrorsPMean, final double refErrorsPMax, final double refErrorsVMedian, final double refErrorsVMean, final double refErrorsVMax) 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 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, propagatorBuilder);
    closePropagator.setEphemerisMode();
    closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
    final BoundedPropagator ephemeris = closePropagator.getGeneratedEphemeris();
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new InterSatellitesRangeMeasurementCreator(ephemeris), 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> errorsP = new ArrayList<Double>();
    final List<Double> errorsV = 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.
                final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
                final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
                final SpacecraftState[] states = { interpolator.getInterpolatedState(date), ephemeris.propagate(date) };
                final double[][] jacobian = measurement.estimate(0, 0, states).getStateDerivatives(index);
                // Jacobian reference value
                final double[][] jacobianRef;
                // Compute a reference value using finite differences
                jacobianRef = Differentiation.differentiate(new StateFunction() {

                    public double[] value(final SpacecraftState state) throws OrekitException {
                        final SpacecraftState[] s = states.clone();
                        s[index] = state;
                        return measurement.estimate(0, 0, s).getEstimatedValue();
                    }
                }, measurement.getDimension(), propagator.getAttitudeProvider(), OrbitType.CARTESIAN, PositionAngle.TRUE, 2.0, 3).value(states[index]);
                Assert.assertEquals(jacobianRef.length, jacobian.length);
                Assert.assertEquals(jacobianRef[0].length, jacobian[0].length);
                // Errors & relative errors on the Jacobian
                double[][] dJacobian = new double[jacobian.length][jacobian[0].length];
                double[][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
                for (int i = 0; i < jacobian.length; ++i) {
                    for (int j = 0; j < jacobian[i].length; ++j) {
                        dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
                        dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j] / jacobianRef[i][j]);
                        if (j < 3) {
                            errorsP.add(dJacobianRelative[i][j]);
                        } else {
                            errorsV.add(dJacobianRelative[i][j]);
                        }
                    }
                }
                // Print values in console ?
                if (printResults) {
                    System.out.format(Locale.US, "%-23s  %-23s  " + "%10.3e  %10.3e  %10.3e  " + "%10.3e  %10.3e  %10.3e  " + "%10.3e  %10.3e  %10.3e  " + "%10.3e  %10.3e  %10.3e%n", measurement.getDate(), date, dJacobian[0][0], dJacobian[0][1], dJacobian[0][2], dJacobian[0][3], dJacobian[0][4], dJacobian[0][5], dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2], dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
                }
            }
        // End if measurement date between previous and current interpolator step
        }
    // End for loop on the measurements
    });
    // Print results on console ?
    if (printResults) {
        System.out.format(Locale.US, "%-23s  %-23s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s%n", "Measurement Date", "State Date", "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz", "rel ΔdPx", "rel ΔdPy", "rel ΔdPz", "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
    }
    // 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[] and evaluate some statistics
    final double[] relErrorsP = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
    final double[] relErrorsV = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
    final double errorsPMedian = new Median().evaluate(relErrorsP);
    final double errorsPMean = new Mean().evaluate(relErrorsP);
    final double errorsPMax = new Max().evaluate(relErrorsP);
    final double errorsVMedian = new Median().evaluate(relErrorsV);
    final double errorsVMean = new Mean().evaluate(relErrorsV);
    final double errorsVMax = new Max().evaluate(relErrorsV);
    // Print the results on console ?
    if (printResults) {
        System.out.println();
        System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", errorsPMedian, errorsPMean, errorsPMax);
        System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", errorsVMedian, errorsVMean, errorsVMax);
    }
    Assert.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
    Assert.assertEquals(0.0, errorsPMean, refErrorsPMean);
    Assert.assertEquals(0.0, errorsPMax, refErrorsPMax);
    Assert.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
    Assert.assertEquals(0.0, errorsVMean, refErrorsVMean);
    Assert.assertEquals(0.0, errorsVMax, refErrorsVMax);
}

Example 2 with ChronologicalComparator

use of org.orekit.time.ChronologicalComparator in project Orekit by CS-SI.

the class InterSatellitesRangeTest method genericTestValues.

/**
 * Generic test function for values of the inter-satellites 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 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, propagatorBuilder);
    closePropagator.setEphemerisMode();
    closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
    final BoundedPropagator ephemeris = closePropagator.getGeneratedEphemeris();
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new InterSatellitesRangeMeasurementCreator(ephemeris), 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.
                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, ephemeris.propagate(state.getDate()) });
                // 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.000006);
                Assert.assertTrue(adjustment < 0.0003);
                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();
                    System.out.format(Locale.US, "%-23s  %-23s  %19.6f  %19.6f  %13.6e  %13.6e%n", 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, "%-23s  %-23s  %19s  %19s  %13s  %13s%n", "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, 1.3e-7);
    Assert.assertEquals(0.0, absErrorsMin, 7.3e-7);
    Assert.assertEquals(0.0, absErrorsMax, 1.8e-7);
    Assert.assertEquals(0.0, relErrorsMedian, 1.0e-12);
    Assert.assertEquals(0.0, relErrorsMax, 3.2e-12);
}

Example 3 with ChronologicalComparator

use of org.orekit.time.ChronologicalComparator 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);
}

Example 4 with ChronologicalComparator

use of org.orekit.time.ChronologicalComparator in project Orekit by CS-SI.

the class RangeAnalyticTest method genericTestStateDerivatives.

/**
 * Generic test function for derivatives with respect to state
 * @param isModifier Use of atmospheric modifiers
 * @param isFiniteDifferences Finite differences reference calculation if true, Range class otherwise
 * @param printResults Print the results ?
 * @throws OrekitException
 */
void genericTestStateDerivatives(final boolean isModifier, final boolean isFiniteDifferences, final boolean printResults, final double refErrorsPMedian, final double refErrorsPMean, final double refErrorsPMax, final double refErrorsVMedian, final double refErrorsVMean, final double refErrorsVMax) 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> errorsP = new ArrayList<Double>();
    final List<Double> errorsV = 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.)) {
                // Add modifiers if test implies it
                final RangeTroposphericDelayModifier modifier = new RangeTroposphericDelayModifier(SaastamoinenModel.getStandardModel());
                if (isModifier) {
                    ((Range) measurement).addModifier(modifier);
                }
                // 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);
                final EstimatedMeasurement<Range> range = new RangeAnalytic((Range) measurement).theoreticalEvaluationAnalytic(0, 0, state);
                if (isModifier) {
                    modifier.modify(range);
                }
                final double[][] jacobian = range.getStateDerivatives(0);
                // Jacobian reference value
                final double[][] jacobianRef;
                if (isFiniteDifferences) {
                    // Compute a reference value using finite differences
                    jacobianRef = Differentiation.differentiate(new StateFunction() {

                        public double[] value(final SpacecraftState state) throws OrekitException {
                            return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
                        }
                    }, measurement.getDimension(), propagator.getAttitudeProvider(), OrbitType.CARTESIAN, PositionAngle.TRUE, 2.0, 3).value(state);
                } else {
                    // Compute a reference value using Range class function
                    jacobianRef = ((Range) measurement).theoreticalEvaluation(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
                }
                // //Test: Test point by point with the debugger
                // if (!isFiniteDifferences && !isModifier) {
                // final EstimatedMeasurement<Range> test =
                // new RangeAnalytic((Range)measurement).theoreticalEvaluationValidation(0, 0, state);
                // }
                // //Test
                Assert.assertEquals(jacobianRef.length, jacobian.length);
                Assert.assertEquals(jacobianRef[0].length, jacobian[0].length);
                // Errors & relative errors on the jacobian
                double[][] dJacobian = new double[jacobian.length][jacobian[0].length];
                double[][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
                for (int i = 0; i < jacobian.length; ++i) {
                    for (int j = 0; j < jacobian[i].length; ++j) {
                        dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
                        dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j] / jacobianRef[i][j]);
                        if (j < 3) {
                            errorsP.add(dJacobianRelative[i][j]);
                        } else {
                            errorsV.add(dJacobianRelative[i][j]);
                        }
                    }
                }
                // Print values in console ?
                if (printResults) {
                    String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
                    System.out.format(Locale.US, "%-15s  %-23s  %-23s  " + "%10.3e  %10.3e  %10.3e  " + "%10.3e  %10.3e  %10.3e  " + "%10.3e  %10.3e  %10.3e  " + "%10.3e  %10.3e  %10.3e%n", stationName, measurement.getDate(), date, dJacobian[0][0], dJacobian[0][1], dJacobian[0][2], dJacobian[0][3], dJacobian[0][4], dJacobian[0][5], dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2], dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
                }
            }
        // End if measurement date between previous and current interpolator step
        }
    // End for loop on the measurements
    });
    // Print results on console ?
    if (printResults) {
        System.out.format(Locale.US, "%-15s  %-23s  %-23s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s%n", "Station", "Measurement Date", "State Date", "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz", "rel ΔdPx", "rel ΔdPy", "rel ΔdPz", "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
    }
    // 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[] and evaluate some statistics
    final double[] relErrorsP = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
    final double[] relErrorsV = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
    final double errorsPMedian = new Median().evaluate(relErrorsP);
    final double errorsPMean = new Mean().evaluate(relErrorsP);
    final double errorsPMax = new Max().evaluate(relErrorsP);
    final double errorsVMedian = new Median().evaluate(relErrorsV);
    final double errorsVMean = new Mean().evaluate(relErrorsV);
    final double errorsVMax = new Max().evaluate(relErrorsV);
    // Print the results on console ?
    if (printResults) {
        System.out.println();
        System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", errorsPMedian, errorsPMean, errorsPMax);
        System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", errorsVMedian, errorsVMean, errorsVMax);
    }
    // Reference comparison with Range class
    Assert.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
    Assert.assertEquals(0.0, errorsPMean, refErrorsPMean);
    Assert.assertEquals(0.0, errorsPMax, refErrorsPMax);
    Assert.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
    Assert.assertEquals(0.0, errorsVMean, refErrorsVMean);
    Assert.assertEquals(0.0, errorsVMax, refErrorsVMax);
}

Example 5 with ChronologicalComparator

use of org.orekit.time.ChronologicalComparator in project Orekit by CS-SI.

the class RangeAnalyticTest method genericTestParameterDerivatives.

/**
 * Generic test function for derivatives with respect to parameters (station's position in station's topocentric frame)
 * @param isModifier Use of atmospheric modifiers
 * @param isFiniteDifferences Finite differences reference calculation if true, Range class otherwise
 * @param printResults Print the results ?
 * @throws OrekitException
 */
void genericTestParameterDerivatives(final boolean isModifier, final boolean isFiniteDifferences, 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
    for (final GroundStation station : context.stations) {
        station.getEastOffsetDriver().setSelected(true);
        station.getNorthOffsetDriver().setSelected(true);
        station.getZenithOffsetDriver().setSelected(true);
    }
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context), 1.0, 3.0, 300.0);
    // List to store the results
    final List<Double> relErrorList = 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.)) {
                // Add modifiers if test implies it
                final RangeTroposphericDelayModifier modifier = new RangeTroposphericDelayModifier(SaastamoinenModel.getStandardModel());
                if (isModifier) {
                    ((Range) measurement).addModifier(modifier);
                }
                // Parameter corresponding to station position offset
                final GroundStation stationParameter = ((Range) measurement).getStation();
                // 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);
                final ParameterDriver[] drivers = new ParameterDriver[] { stationParameter.getEastOffsetDriver(), stationParameter.getNorthOffsetDriver(), stationParameter.getZenithOffsetDriver() };
                if (printResults) {
                    String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
                    System.out.format(Locale.US, "%-15s  %-23s  %-23s  ", stationName, measurement.getDate(), date);
                }
                for (int i = 0; i < 3; ++i) {
                    final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
                    Assert.assertEquals(1, measurement.getDimension());
                    Assert.assertEquals(1, gradient.length);
                    // Compute a reference value using analytical formulas
                    final EstimatedMeasurement<Range> rangeAnalytic = new RangeAnalytic((Range) measurement).theoreticalEvaluationAnalytic(0, 0, state);
                    if (isModifier) {
                        modifier.modify(rangeAnalytic);
                    }
                    final double ref = rangeAnalytic.getParameterDerivatives(drivers[i])[0];
                    if (printResults) {
                        System.out.format(Locale.US, "%10.3e  %10.3e  ", gradient[0] - ref, FastMath.abs((gradient[0] - ref) / ref));
                    }
                    final double relError = FastMath.abs((ref - gradient[0]) / ref);
                    relErrorList.add(relError);
                // Assert.assertEquals(ref, gradient[0], 6.1e-5 * FastMath.abs(ref));
                }
                if (printResults) {
                    System.out.format(Locale.US, "%n");
                }
            }
        // End if measurement date between previous and current interpolator step
        }
    // End for loop on the measurements
    });
    // Rewind the propagator to initial date
    propagator.propagate(context.initialOrbit.getDate());
    // Sort measurements chronologically
    measurements.sort(new ChronologicalComparator());
    // Print results ? Header
    if (printResults) {
        System.out.format(Locale.US, "%-15s  %-23s  %-23s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s%n", "Station", "Measurement Date", "State Date", "ΔdQx", "rel ΔdQx", "ΔdQy", "rel ΔdQy", "ΔdQz", "rel ΔdQz");
    }
    // Propagate to final measurement's date
    propagator.propagate(measurements.get(measurements.size() - 1).getDate());
    // Convert error list to double[]
    final double[] relErrors = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
    // Compute statistics
    final double relErrorsMedian = new Median().evaluate(relErrors);
    final double relErrorsMean = new Mean().evaluate(relErrors);
    final double relErrorsMax = new Max().evaluate(relErrors);
    // Print the results on console ?
    if (printResults) {
        System.out.println();
        System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", relErrorsMedian, relErrorsMean, relErrorsMax);
    }
    // Assert the results / max values depend on the test
    double refErrorsMedian, refErrorsMean, refErrorsMax;
    // Analytic references
    refErrorsMedian = 1.55e-06;
    refErrorsMean = 3.64e-06;
    refErrorsMax = 6.1e-05;
    Assert.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
    Assert.assertEquals(0.0, relErrorsMean, refErrorsMean);
    Assert.assertEquals(0.0, relErrorsMax, refErrorsMax);
}