Examples with DSFactory org.hipparchus.analysis.differentiation.DSFactory used on opensource projects

Example 31 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class InterSatellitesRange method theoreticalEvaluation.

/**
 * {@inheritDoc}
 */
@Override
protected EstimatedMeasurement<InterSatellitesRange> theoreticalEvaluation(final int iteration, final int evaluation, final SpacecraftState[] states) throws OrekitException {
    // Range derivatives are computed with respect to spacecrafts states in inertial frame
    // ----------------------
    // 
    // Parameters:
    // - 0..2  - Position of the satellite 1 in inertial frame
    // - 3..5  - Velocity of the satellite 1 in inertial frame
    // - 6..8  - Position of the satellite 2 in inertial frame
    // - 9..11 - Velocity of the satellite 2 in inertial frame
    final int nbParams = 12;
    final DSFactory factory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = factory.getDerivativeField();
    // coordinates of both satellites
    final SpacecraftState state1 = states[getPropagatorsIndices().get(0)];
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pva1 = getCoordinates(state1, 0, factory);
    final SpacecraftState state2 = states[getPropagatorsIndices().get(1)];
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pva2 = getCoordinates(state2, 6, factory);
    // compute propagation times
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have delta == tauD and transitState will be the same as state)
    // downlink delay
    final FieldAbsoluteDate<DerivativeStructure> arrivalDate = new FieldAbsoluteDate<>(field, getDate());
    final TimeStampedFieldPVCoordinates<DerivativeStructure> s1Downlink = pva1.shiftedBy(arrivalDate.durationFrom(pva1.getDate()));
    final DerivativeStructure tauD = signalTimeOfFlight(pva2, s1Downlink.getPosition(), arrivalDate);
    // Transit state
    final double delta = getDate().durationFrom(state2.getDate());
    final DerivativeStructure deltaMTauD = tauD.negate().add(delta);
    // prepare the evaluation
    final EstimatedMeasurement<InterSatellitesRange> estimated;
    final DerivativeStructure range;
    if (twoway) {
        // Transit state (re)computed with derivative structures
        final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateDS = pva2.shiftedBy(deltaMTauD);
        // uplink delay
        final DerivativeStructure tauU = signalTimeOfFlight(pva1, transitStateDS.getPosition(), transitStateDS.getDate());
        estimated = new EstimatedMeasurement<>(this, iteration, evaluation, new SpacecraftState[] { state1.shiftedBy(deltaMTauD.getValue()), state2.shiftedBy(deltaMTauD.getValue()) }, new TimeStampedPVCoordinates[] { state1.shiftedBy(delta - tauD.getValue() - tauU.getValue()).getPVCoordinates(), state2.shiftedBy(delta - tauD.getValue()).getPVCoordinates(), state1.shiftedBy(delta).getPVCoordinates() });
        // Range value
        range = tauD.add(tauU).multiply(0.5 * Constants.SPEED_OF_LIGHT);
    } else {
        estimated = new EstimatedMeasurement<>(this, iteration, evaluation, new SpacecraftState[] { state1.shiftedBy(deltaMTauD.getValue()), state2.shiftedBy(deltaMTauD.getValue()) }, new TimeStampedPVCoordinates[] { state2.shiftedBy(delta - tauD.getValue()).getPVCoordinates(), state1.shiftedBy(delta).getPVCoordinates() });
        // Range value
        range = tauD.multiply(Constants.SPEED_OF_LIGHT);
    }
    estimated.setEstimatedValue(range.getValue());
    // Range partial derivatives with respect to states
    final double[] derivatives = range.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 1, 7));
    estimated.setStateDerivatives(1, Arrays.copyOfRange(derivatives, 7, 13));
    return estimated;
}

Example 32 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class FieldEquinoctialOrbitTest method differentiate.

private <T extends RealFieldElement<T>, S extends Function<FieldEquinoctialOrbit<T>, T>> double differentiate(TimeStampedFieldPVCoordinates<T> pv, Frame frame, double mu, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(new FieldEquinoctialOrbit<>(pv.shiftedBy(dt), frame, mu)).getReal();
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}

Example 33 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class FieldKeplerianOrbitTest method differentiate.

