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

Example 46 with ParameterDriver

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

the class AbstractForceModelTest method checkParameterDerivative.

protected void checkParameterDerivative(SpacecraftState state, ForceModel forceModel, String name, double hFactor, double tol) throws OrekitException {
    final DSFactory factory11 = new DSFactory(1, 1);
    final Field<DerivativeStructure> field = factory11.getDerivativeField();
    final FieldSpacecraftState<DerivativeStructure> stateF = new FieldSpacecraftState<DerivativeStructure>(field, state);
    final ParameterDriver[] drivers = forceModel.getParametersDrivers();
    final DerivativeStructure[] parametersDS = new DerivativeStructure[drivers.length];
    for (int i = 0; i < parametersDS.length; ++i) {
        if (drivers[i].getName().equals(name)) {
            parametersDS[i] = factory11.variable(0, drivers[i].getValue());
        } else {
            parametersDS[i] = factory11.constant(drivers[i].getValue());
        }
    }
    FieldVector3D<DerivativeStructure> accDer = forceModel.acceleration(stateF, parametersDS);
    Vector3D derivative = new Vector3D(accDer.getX().getPartialDerivative(1), accDer.getY().getPartialDerivative(1), accDer.getZ().getPartialDerivative(1));
    int selected = -1;
    final double[] parameters = new double[drivers.length];
    for (int i = 0; i < drivers.length; ++i) {
        parameters[i] = drivers[i].getValue();
        if (drivers[i].getName().equals(name)) {
            selected = i;
        }
    }
    double p0 = parameters[selected];
    double hParam = hFactor * p0;
    drivers[selected].setValue(p0 - 1 * hParam);
    parameters[selected] = drivers[selected].getValue();
    Assert.assertEquals(p0 - 1 * hParam, parameters[selected], 1.0e-10);
    final Vector3D gammaM1h = forceModel.acceleration(state, parameters);
    drivers[selected].setValue(p0 + 1 * hParam);
    parameters[selected] = drivers[selected].getValue();
    Assert.assertEquals(p0 + 1 * hParam, parameters[selected], 1.0e-10);
    final Vector3D gammaP1h = forceModel.acceleration(state, parameters);
    drivers[selected].setValue(p0);
    final Vector3D reference = new Vector3D(1 / (2 * hParam), gammaP1h.subtract(gammaM1h));
    final Vector3D delta = derivative.subtract(reference);
    Assert.assertEquals(0, delta.getNorm(), tol * reference.getNorm());
}

Example 47 with ParameterDriver

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

the class TurnAroundRangeTest method genericTestParameterDerivatives.

