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

Example 61 with DSFactory

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

the class AngularAzEl method theoreticalEvaluation.

/**
 * {@inheritDoc}
 */
@Override
protected EstimatedMeasurement<AngularAzEl> theoreticalEvaluation(final int iteration, final int evaluation, final SpacecraftState[] states) throws OrekitException {
    final SpacecraftState state = states[getPropagatorsIndices().get(0)];
    // Azimuth/elevation derivatives are computed with respect to spacecraft state in inertial frame
    // and station parameters
    // ----------------------
    // 
    // Parameters:
    // - 0..2 - Position of the spacecraft in inertial frame
    // - 3..5 - Velocity of the spacecraft in inertial frame
    // - 6..n - station parameters (station offsets, pole, prime meridian...)
    // Get the number of parameters used for derivation
    // Place the selected drivers into a map
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        if (driver.isSelected()) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory factory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, factory);
    // 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 = station.getOffsetToInertial(state.getFrame(), downlinkDateDS, factory, indices);
    // Station position/velocity in inertial frame at end of the downlink leg
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink = offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, zero, zero, zero));
    // Station topocentric frame (east-north-zenith) in inertial frame expressed as DerivativeStructures
    final FieldVector3D<DerivativeStructure> east = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusI(field));
    final FieldVector3D<DerivativeStructure> north = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusJ(field));
    final FieldVector3D<DerivativeStructure> zenith = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusK(field));
    // 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 DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
    // Transit state
    final double delta = downlinkDate.durationFrom(state.getDate());
    final DerivativeStructure deltaMTauD = tauD.negate().add(delta);
    final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue());
    // Transit state (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateDS = pvaDS.shiftedBy(deltaMTauD);
    // Station-satellite vector expressed in inertial frame
    final FieldVector3D<DerivativeStructure> staSat = transitStateDS.getPosition().subtract(stationDownlink.getPosition());
    // Compute azimuth/elevation
    final DerivativeStructure baseAzimuth = DerivativeStructure.atan2(staSat.dotProduct(east), staSat.dotProduct(north));
    final double twoPiWrap = MathUtils.normalizeAngle(baseAzimuth.getReal(), getObservedValue()[0]) - baseAzimuth.getReal();
    final DerivativeStructure azimuth = baseAzimuth.add(twoPiWrap);
    final DerivativeStructure elevation = staSat.dotProduct(zenith).divide(staSat.getNorm()).asin();
    // Prepare the estimation
    final EstimatedMeasurement<AngularAzEl> estimated = new EstimatedMeasurement<>(this, iteration, evaluation, new SpacecraftState[] { transitState }, new TimeStampedPVCoordinates[] { transitStateDS.toTimeStampedPVCoordinates(), stationDownlink.toTimeStampedPVCoordinates() });
    // azimuth - elevation values
    estimated.setEstimatedValue(azimuth.getValue(), elevation.getValue());
    // Partial derivatives of azimuth/elevation with respect to state
    // (beware element at index 0 is the value, not a derivative)
    final double[] azDerivatives = azimuth.getAllDerivatives();
    final double[] elDerivatives = elevation.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(azDerivatives, 1, 7), Arrays.copyOfRange(elDerivatives, 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, azDerivatives[index + 1], elDerivatives[index + 1]);
        }
    }
    return estimated;
}

Example 62 with DSFactory

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

the class OneAxisEllipsoidTest method testMovingGeodeticPointSymmetry.

@Test
public void testMovingGeodeticPointSymmetry() 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)));
    FieldGeodeticPoint<DerivativeStructure> rebuilt = earth.transform(pv, earth.getBodyFrame(), null);
    Assert.assertEquals(lat0, rebuilt.getLatitude().getReal(), 1.0e-16);
    Assert.assertEquals(lat1, rebuilt.getLatitude().getPartialDerivative(1), 5.0e-19);
    Assert.assertEquals(lat2, rebuilt.getLatitude().getPartialDerivative(2), 5.0e-14);
    Assert.assertEquals(lon0, rebuilt.getLongitude().getReal(), 1.0e-16);
    Assert.assertEquals(lon1, rebuilt.getLongitude().getPartialDerivative(1), 5.0e-19);
    Assert.assertEquals(lon2, rebuilt.getLongitude().getPartialDerivative(2), 1.0e-20);
    Assert.assertEquals(alt0, rebuilt.getAltitude().getReal(), 2.0e-11);
    Assert.assertEquals(alt1, rebuilt.getAltitude().getPartialDerivative(1), 6.0e-13);
    Assert.assertEquals(alt2, rebuilt.getAltitude().getPartialDerivative(2), 2.0e-14);
}

