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

Example 56 with DerivativeStructure

use of org.hipparchus.analysis.differentiation.DerivativeStructure 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 57 with DerivativeStructure

use of org.hipparchus.analysis.differentiation.DerivativeStructure 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)));
}

Example 58 with DerivativeStructure

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

the class FundamentalNutationArgumentsTest method testDotField.

@Test
public void testDotField() throws OrekitException {
    final IERSConventions conventions = IERSConventions.IERS_2010;
    final TimeScale ut1 = TimeScalesFactory.getUT1(conventions, false);
    final FundamentalNutationArguments fna = conventions.getNutationArguments(ut1);
    final FieldAbsoluteDate<Decimal64> t0 = new FieldAbsoluteDate<>(Decimal64Field.getInstance(), 2002, 4, 7, 12, 34, 22.5, TimeScalesFactory.getUTC());
    final UnivariateDifferentiableFunction gamma = differentiate(fna, t0, b -> b.getGamma());
    final UnivariateDifferentiableFunction l = differentiate(fna, t0, b -> b.getL());
    final UnivariateDifferentiableFunction lPrime = differentiate(fna, t0, b -> b.getLPrime());
    final UnivariateDifferentiableFunction f = differentiate(fna, t0, b -> b.getF());
    final UnivariateDifferentiableFunction d = differentiate(fna, t0, b -> b.getD());
    final UnivariateDifferentiableFunction lMe = differentiate(fna, t0, b -> b.getLMe());
    final UnivariateDifferentiableFunction lVe = differentiate(fna, t0, b -> b.getLVe());
    final UnivariateDifferentiableFunction lE = differentiate(fna, t0, b -> b.getLE());
    final UnivariateDifferentiableFunction lMa = differentiate(fna, t0, b -> b.getLMa());
    final UnivariateDifferentiableFunction lJu = differentiate(fna, t0, b -> b.getLJu());
    final UnivariateDifferentiableFunction lSa = differentiate(fna, t0, b -> b.getLSa());
    final UnivariateDifferentiableFunction lUr = differentiate(fna, t0, b -> b.getLUr());
    final UnivariateDifferentiableFunction lNe = differentiate(fna, t0, b -> b.getLNe());
    final UnivariateDifferentiableFunction pa = differentiate(fna, t0, b -> b.getPa());
    final DSFactory factory = new DSFactory(1, 1);
    double maxErrorGamma = 0;
    double maxErrorL = 0;
    double maxErrorLPrime = 0;
    double maxErrorF = 0;
    double maxErrorD = 0;
    double maxErrorLMe = 0;
    double maxErrorLVe = 0;
    double maxErrorLE = 0;
    double maxErrorLMa = 0;
    double maxErrorLJu = 0;
    double maxErrorLSa = 0;
    double maxErrorLUr = 0;
    double maxErrorLNe = 0;
    double maxErrorPa = 0;
    for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 60.0) {
        FieldBodiesElements<Decimal64> be = fna.evaluateAll(t0.shiftedBy(dt));
        DerivativeStructure dtDS = factory.variable(0, dt);
        maxErrorGamma = FastMath.max(maxErrorGamma, FastMath.abs(gamma.value(dtDS).getPartialDerivative(1) - be.getGammaDot().getReal()));
        maxErrorL = FastMath.max(maxErrorL, FastMath.abs(l.value(dtDS).getPartialDerivative(1) - be.getLDot().getReal()));
        maxErrorLPrime = FastMath.max(maxErrorLPrime, FastMath.abs(lPrime.value(dtDS).getPartialDerivative(1) - be.getLPrimeDot().getReal()));
        maxErrorF = FastMath.max(maxErrorF, FastMath.abs(f.value(dtDS).getPartialDerivative(1) - be.getFDot().getReal()));
        maxErrorD = FastMath.max(maxErrorD, FastMath.abs(d.value(dtDS).getPartialDerivative(1) - be.getDDot().getReal()));
        maxErrorLMe = FastMath.max(maxErrorLMe, FastMath.abs(lMe.value(dtDS).getPartialDerivative(1) - be.getLMeDot().getReal()));
        maxErrorLVe = FastMath.max(maxErrorLVe, FastMath.abs(lVe.value(dtDS).getPartialDerivative(1) - be.getLVeDot().getReal()));
        maxErrorLE = FastMath.max(maxErrorLE, FastMath.abs(lE.value(dtDS).getPartialDerivative(1) - be.getLEDot().getReal()));
        maxErrorLMa = FastMath.max(maxErrorLMa, FastMath.abs(lMa.value(dtDS).getPartialDerivative(1) - be.getLMaDot().getReal()));
        maxErrorLJu = FastMath.max(maxErrorLJu, FastMath.abs(lJu.value(dtDS).getPartialDerivative(1) - be.getLJuDot().getReal()));
        maxErrorLSa = FastMath.max(maxErrorLSa, FastMath.abs(lSa.value(dtDS).getPartialDerivative(1) - be.getLSaDot().getReal()));
        maxErrorLUr = FastMath.max(maxErrorLUr, FastMath.abs(lUr.value(dtDS).getPartialDerivative(1) - be.getLUrDot().getReal()));
        maxErrorLNe = FastMath.max(maxErrorLNe, FastMath.abs(lNe.value(dtDS).getPartialDerivative(1) - be.getLNeDot().getReal()));
        maxErrorPa = FastMath.max(maxErrorPa, FastMath.abs(pa.value(dtDS).getPartialDerivative(1) - be.getPaDot().getReal()));
    }
    Assert.assertEquals(0, maxErrorGamma, 8.0e-13);
    Assert.assertEquals(0, maxErrorL, 1.0e-14);
    Assert.assertEquals(0, maxErrorLPrime, 6.0e-16);
    Assert.assertEquals(0, maxErrorF, 6.0e-15);
    Assert.assertEquals(0, maxErrorD, 6.0e-15);
    Assert.assertEquals(0, maxErrorLMe, 2.0e-15);
    Assert.assertEquals(0, maxErrorLVe, 5.0e-16);
    Assert.assertEquals(0, maxErrorLE, 3.0e-16);
    Assert.assertEquals(0, maxErrorLMa, 4.0e-16);
    Assert.assertEquals(0, maxErrorLJu, 3.0e-17);
    Assert.assertEquals(0, maxErrorLSa, 4.0e-17);
    Assert.assertEquals(0, maxErrorLUr, 1.0e-16);
    Assert.assertEquals(0, maxErrorLNe, 8.0e-17);
    Assert.assertEquals(0, maxErrorPa, 3.0e-20);
}