void genericTestParameterDerivatives(final boolean isModifier, final boolean printResults, final double refErrorQMMedian, final double refErrorQMMean, final double refErrorQMMax, final double refErrorQSMedian, final double refErrorQSMean, final double refErrorQSMax) 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 TAR measurements
    for (Map.Entry<GroundStation, GroundStation> entry : context.TARstations.entrySet()) {
        final GroundStation masterStation = entry.getKey();
        final GroundStation slaveStation = entry.getValue();
        masterStation.getEastOffsetDriver().setSelected(true);
        masterStation.getNorthOffsetDriver().setSelected(true);
        masterStation.getZenithOffsetDriver().setSelected(true);
        slaveStation.getEastOffsetDriver().setSelected(true);
        slaveStation.getNorthOffsetDriver().setSelected(true);
        slaveStation.getZenithOffsetDriver().setSelected(true);
    }
    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();
    // Print results on console ? 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", "ΔdQMx", "rel ΔdQMx", "ΔdQMy", "rel ΔdQMy", "ΔdQMz", "rel ΔdQMz", "ΔdQSx", "rel ΔdQSx", "ΔdQSy", "rel ΔdQSy", "ΔdQSz", "rel ΔdQSz");
    }
    // List to store the results for master and slave station
    final List<Double> relErrorQMList = new ArrayList<Double>();
    final List<Double> relErrorQSList = new ArrayList<Double>();
    // 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);
        }
        // parameter corresponding to station position offset
        final GroundStation masterStationParameter = ((TurnAroundRange) measurement).getMasterStation();
        final GroundStation slaveStationParameter = ((TurnAroundRange) measurement).getSlaveStation();
        // 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 ParameterDriver[] drivers = new ParameterDriver[] { masterStationParameter.getEastOffsetDriver(), masterStationParameter.getNorthOffsetDriver(), masterStationParameter.getZenithOffsetDriver(), slaveStationParameter.getEastOffsetDriver(), slaveStationParameter.getNorthOffsetDriver(), slaveStationParameter.getZenithOffsetDriver() };
        // Print results on console ? Stations' names
        if (printResults) {
            String masterStationName = masterStationParameter.getBaseFrame().getName();
            String slaveStationName = slaveStationParameter.getBaseFrame().getName();
            System.out.format(Locale.US, "%-15s %-15s %-23s  %-23s  ", masterStationName, slaveStationName, measurement.getDate(), date);
        }
        // Loop on the parameters
        for (int i = 0; i < 6; ++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 finite differences
            final ParameterFunction dMkdP = Differentiation.differentiate(new ParameterFunction() {

                /**
                 * {@inheritDoc}
                 */
                @Override
                public double value(final ParameterDriver parameterDriver) throws OrekitException {
                    return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
                }
            }, drivers[i], 3, 20.0);
            final double ref = dMkdP.value(drivers[i]);
            // Deltas
            double dGradient = gradient[0] - ref;
            double dGradientRelative = FastMath.abs(dGradient / ref);
            // Print results on console ? Gradient difference
            if (printResults) {
                System.out.format(Locale.US, "%10.3e  %10.3e  ", dGradient, dGradientRelative);
            }
            // Add relative error to the list
            if (i < 3) {
                relErrorQMList.add(dGradientRelative);
            } else {
                relErrorQSList.add(dGradientRelative);
            }
        }
        // End for loop on the parameters
        if (printResults) {
            System.out.format(Locale.US, "%n");
        }
    }
    // End for loop on the measurements
    // Convert error list to double[]
    final double[] relErrorQM = relErrorQMList.stream().mapToDouble(Double::doubleValue).toArray();
    final double[] relErrorQS = relErrorQSList.stream().mapToDouble(Double::doubleValue).toArray();
    // Compute statistics
    final double relErrorsQMMedian = new Median().evaluate(relErrorQM);
    final double relErrorsQMMean = new Mean().evaluate(relErrorQM);
    final double relErrorsQMMax = new Max().evaluate(relErrorQM);
    final double relErrorsQSMedian = new Median().evaluate(relErrorQS);
    final double relErrorsQSMean = new Mean().evaluate(relErrorQS);
    final double relErrorsQSMax = new Max().evaluate(relErrorQS);
    // Print the results on console ?
    if (printResults) {
        System.out.println();
        System.out.format(Locale.US, "Relative errors dR/dQ master station -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", relErrorsQMMedian, relErrorsQMMean, relErrorsQMMax);
        System.out.format(Locale.US, "Relative errors dR/dQ slave station  -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", relErrorsQSMedian, relErrorsQSMean, relErrorsQSMax);
    }
    // Check values
    Assert.assertEquals(0.0, relErrorsQMMedian, refErrorQMMedian);
    Assert.assertEquals(0.0, relErrorsQMMean, refErrorQMMean);
    Assert.assertEquals(0.0, relErrorsQMMax, refErrorQMMax);
    Assert.assertEquals(0.0, relErrorsQSMedian, refErrorQSMedian);
    Assert.assertEquals(0.0, relErrorsQSMean, refErrorQSMean);
    Assert.assertEquals(0.0, relErrorsQSMax, refErrorQSMax);
}

Example 48 with ParameterDriver

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

the class BiasTest method testEstimateBias.

