Examples with StateFunction org.orekit.utils.StateFunction used on opensource projects

Example 1 with StateFunction

use of org.orekit.utils.StateFunction in project Orekit by CS-SI.

the class TurnAroundRangeTest method genericTestStateDerivatives.

void genericTestStateDerivatives(final boolean isModifier, 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");
    // Context context = EstimationTestUtils.geoStationnaryContext();
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 0.001);
    // create perfect range2 measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new TurnAroundRangeMeasurementCreator(context), 1.0, 3.0, 300.0);
    propagator.setSlaveMode();
    double[] errorsP = new double[3 * measurements.size()];
    double[] errorsV = new double[3 * measurements.size()];
    int indexP = 0;
    int indexV = 0;
    // Print the results ? Header
    if (printResults) {
        System.out.format(Locale.US, "%-15s  %-15s  %-23s  %-23s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s%n", "Master Station", "Slave Station", "Measurement Date", "State Date", "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz", "rel ΔdPx", "rel ΔdPy", "rel ΔdPz", "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
    }
    // Loop on the measurements
    for (final ObservedMeasurement<?> measurement : measurements) {
        // Add modifiers if test implies it
        final TurnAroundRangeTroposphericDelayModifier modifier = new TurnAroundRangeTroposphericDelayModifier(SaastamoinenModel.getStandardModel());
        if (isModifier) {
            ((TurnAroundRange) 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 = propagator.propagate(date);
        final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
        // 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 {
                return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
            }
        }, measurement.getDimension(), propagator.getAttitudeProvider(), OrbitType.CARTESIAN, PositionAngle.TRUE, 2.0, 3).value(state);
        Assert.assertEquals(jacobianRef.length, jacobian.length);
        Assert.assertEquals(jacobianRef[0].length, jacobian[0].length);
        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[indexP++] = dJacobianRelative[i][j];
                } else {
                    errorsV[indexV++] = dJacobianRelative[i][j];
                }
            }
        }
        // Print results on the console ? Print the Jacobian
        if (printResults) {
            String masterStationName = ((TurnAroundRange) measurement).getMasterStation().getBaseFrame().getName();
            String slaveStationName = ((TurnAroundRange) measurement).getSlaveStation().getBaseFrame().getName();
            System.out.format(Locale.US, "%-15s  %-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", masterStationName, slaveStationName, 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 loop on the measurements
    // Compute some statistics
    final double errorsPMedian = new Median().evaluate(errorsP);
    final double errorsPMean = new Mean().evaluate(errorsP);
    final double errorsPMax = new Max().evaluate(errorsP);
    final double errorsVMedian = new Median().evaluate(errorsV);
    final double errorsVMean = new Mean().evaluate(errorsV);
    final double errorsVMax = new Max().evaluate(errorsV);
    // Print the results on console ? Final results
    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 StateFunction

use of org.orekit.utils.StateFunction in project Orekit by CS-SI.

the class AngularRaDecTest method testStateDerivatives.

@Test
public void testStateDerivatives() throws OrekitException {
    Context context = EstimationTestUtils.geoStationnaryContext("regular-data:potential:tides");
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(OrbitType.EQUINOCTIAL, PositionAngle.TRUE, false, 1.0e-6, 60.0, 0.001);
    // create perfect azimuth-elevation measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new AngularRaDecMeasurementCreator(context), 0.25, 3.0, 600.0);
    propagator.setSlaveMode();
    // Compute measurements.
    double[] RaerrorsP = new double[3 * measurements.size()];
    double[] RaerrorsV = new double[3 * measurements.size()];
    double[] DecerrorsP = new double[3 * measurements.size()];
    double[] DecerrorsV = new double[3 * measurements.size()];
    int RaindexP = 0;
    int RaindexV = 0;
    int DecindexP = 0;
    int DecindexV = 0;
    for (final ObservedMeasurement<?> measurement : measurements) {
        // parameter corresponding to station position offset
        final GroundStation stationParameter = ((AngularRaDec) 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
        // angular would have depended only on the current position but
        // not on the current velocity.
        final AbsoluteDate datemeas = measurement.getDate();
        SpacecraftState state = propagator.propagate(datemeas);
        final Vector3D stationP = stationParameter.getOffsetToInertial(state.getFrame(), datemeas).transformPosition(Vector3D.ZERO);
        final double meanDelay = AbstractMeasurement.signalTimeOfFlight(state.getPVCoordinates(), stationP, datemeas);
        final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
        state = propagator.propagate(date);
        final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
        Assert.assertEquals(2, estimated.getParticipants().length);
        final double[][] jacobian = estimated.getStateDerivatives(0);
        // compute a reference value using finite differences
        final double[][] finiteDifferencesJacobian = 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, 250.0, 4).value(state);
        Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
        Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
        final double smallest = FastMath.ulp((double) 1.0);
        for (int i = 0; i < jacobian.length; ++i) {
            for (int j = 0; j < jacobian[i].length; ++j) {
                double relativeError = FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) / finiteDifferencesJacobian[i][j]);
                if ((FastMath.sqrt(finiteDifferencesJacobian[i][j]) < smallest) && (FastMath.sqrt(jacobian[i][j]) < smallest)) {
                    relativeError = 0.0;
                }
                if (j < 3) {
                    if (i == 0) {
                        RaerrorsP[RaindexP++] = relativeError;
                    } else {
                        DecerrorsP[DecindexP++] = relativeError;
                    }
                } else {
                    if (i == 0) {
                        RaerrorsV[RaindexV++] = relativeError;
                    } else {
                        DecerrorsV[DecindexV++] = relativeError;
                    }
                }
            }
        }
    }
    // median errors on Azimuth
    Assert.assertEquals(0.0, new Median().evaluate(RaerrorsP), 4.8e-11);
    Assert.assertEquals(0.0, new Median().evaluate(RaerrorsV), 2.2e-5);
    // median errors on Elevation
    Assert.assertEquals(0.0, new Median().evaluate(DecerrorsP), 1.5e-11);
    Assert.assertEquals(0.0, new Median().evaluate(DecerrorsV), 5.4e-6);
}

Example 3 with StateFunction

use of org.orekit.utils.StateFunction 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 4 with StateFunction

use of org.orekit.utils.StateFunction 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 StateFunction

use of org.orekit.utils.StateFunction in project Orekit by CS-SI.

the class RangeRateTest method testStateDerivativesOneWay.

@Test
public void testStateDerivativesOneWay() 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 rate 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);
    for (final ObservedMeasurement<?> m : measurements) {
        Assert.assertFalse(((RangeRate) m).isTwoWay());
    }
    propagator.setSlaveMode();
    double maxRelativeError = 0;
    for (final ObservedMeasurement<?> measurement : measurements) {
        // measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
        final double meanDelay = 1;
        final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
        final SpacecraftState state = propagator.propagate(date);
        final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
        Assert.assertEquals(2, estimated.getParticipants().length);
        final double[][] jacobian = estimated.getStateDerivatives(0);
        final double[][] finiteDifferencesJacobian = Differentiation.differentiate(new StateFunction() {

            public double[] value(final SpacecraftState state) throws OrekitException {
                return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
            }
        }, 1, propagator.getAttitudeProvider(), OrbitType.CARTESIAN, PositionAngle.TRUE, 15.0, 3).value(state);
        Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
        Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
        for (int i = 0; i < jacobian.length; ++i) {
            for (int j = 0; j < jacobian[i].length; ++j) {
                // check the values returned by getStateDerivatives() are correct
                maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) / finiteDifferencesJacobian[i][j]));
            }
        }
    }
    Assert.assertEquals(0, maxRelativeError, 1.6e-8);
}