Example 59 with DerivativeStructure

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

the class FundamentalNutationArgumentsTest method testDotDouble.

@Test
public void testDotDouble() throws OrekitException {
    final IERSConventions conventions = IERSConventions.IERS_2010;
    final TimeScale ut1 = TimeScalesFactory.getUT1(conventions, false);
    final FundamentalNutationArguments fna = conventions.getNutationArguments(ut1);
    final AbsoluteDate t0 = new AbsoluteDate(2002, 4, 7, 12, 34, 22.5, TimeScalesFactory.getUTC());
    final UnivariateDifferentiableFunction gamma = differentiate(fna, t0, b -> b.getGamma());
    final UnivariateDifferentiableFunction l = differentiate(fna, t0, b -> b.getL());
    final UnivariateDifferentiableFunction lPrime = differentiate(fna, t0, b -> b.getLPrime());
    final UnivariateDifferentiableFunction f = differentiate(fna, t0, b -> b.getF());
    final UnivariateDifferentiableFunction d = differentiate(fna, t0, b -> b.getD());
    final UnivariateDifferentiableFunction lMe = differentiate(fna, t0, b -> b.getLMe());
    final UnivariateDifferentiableFunction lVe = differentiate(fna, t0, b -> b.getLVe());
    final UnivariateDifferentiableFunction lE = differentiate(fna, t0, b -> b.getLE());
    final UnivariateDifferentiableFunction lMa = differentiate(fna, t0, b -> b.getLMa());
    final UnivariateDifferentiableFunction lJu = differentiate(fna, t0, b -> b.getLJu());
    final UnivariateDifferentiableFunction lSa = differentiate(fna, t0, b -> b.getLSa());
    final UnivariateDifferentiableFunction lUr = differentiate(fna, t0, b -> b.getLUr());
    final UnivariateDifferentiableFunction lNe = differentiate(fna, t0, b -> b.getLNe());
    final UnivariateDifferentiableFunction pa = differentiate(fna, t0, b -> b.getPa());
    final DSFactory factory = new DSFactory(1, 1);
    double maxErrorGamma = 0;
    double maxErrorL = 0;
    double maxErrorLPrime = 0;
    double maxErrorF = 0;
    double maxErrorD = 0;
    double maxErrorLMe = 0;
    double maxErrorLVe = 0;
    double maxErrorLE = 0;
    double maxErrorLMa = 0;
    double maxErrorLJu = 0;
    double maxErrorLSa = 0;
    double maxErrorLUr = 0;
    double maxErrorLNe = 0;
    double maxErrorPa = 0;
    for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 60.0) {
        BodiesElements be = fna.evaluateAll(t0.shiftedBy(dt));
        DerivativeStructure dtDS = factory.variable(0, dt);
        maxErrorGamma = FastMath.max(maxErrorGamma, FastMath.abs(gamma.value(dtDS).getPartialDerivative(1) - be.getGammaDot()));
        maxErrorL = FastMath.max(maxErrorL, FastMath.abs(l.value(dtDS).getPartialDerivative(1) - be.getLDot()));
        maxErrorLPrime = FastMath.max(maxErrorLPrime, FastMath.abs(lPrime.value(dtDS).getPartialDerivative(1) - be.getLPrimeDot()));
        maxErrorF = FastMath.max(maxErrorF, FastMath.abs(f.value(dtDS).getPartialDerivative(1) - be.getFDot()));
        maxErrorD = FastMath.max(maxErrorD, FastMath.abs(d.value(dtDS).getPartialDerivative(1) - be.getDDot()));
        maxErrorLMe = FastMath.max(maxErrorLMe, FastMath.abs(lMe.value(dtDS).getPartialDerivative(1) - be.getLMeDot()));
        maxErrorLVe = FastMath.max(maxErrorLVe, FastMath.abs(lVe.value(dtDS).getPartialDerivative(1) - be.getLVeDot()));
        maxErrorLE = FastMath.max(maxErrorLE, FastMath.abs(lE.value(dtDS).getPartialDerivative(1) - be.getLEDot()));
        maxErrorLMa = FastMath.max(maxErrorLMa, FastMath.abs(lMa.value(dtDS).getPartialDerivative(1) - be.getLMaDot()));
        maxErrorLJu = FastMath.max(maxErrorLJu, FastMath.abs(lJu.value(dtDS).getPartialDerivative(1) - be.getLJuDot()));
        maxErrorLSa = FastMath.max(maxErrorLSa, FastMath.abs(lSa.value(dtDS).getPartialDerivative(1) - be.getLSaDot()));
        maxErrorLUr = FastMath.max(maxErrorLUr, FastMath.abs(lUr.value(dtDS).getPartialDerivative(1) - be.getLUrDot()));
        maxErrorLNe = FastMath.max(maxErrorLNe, FastMath.abs(lNe.value(dtDS).getPartialDerivative(1) - be.getLNeDot()));
        maxErrorPa = FastMath.max(maxErrorPa, FastMath.abs(pa.value(dtDS).getPartialDerivative(1) - be.getPaDot()));
    }
    Assert.assertEquals(0, maxErrorGamma, 8.0e-13);
    Assert.assertEquals(0, maxErrorL, 1.0e-14);
    Assert.assertEquals(0, maxErrorLPrime, 6.0e-16);
    Assert.assertEquals(0, maxErrorF, 6.0e-15);
    Assert.assertEquals(0, maxErrorD, 6.0e-15);
    Assert.assertEquals(0, maxErrorLMe, 2.0e-15);
    Assert.assertEquals(0, maxErrorLVe, 5.0e-16);
    Assert.assertEquals(0, maxErrorLE, 3.0e-16);
    Assert.assertEquals(0, maxErrorLMa, 4.0e-16);
    Assert.assertEquals(0, maxErrorLJu, 3.0e-17);
    Assert.assertEquals(0, maxErrorLSa, 4.0e-17);
    Assert.assertEquals(0, maxErrorLUr, 1.0e-16);
    Assert.assertEquals(0, maxErrorLNe, 8.0e-17);
    Assert.assertEquals(0, maxErrorPa, 3.0e-20);
}