private <T extends RealFieldElement<T>, S extends Function<FieldKeplerianOrbit<T>, T>> double differentiate(TimeStampedFieldPVCoordinates<T> pv, Frame frame, double mu, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(new FieldKeplerianOrbit<>(pv.shiftedBy(dt), frame, mu)).getReal();
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}

Example 34 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class KeplerianOrbitTest method differentiate.

private <S extends Function<KeplerianOrbit, Double>> double differentiate(KeplerianOrbit orbit, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(orbit.shiftedBy(dt));
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}

Example 35 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class OneAxisEllipsoidTest method testMovingGeodeticPoint.

@Test
public void testMovingGeodeticPoint() throws OrekitException {
    final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
    double lat0 = FastMath.toRadians(60.0);
    double lon0 = FastMath.toRadians(25.0);
    double alt0 = 100.0;
    double lat1 = 1.0e-3;
    double lon1 = -2.0e-3;
    double alt1 = 1.2;
    double lat2 = -1.0e-5;
    double lon2 = -3.0e-5;
    double alt2 = -0.01;
    final DSFactory factory = new DSFactory(1, 2);
    final DerivativeStructure latDS = factory.build(lat0, lat1, lat2);
    final DerivativeStructure lonDS = factory.build(lon0, lon1, lon2);
    final DerivativeStructure altDS = factory.build(alt0, alt1, alt2);
    // direct computation of position, velocity and acceleration
    PVCoordinates pv = new PVCoordinates(earth.transform(new FieldGeodeticPoint<>(latDS, lonDS, altDS)));
    // finite differences computation
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(5, 0.1);
    UnivariateDifferentiableFunction fx = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            GeodeticPoint gp = new GeodeticPoint(latDS.taylor(dt), lonDS.taylor(dt), altDS.taylor(dt));
            return earth.transform(gp).getX();
        }
    });
    UnivariateDifferentiableFunction fy = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            GeodeticPoint gp = new GeodeticPoint(latDS.taylor(dt), lonDS.taylor(dt), altDS.taylor(dt));
            return earth.transform(gp).getY();
        }
    });
    UnivariateDifferentiableFunction fz = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            GeodeticPoint gp = new GeodeticPoint(latDS.taylor(dt), lonDS.taylor(dt), altDS.taylor(dt));
            return earth.transform(gp).getZ();
        }
    });
    DerivativeStructure dtZero = factory.variable(0, 0.0);
    DerivativeStructure xDS = fx.value(dtZero);
    DerivativeStructure yDS = fy.value(dtZero);
    DerivativeStructure zDS = fz.value(dtZero);
    Assert.assertEquals(xDS.getValue(), pv.getPosition().getX(), 2.0e-20 * FastMath.abs(xDS.getValue()));
    Assert.assertEquals(xDS.getPartialDerivative(1), pv.getVelocity().getX(), 2.0e-12 * FastMath.abs(xDS.getPartialDerivative(1)));
    Assert.assertEquals(xDS.getPartialDerivative(2), pv.getAcceleration().getX(), 2.0e-9 * FastMath.abs(xDS.getPartialDerivative(2)));
    Assert.assertEquals(yDS.getValue(), pv.getPosition().getY(), 2.0e-20 * FastMath.abs(yDS.getValue()));
    Assert.assertEquals(yDS.getPartialDerivative(1), pv.getVelocity().getY(), 2.0e-12 * FastMath.abs(yDS.getPartialDerivative(1)));
    Assert.assertEquals(yDS.getPartialDerivative(2), pv.getAcceleration().getY(), 2.0e-9 * FastMath.abs(yDS.getPartialDerivative(2)));
    Assert.assertEquals(zDS.getValue(), pv.getPosition().getZ(), 2.0e-20 * FastMath.abs(zDS.getValue()));
    Assert.assertEquals(zDS.getPartialDerivative(1), pv.getVelocity().getZ(), 2.0e-12 * FastMath.abs(zDS.getPartialDerivative(1)));
    Assert.assertEquals(zDS.getPartialDerivative(2), pv.getAcceleration().getZ(), 2.0e-9 * FastMath.abs(zDS.getPartialDerivative(2)));
}