Example 63 with DSFactory

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

the class PredefinedIAUPolesTest method testDerivatives.

@Test
public void testDerivatives() {
    final DSFactory factory = new DSFactory(1, 1);
    final AbsoluteDate ref = AbsoluteDate.J2000_EPOCH;
    final FieldAbsoluteDate<DerivativeStructure> refDS = new FieldAbsoluteDate<>(factory.getDerivativeField(), ref);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 60.0);
    for (final IAUPole iaupole : PredefinedIAUPoles.values()) {
        UnivariateDifferentiableVectorFunction dPole = differentiator.differentiate(new UnivariateVectorFunction() {

            @Override
            public double[] value(double t) {
                return iaupole.getPole(ref.shiftedBy(t)).toArray();
            }
        });
        UnivariateDifferentiableFunction dMeridian = differentiator.differentiate(new UnivariateFunction() {

            @Override
            public double value(double t) {
                return iaupole.getPrimeMeridianAngle(ref.shiftedBy(t));
            }
        });
        for (double dt = 0; dt < Constants.JULIAN_YEAR; dt += 3600) {
            final DerivativeStructure dtDS = factory.variable(0, dt);
            final DerivativeStructure[] refPole = dPole.value(dtDS);
            final DerivativeStructure[] fieldPole = iaupole.getPole(refDS.shiftedBy(dtDS)).toArray();
            for (int i = 0; i < 3; ++i) {
                Assert.assertEquals(refPole[i].getValue(), fieldPole[i].getValue(), 2.0e-15);
                Assert.assertEquals(refPole[i].getPartialDerivative(1), fieldPole[i].getPartialDerivative(1), 4.0e-17);
            }
            final DerivativeStructure refMeridian = dMeridian.value(dtDS);
            final DerivativeStructure fieldMeridian = iaupole.getPrimeMeridianAngle(refDS.shiftedBy(dtDS));
            Assert.assertEquals(refMeridian.getValue(), fieldMeridian.getValue(), 4.0e-12);
            Assert.assertEquals(refMeridian.getPartialDerivative(1), fieldMeridian.getPartialDerivative(1), 9.0e-14);
        }
    }
}

Example 64 with DSFactory

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

the class PoissonSeriesParserTest method testDerivativesFromFieldAPI.

@Test
public void testDerivativesFromFieldAPI() throws OrekitException {
    Utils.setDataRoot("regular-data");
    String directory = "/assets/org/orekit/IERS-conventions/";
    PoissonSeriesParser parser = new PoissonSeriesParser(17).withPolynomialPart('t', PolynomialParser.Unit.NO_UNITS).withFirstDelaunay(4).withFirstPlanetary(9).withSinCos(0, 2, 1.0, 3, 1.0);
    InputStream xStream = getClass().getResourceAsStream(directory + "2010/tab5.2a.txt");
    PoissonSeries xSeries = parser.parse(xStream, "2010/tab5.2a.txt");
    InputStream yStream = getClass().getResourceAsStream(directory + "2010/tab5.2b.txt");
    PoissonSeries ySeries = parser.parse(yStream, "2010/tab5.2b.txt");
    InputStream zStream = getClass().getResourceAsStream(directory + "2010/tab5.2d.txt");
    PoissonSeries zSeries = parser.parse(zStream, "2010/tab5.2d.txt");
    final PoissonSeries.CompiledSeries compiled = PoissonSeries.compile(xSeries, ySeries, zSeries);
    TimeScale ut1 = TimeScalesFactory.getUT1(FramesFactory.getEOPHistory(IERSConventions.IERS_2010, true));
    final FundamentalNutationArguments arguments = IERSConventions.IERS_2010.getNutationArguments(ut1);
    UnivariateDifferentiableVectorFunction finite = new FiniteDifferencesDifferentiator(4, 0.4).differentiate((double t) -> compiled.value(arguments.evaluateAll(AbsoluteDate.J2000_EPOCH.shiftedBy(t))));
    DSFactory factory = new DSFactory(1, 1);
    for (double t = 0; t < Constants.JULIAN_DAY; t += 120) {
        // computation of derivatives from API
        Decimal64[] dAPI = compiled.derivative(arguments.evaluateAll(FieldAbsoluteDate.getJ2000Epoch(Decimal64Field.getInstance()).shiftedBy(t)));
        // finite differences computation of derivatives
        DerivativeStructure[] d = finite.value(factory.variable(0, t));
        Assert.assertEquals(d.length, dAPI.length);
        for (int i = 0; i < d.length; ++i) {
            Assert.assertEquals(d[i].getPartialDerivative(1), dAPI[i].getReal(), FastMath.abs(2.0e-7 * d[i].getPartialDerivative(1)));
        }
    }
}