Example 60 with DerivativeStructure

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

the class PVCoordinates method toDerivativeStructureVector.

/**
 * Transform the instance to a {@link FieldVector3D}<{@link DerivativeStructure}>.
 * <p>
 * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
 * to the user-specified order.
 * </p>
 * @param order derivation order for the vector components (must be either 0, 1 or 2)
 * @return vector with time-derivatives embedded within the coordinates
 * @exception OrekitException if the user specified order is too large
 */
public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order) throws OrekitException {
    final DSFactory factory;
    final DerivativeStructure x;
    final DerivativeStructure y;
    final DerivativeStructure z;
    switch(order) {
        case 0:
            factory = new DSFactory(1, order);
            x = factory.build(position.getX());
            y = factory.build(position.getY());
            z = factory.build(position.getZ());
            break;
        case 1:
            factory = new DSFactory(1, order);
            x = factory.build(position.getX(), velocity.getX());
            y = factory.build(position.getY(), velocity.getY());
            z = factory.build(position.getZ(), velocity.getZ());
            break;
        case 2:
            factory = new DSFactory(1, order);
            x = factory.build(position.getX(), velocity.getX(), acceleration.getX());
            y = factory.build(position.getY(), velocity.getY(), acceleration.getY());
            z = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
            break;
        default:
            throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
    }
    return new FieldVector3D<>(x, y, z);
}