@SuppressWarnings("unchecked")
@Test
public void testEstimateBias() 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);
    // create range biases: one bias for each station
    final RandomGenerator random = new Well19937a(0x0c4b69da5d64b35al);
    final Bias<?>[] stationsRangeBiases = new Bias<?>[context.stations.size()];
    final double[] realStationsBiases = new double[context.stations.size()];
    for (int i = 0; i < context.stations.size(); ++i) {
        final TopocentricFrame base = context.stations.get(i).getBaseFrame();
        stationsRangeBiases[i] = new Bias<Range>(new String[] { base.getName() + " range bias" }, new double[] { 0.0 }, new double[] { 1.0 }, new double[] { Double.NEGATIVE_INFINITY }, new double[] { Double.POSITIVE_INFINITY });
        realStationsBiases[i] = 2 * random.nextDouble() - 1;
    }
    // create orbit estimator
    final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(), propagatorBuilder);
    // add the measurements, with both spacecraft and stations biases
    for (final ObservedMeasurement<?> measurement : measurements) {
        final Range range = (Range) measurement;
        for (int i = 0; i < context.stations.size(); ++i) {
            if (range.getStation() == context.stations.get(i)) {
                double biasedRange = range.getObservedValue()[0] + realStationsBiases[i];
                final Range m = new Range(range.getStation(), range.getDate(), biasedRange, range.getTheoreticalStandardDeviation()[0], range.getBaseWeight()[0]);
                m.addModifier((Bias<Range>) stationsRangeBiases[i]);
                estimator.addMeasurement(m);
            }
        }
    }
    estimator.setParametersConvergenceThreshold(1.0e-3);
    estimator.setMaxIterations(10);
    estimator.setMaxEvaluations(20);
    // we want to estimate the biases
    for (Bias<?> bias : stationsRangeBiases) {
        for (final ParameterDriver driver : bias.getParametersDrivers()) {
            driver.setSelected(true);
        }
    }
    EstimationTestUtils.checkFit(context, estimator, 2, 3, 0.0, 7.2e-7, 0.0, 2.1e-6, 0.0, 3.7e-7, 0.0, 1.7e-10);
    for (int i = 0; i < stationsRangeBiases.length; ++i) {
        Assert.assertEquals(realStationsBiases[i], stationsRangeBiases[i].getParametersDrivers().get(0).getValue(), 3.3e-6);
    }
}

Example 49 with ParameterDriver

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

the class IonoModifierTest method testRangeIonoModifier.

@Test
public void testRangeIonoModifier() 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);
    propagator.setSlaveMode();
    final RangeIonosphericDelayModifier modifier = new RangeIonosphericDelayModifier(model);
    for (final ObservedMeasurement<?> measurement : measurements) {
        final AbsoluteDate date = measurement.getDate();
        final SpacecraftState refstate = propagator.propagate(date);
        Range range = (Range) measurement;
        EstimatedMeasurement<Range> evalNoMod = range.estimate(12, 17, new SpacecraftState[] { refstate });
        Assert.assertEquals(12, evalNoMod.getIteration());
        Assert.assertEquals(17, evalNoMod.getCount());
        // add modifier
        range.addModifier(modifier);
        boolean found = false;
        for (final EstimationModifier<Range> existing : range.getModifiers()) {
            found = found || existing == modifier;
        }
        Assert.assertTrue(found);
        // 
        EstimatedMeasurement<Range> eval = range.estimate(0, 0, new SpacecraftState[] { refstate });
        Assert.assertEquals(evalNoMod.getStatus(), eval.getStatus());
        eval.setStatus(EstimatedMeasurement.Status.REJECTED);
        Assert.assertEquals(EstimatedMeasurement.Status.REJECTED, eval.getStatus());
        eval.setStatus(evalNoMod.getStatus());
        try {
            eval.getParameterDerivatives(new ParameterDriver("extra", 0, 1, -1, +1));
            Assert.fail("an exception should have been thrown");
        } catch (OrekitIllegalArgumentException oiae) {
            Assert.assertEquals(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, oiae.getSpecifier());
        }
        final double diffMeters = eval.getEstimatedValue()[0] - evalNoMod.getEstimatedValue()[0];
        // TODO: check threshold
        Assert.assertEquals(0.0, diffMeters, 30.0);
    }
}

Example 50 with ParameterDriver

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

the class RangeAnalytic method theoreticalEvaluationValidation.