Example 65 with DSFactory

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

the class AngularCoordinates method toDerivativeStructureRotation.

/**
 * Transform the instance to a {@link FieldRotation}<{@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
 * @return rotation with time-derivatives embedded within the coordinates
 * @exception OrekitException if the user specified order is too large
 */
public FieldRotation<DerivativeStructure> toDerivativeStructureRotation(final int order) throws OrekitException {
    // quaternion components
    final double q0 = rotation.getQ0();
    final double q1 = rotation.getQ1();
    final double q2 = rotation.getQ2();
    final double q3 = rotation.getQ3();
    // first time-derivatives of the quaternion
    final double oX = rotationRate.getX();
    final double oY = rotationRate.getY();
    final double oZ = rotationRate.getZ();
    final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
    final double q1Dot = 0.5 * MathArrays.linearCombination(q0, oX, -q3, oY, q2, oZ);
    final double q2Dot = 0.5 * MathArrays.linearCombination(q3, oX, q0, oY, -q1, oZ);
    final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ);
    // second time-derivatives of the quaternion
    final double oXDot = rotationAcceleration.getX();
    final double oYDot = rotationAcceleration.getY();
    final double oZDot = rotationAcceleration.getZ();
    final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] { q1, q2, q3, q1Dot, q2Dot, q3Dot }, new double[] { oXDot, oYDot, oZDot, oX, oY, oZ });
    final double q1DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q2, -q3, q0Dot, q2Dot, -q3Dot }, new double[] { oXDot, oZDot, oYDot, oX, oZ, oY });
    final double q2DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q3, -q1, q0Dot, q3Dot, -q1Dot }, new double[] { oYDot, oXDot, oZDot, oY, oX, oZ });
    final double q3DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q1, -q2, q0Dot, q1Dot, -q2Dot }, new double[] { oZDot, oYDot, oXDot, oZ, oY, oX });
    final DSFactory factory;
    final DerivativeStructure q0DS;
    final DerivativeStructure q1DS;
    final DerivativeStructure q2DS;
    final DerivativeStructure q3DS;
    switch(order) {
        case 0:
            factory = new DSFactory(1, order);
            q0DS = factory.build(q0);
            q1DS = factory.build(q1);
            q2DS = factory.build(q2);
            q3DS = factory.build(q3);
            break;
        case 1:
            factory = new DSFactory(1, order);
            q0DS = factory.build(q0, q0Dot);
            q1DS = factory.build(q1, q1Dot);
            q2DS = factory.build(q2, q2Dot);
            q3DS = factory.build(q3, q3Dot);
            break;
        case 2:
            factory = new DSFactory(1, order);
            q0DS = factory.build(q0, q0Dot, q0DotDot);
            q1DS = factory.build(q1, q1Dot, q1DotDot);
            q2DS = factory.build(q2, q2Dot, q2DotDot);
            q3DS = factory.build(q3, q3Dot, q3DotDot);
            break;
        default:
            throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
    }
    return new FieldRotation<>(q0DS, q1DS, q2DS, q3DS, false);
}