/**
 * Added for validation
 * Compares directly numeric and analytic computations
 * @param iteration
 * @param evaluation
 * @param state
 * @return
 * @throws OrekitException
 */
protected EstimatedMeasurement<Range> theoreticalEvaluationValidation(final int iteration, final int evaluation, final SpacecraftState state) throws OrekitException {
    // Station & DSFactory attributes from parent Range class
    final GroundStation groundStation = getStation();
    // get the number of parameters used for derivation
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        if (driver.isSelected()) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory dsFactory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = dsFactory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Range derivatives are computed with respect to spacecraft state in inertial frame
    // and station position in station's offset frame
    // -------
    // 
    // Parameters:
    // - 0..2 - Px, Py, Pz   : Position of the spacecraft in inertial frame
    // - 3..5 - Vx, Vy, Vz   : Velocity of the spacecraft in inertial frame
    // - 6..8 - QTx, QTy, QTz: Position of the station in station's offset frame
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, dsFactory);
    // transform between station and inertial frame, expressed as a derivative structure
    // The components of station's position in offset frame are the 3 last derivative parameters
    final AbsoluteDate downlinkDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> downlinkDateDS = new FieldAbsoluteDate<>(field, downlinkDate);
    final FieldTransform<DerivativeStructure> offsetToInertialDownlink = groundStation.getOffsetToInertial(state.getFrame(), downlinkDateDS, dsFactory, indices);
    // Station position in inertial frame at end of the downlink leg
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink = offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, zero, zero, zero));
    // Compute propagation times
    // (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 delay
    final DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
    // Transit state
    final double delta = downlinkDate.durationFrom(state.getDate());
    final DerivativeStructure tauDMDelta = tauD.negate().add(delta);
    final SpacecraftState transitState = state.shiftedBy(tauDMDelta.getValue());
    // Transit state position (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateDS = pvaDS.shiftedBy(tauDMDelta);
    // Station at transit state date (derivatives of tauD taken into account)
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationAtTransitDate = stationDownlink.shiftedBy(tauD.negate());
    // Uplink delay
    final DerivativeStructure tauU = signalTimeOfFlight(stationAtTransitDate, transitStateDS.getPosition(), transitStateDS.getDate());
    // Prepare the evaluation
    final EstimatedMeasurement<Range> estimated = new EstimatedMeasurement<Range>(this, iteration, evaluation, new SpacecraftState[] { transitState }, null);
    // Range value
    final DerivativeStructure tau = tauD.add(tauU);
    final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
    final DerivativeStructure range = tau.multiply(cOver2);
    estimated.setEstimatedValue(range.getValue());
    // Range partial derivatives with respect to state
    final double[] derivatives = range.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 1, 7));
    // (beware element at index 0 is the value, not a derivative)
    for (final ParameterDriver driver : getParametersDrivers()) {
        final Integer index = indices.get(driver.getName());
        if (index != null) {
            estimated.setParameterDerivatives(driver, derivatives[index + 1]);
        }
    }
    // ----------
    // VALIDATION
    // -----------
    // Computation of the value without DS
    // ----------------------------------
    // Time difference between t (date of the measurement) and t' (date tagged in spacecraft state)
    // Station position at signal arrival
    final Transform topoToInertDownlink = groundStation.getOffsetToInertial(state.getFrame(), downlinkDate);
    final PVCoordinates QDownlink = topoToInertDownlink.transformPVCoordinates(PVCoordinates.ZERO);
    // Downlink time of flight from spacecraft to station
    final double td = signalTimeOfFlight(state.getPVCoordinates(), QDownlink.getPosition(), downlinkDate);
    final double dt = delta - td;
    // Transit state position
    final AbsoluteDate transitT = state.getDate().shiftedBy(dt);
    final SpacecraftState transit = state.shiftedBy(dt);
    final Vector3D transitP = transitState.getPVCoordinates().getPosition();
    // Station position at signal departure
    // First guess
    // AbsoluteDate uplinkDate = downlinkDate.shiftedBy(-getObservedValue()[0] / cOver2);
    // final Transform topoToInertUplink =
    // station.getOffsetFrame().getTransformTo(state.getFrame(), uplinkDate);
    // TimeStampedPVCoordinates QUplink = topoToInertUplink.
    // transformPVCoordinates(new TimeStampedPVCoordinates(uplinkDate, PVCoordinates.ZERO));
    // Station position at transit state date
    final Transform topoToInertAtTransitDate = groundStation.getOffsetToInertial(state.getFrame(), transitT);
    TimeStampedPVCoordinates QAtTransitDate = topoToInertAtTransitDate.transformPVCoordinates(new TimeStampedPVCoordinates(transitT, PVCoordinates.ZERO));
    // Uplink time of flight
    final double tu = signalTimeOfFlight(QAtTransitDate, transitP, transitT);
    // Total time of flight
    final double t = td + tu;
    // Real date and position of station at signal departure
    AbsoluteDate uplinkDate = downlinkDate.shiftedBy(-t);
    TimeStampedPVCoordinates QUplink = topoToInertDownlink.shiftedBy(-t).transformPVCoordinates(new TimeStampedPVCoordinates(uplinkDate, PVCoordinates.ZERO));
    // Range value
    double r = t * cOver2;
    double dR = r - range.getValue();
    // td derivatives / state
    // -----------------------
    // Qt = Master station position at tmeas = t = signal arrival at master station
    final Vector3D vel = state.getPVCoordinates().getVelocity();
    final Vector3D Qt_V = QDownlink.getVelocity();
    final Vector3D Ptr = transit.getPVCoordinates().getPosition();
    final Vector3D Ptr_Qt = QDownlink.getPosition().subtract(Ptr);
    final double dDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * td - Vector3D.dotProduct(Ptr_Qt, vel);
    // Derivatives of the downlink time of flight
    final double dtddPx = -Ptr_Qt.getX() / dDown;
    final double dtddPy = -Ptr_Qt.getY() / dDown;
    final double dtddPz = -Ptr_Qt.getZ() / dDown;
    final double dtddVx = dtddPx * dt;
    final double dtddVy = dtddPy * dt;
    final double dtddVz = dtddPz * dt;
    // From the DS
    final double dtddPxDS = tauD.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtddPyDS = tauD.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtddPzDS = tauD.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0);
    final double dtddVxDS = tauD.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0);
    final double dtddVyDS = tauD.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0);
    final double dtddVzDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0);
    // Difference
    final double d_dtddPx = dtddPxDS - dtddPx;
    final double d_dtddPy = dtddPyDS - dtddPy;
    final double d_dtddPz = dtddPzDS - dtddPz;
    final double d_dtddVx = dtddVxDS - dtddVx;
    final double d_dtddVy = dtddVyDS - dtddVy;
    final double d_dtddVz = dtddVzDS - dtddVz;
    // tu derivatives / state
    // -----------------------
    final Vector3D Qt2_Ptr = Ptr.subtract(QUplink.getPosition());
    final double dUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tu - Vector3D.dotProduct(Qt2_Ptr, Qt_V);
    // test
    // // Speed of the station at tmeas-t
    // // Note: Which one to use in the calculation of dUp ???
    // final Vector3D Qt2_V    = QUplink.getVelocity();
    // final double   dUp      = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tu -
    // Vector3D.dotProduct(Qt2_Ptr, Qt2_V);
    // test
    // tu derivatives
    final double dtudPx = 1. / dUp * Qt2_Ptr.dotProduct(Vector3D.PLUS_I.add((Qt_V.subtract(vel)).scalarMultiply(dtddPx)));
    final double dtudPy = 1. / dUp * Qt2_Ptr.dotProduct(Vector3D.PLUS_J.add((Qt_V.subtract(vel)).scalarMultiply(dtddPy)));
    final double dtudPz = 1. / dUp * Qt2_Ptr.dotProduct(Vector3D.PLUS_K.add((Qt_V.subtract(vel)).scalarMultiply(dtddPz)));
    final double dtudVx = dtudPx * dt;
    final double dtudVy = dtudPy * dt;
    final double dtudVz = dtudPz * dt;
    // From the DS
    final double dtudPxDS = tauU.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtudPyDS = tauU.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtudPzDS = tauU.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0);
    final double dtudVxDS = tauU.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0);
    final double dtudVyDS = tauU.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0);
    final double dtudVzDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0);
    // Difference
    final double d_dtudPx = dtudPxDS - dtudPx;
    final double d_dtudPy = dtudPyDS - dtudPy;
    final double d_dtudPz = dtudPzDS - dtudPz;
    final double d_dtudVx = dtudVxDS - dtudVx;
    final double d_dtudVy = dtudVyDS - dtudVy;
    final double d_dtudVz = dtudVzDS - dtudVz;
    // Range derivatives / state
    // -----------------------
    // R = Range
    double dRdPx = (dtddPx + dtudPx) * cOver2;
    double dRdPy = (dtddPy + dtudPy) * cOver2;
    double dRdPz = (dtddPz + dtudPz) * cOver2;
    double dRdVx = (dtddVx + dtudVx) * cOver2;
    double dRdVy = (dtddVy + dtudVy) * cOver2;
    double dRdVz = (dtddVz + dtudVz) * cOver2;
    // With DS
    double dRdPxDS = range.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0);
    double dRdPyDS = range.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0);
    double dRdPzDS = range.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0);
    double dRdVxDS = range.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dRdVyDS = range.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dRdVzDS = range.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0);
    // Diff
    final double d_dRdPx = dRdPxDS - dRdPx;
    final double d_dRdPy = dRdPyDS - dRdPy;
    final double d_dRdPz = dRdPzDS - dRdPz;
    final double d_dRdVx = dRdVxDS - dRdVx;
    final double d_dRdVy = dRdVyDS - dRdVy;
    final double d_dRdVz = dRdVzDS - dRdVz;
    // td derivatives / station
    // -----------------------
    final AngularCoordinates ac = topoToInertDownlink.getAngular().revert();
    final Rotation rotTopoToInert = ac.getRotation();
    final Vector3D omega = ac.getRotationRate();
    final Vector3D dtddQI = Ptr_Qt.scalarMultiply(1. / dDown);
    final double dtddQIx = dtddQI.getX();
    final double dtddQIy = dtddQI.getY();
    final double dtddQIz = dtddQI.getZ();
    final Vector3D dtddQ = rotTopoToInert.applyTo(dtddQI);
    // With DS
    double dtddQxDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtddQyDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtddQzDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtddQx = dtddQxDS - dtddQ.getX();
    final double d_dtddQy = dtddQyDS - dtddQ.getY();
    final double d_dtddQz = dtddQzDS - dtddQ.getZ();
    // tu derivatives / station
    // -----------------------
    // Inertial frame
    final double dtudQIx = 1 / dUp * Qt2_Ptr.dotProduct(Vector3D.MINUS_I.add((Qt_V.subtract(vel)).scalarMultiply(dtddQIx)).subtract(Vector3D.PLUS_I.crossProduct(omega).scalarMultiply(t)));
    final double dtudQIy = 1 / dUp * Qt2_Ptr.dotProduct(Vector3D.MINUS_J.add((Qt_V.subtract(vel)).scalarMultiply(dtddQIy)).subtract(Vector3D.PLUS_J.crossProduct(omega).scalarMultiply(t)));
    final double dtudQIz = 1 / dUp * Qt2_Ptr.dotProduct(Vector3D.MINUS_K.add((Qt_V.subtract(vel)).scalarMultiply(dtddQIz)).subtract(Vector3D.PLUS_K.crossProduct(omega).scalarMultiply(t)));
    // // test
    // final double dtudQIx = 1/dUp*Qt2_Ptr
    // //                        .dotProduct(Vector3D.MINUS_I);
    // //                                    .dotProduct((Qt_V.subtract(vel)).scalarMultiply(dtddQIx));
    // .dotProduct(Vector3D.MINUS_I.crossProduct(omega).scalarMultiply(t));
    // final double dtudQIy = 1/dUp*Qt2_Ptr
    // //                        .dotProduct(Vector3D.MINUS_J);
    // //                                    .dotProduct((Qt_V.subtract(vel)).scalarMultiply(dtddQIy));
    // .dotProduct(Vector3D.MINUS_J.crossProduct(omega).scalarMultiply(t));
    // final double dtudQIz = 1/dUp*Qt2_Ptr
    // //                        .dotProduct(Vector3D.MINUS_K);
    // //                                    .dotProduct((Qt_V.subtract(vel)).scalarMultiply(dtddQIz));
    // .dotProduct(Vector3D.MINUS_K.crossProduct(omega).scalarMultiply(t));
    // 
    // double dtu_dQxDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    // double dtu_dQyDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    // double dtu_dQzDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // final Vector3D dtudQDS = new Vector3D(dtu_dQxDS, dtu_dQyDS, dtu_dQzDS);
    // final Vector3D dtudQIDS = rotTopoToInert.applyInverseTo(dtudQDS);
    // double dtudQIxDS = dtudQIDS.getX();
    // double dtudQIyDS = dtudQIDS.getY();
    // double dtudQIxzS = dtudQIDS.getZ();
    // // test
    // Topocentric frame
    final Vector3D dtudQI = new Vector3D(dtudQIx, dtudQIy, dtudQIz);
    final Vector3D dtudQ = rotTopoToInert.applyTo(dtudQI);
    // With DS
    double dtudQxDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtudQyDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtudQzDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtudQx = dtudQxDS - dtudQ.getX();
    final double d_dtudQy = dtudQyDS - dtudQ.getY();
    final double d_dtudQz = dtudQzDS - dtudQ.getZ();
    // Range derivatives / station
    // -----------------------
    double dRdQx = (dtddQ.getX() + dtudQ.getX()) * cOver2;
    double dRdQy = (dtddQ.getY() + dtudQ.getY()) * cOver2;
    double dRdQz = (dtddQ.getZ() + dtudQ.getZ()) * cOver2;
    // With DS
    double dRdQxDS = range.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dRdQyDS = range.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dRdQzDS = range.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dRdQx = dRdQxDS - dRdQx;
    final double d_dRdQy = dRdQyDS - dRdQy;
    final double d_dRdQz = dRdQzDS - dRdQz;
    // Print results to avoid warning
    final boolean printResults = false;
    if (printResults) {
        System.out.println("dR = " + dR);
        System.out.println("d_dtddPx = " + d_dtddPx);
        System.out.println("d_dtddPy = " + d_dtddPy);
        System.out.println("d_dtddPz = " + d_dtddPz);
        System.out.println("d_dtddVx = " + d_dtddVx);
        System.out.println("d_dtddVy = " + d_dtddVy);
        System.out.println("d_dtddVz = " + d_dtddVz);
        System.out.println("d_dtudPx = " + d_dtudPx);
        System.out.println("d_dtudPy = " + d_dtudPy);
        System.out.println("d_dtudPz = " + d_dtudPz);
        System.out.println("d_dtudVx = " + d_dtudVx);
        System.out.println("d_dtudVy = " + d_dtudVy);
        System.out.println("d_dtudVz = " + d_dtudVz);
        System.out.println("d_dRdPx = " + d_dRdPx);
        System.out.println("d_dRdPy = " + d_dRdPy);
        System.out.println("d_dRdPz = " + d_dRdPz);
        System.out.println("d_dRdVx = " + d_dRdVx);
        System.out.println("d_dRdVy = " + d_dRdVy);
        System.out.println("d_dRdVz = " + d_dRdVz);
        System.out.println("d_dtddQx = " + d_dtddQx);
        System.out.println("d_dtddQy = " + d_dtddQy);
        System.out.println("d_dtddQz = " + d_dtddQz);
        System.out.println("d_dtudQx = " + d_dtudQx);
        System.out.println("d_dtudQy = " + d_dtudQy);
        System.out.println("d_dtudQz = " + d_dtudQz);
        System.out.println("d_dRdQx = " + d_dRdQx);
        System.out.println("d_dRdQy = " + d_dRdQy);
        System.out.println("d_dRdQz = " + d_dRdQz);
    }
    // Dummy return
    return estimated;
}