Examples with FieldVector3D org.hipparchus.geometry.euclidean.threed.FieldVector3D used on opensource projects

Example 91 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class EcksteinHechlerPropagator method toCartesian.

/**
 * Convert circular parameters <em>with derivatives</em> to Cartesian coordinates.
 * @param date date of the orbital parameters
 * @param parameters circular parameters (a, ex, ey, i, raan, alphaM)
 * @return Cartesian coordinates consistent with values and derivatives
 */
private TimeStampedPVCoordinates toCartesian(final AbsoluteDate date, final DerivativeStructure[] parameters) {
    // evaluate coordinates in the orbit canonical reference frame
    final DerivativeStructure cosOmega = parameters[4].cos();
    final DerivativeStructure sinOmega = parameters[4].sin();
    final DerivativeStructure cosI = parameters[3].cos();
    final DerivativeStructure sinI = parameters[3].sin();
    final DerivativeStructure alphaE = meanToEccentric(parameters[5], parameters[1], parameters[2]);
    final DerivativeStructure cosAE = alphaE.cos();
    final DerivativeStructure sinAE = alphaE.sin();
    final DerivativeStructure ex2 = parameters[1].multiply(parameters[1]);
    final DerivativeStructure ey2 = parameters[2].multiply(parameters[2]);
    final DerivativeStructure exy = parameters[1].multiply(parameters[2]);
    final DerivativeStructure q = ex2.add(ey2).subtract(1).negate().sqrt();
    final DerivativeStructure beta = q.add(1).reciprocal();
    final DerivativeStructure bx2 = beta.multiply(ex2);
    final DerivativeStructure by2 = beta.multiply(ey2);
    final DerivativeStructure bxy = beta.multiply(exy);
    final DerivativeStructure u = bxy.multiply(sinAE).subtract(parameters[1].add(by2.subtract(1).multiply(cosAE)));
    final DerivativeStructure v = bxy.multiply(cosAE).subtract(parameters[2].add(bx2.subtract(1).multiply(sinAE)));
    final DerivativeStructure x = parameters[0].multiply(u);
    final DerivativeStructure y = parameters[0].multiply(v);
    // canonical orbit reference frame
    final FieldVector3D<DerivativeStructure> p = new FieldVector3D<>(x.multiply(cosOmega).subtract(y.multiply(cosI.multiply(sinOmega))), x.multiply(sinOmega).add(y.multiply(cosI.multiply(cosOmega))), y.multiply(sinI));
    // dispatch derivatives
    final Vector3D p0 = new Vector3D(p.getX().getValue(), p.getY().getValue(), p.getZ().getValue());
    final Vector3D p1 = new Vector3D(p.getX().getPartialDerivative(1), p.getY().getPartialDerivative(1), p.getZ().getPartialDerivative(1));
    final Vector3D p2 = new Vector3D(p.getX().getPartialDerivative(2), p.getY().getPartialDerivative(2), p.getZ().getPartialDerivative(2));
    return new TimeStampedPVCoordinates(date, p0, p1, p2);
}

Example 92 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class GPSPropagator method propagateInEcef.

/**
 * Gets the PVCoordinates of the GPS SV in {@link #getECEF() ECEF frame}.
 *
 * <p>The algorithm is defined at Table 20-IV from IS-GPS-200 document,
 * with automatic differentiation added to compute velocity and
 * acceleration.</p>
 *
 * @param date the computation date
 * @return the GPS SV PVCoordinates in {@link #getECEF() ECEF frame}
 */
public PVCoordinates propagateInEcef(final AbsoluteDate date) {
    // Duration from GPS ephemeris Reference date
    final DerivativeStructure tk = factory.variable(0, getTk(date));
    // Mean anomaly
    final DerivativeStructure mk = tk.multiply(gpsOrbit.getMeanMotion()).add(gpsOrbit.getM0());
    // Eccentric Anomaly
    final DerivativeStructure ek = getEccentricAnomaly(mk);
    // True Anomaly
    final DerivativeStructure vk = getTrueAnomaly(ek);
    // Argument of Latitude
    final DerivativeStructure phik = vk.add(gpsOrbit.getPa());
    final DerivativeStructure twoPhik = phik.multiply(2);
    final DerivativeStructure c2phi = twoPhik.cos();
    final DerivativeStructure s2phi = twoPhik.sin();
    // Argument of Latitude Correction
    final DerivativeStructure dphik = c2phi.multiply(gpsOrbit.getCuc()).add(s2phi.multiply(gpsOrbit.getCus()));
    // Radius Correction
    final DerivativeStructure drk = c2phi.multiply(gpsOrbit.getCrc()).add(s2phi.multiply(gpsOrbit.getCrs()));
    // Inclination Correction
    final DerivativeStructure dik = c2phi.multiply(gpsOrbit.getCic()).add(s2phi.multiply(gpsOrbit.getCis()));
    // Corrected Argument of Latitude
    final DerivativeStructure uk = phik.add(dphik);
    // Corrected Radius
    final DerivativeStructure rk = ek.cos().multiply(-gpsOrbit.getE()).add(1).multiply(gpsOrbit.getSma()).add(drk);
    // Corrected Inclination
    final DerivativeStructure ik = tk.multiply(gpsOrbit.getIDot()).add(gpsOrbit.getI0()).add(dik);
    final DerivativeStructure cik = ik.cos();
    // Positions in orbital plane
    final DerivativeStructure xk = uk.cos().multiply(rk);
    final DerivativeStructure yk = uk.sin().multiply(rk);
    // Corrected longitude of ascending node
    final DerivativeStructure omk = tk.multiply(gpsOrbit.getOmegaDot() - GPS_AV).add(gpsOrbit.getOmega0() - GPS_AV * gpsOrbit.getTime());
    final DerivativeStructure comk = omk.cos();
    final DerivativeStructure somk = omk.sin();
    // returns the Earth-fixed coordinates
    final FieldVector3D<DerivativeStructure> positionwithDerivatives = new FieldVector3D<>(xk.multiply(comk).subtract(yk.multiply(somk).multiply(cik)), xk.multiply(somk).add(yk.multiply(comk).multiply(cik)), yk.multiply(ik.sin()));
    return new PVCoordinates(positionwithDerivatives);
}

Example 93 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class CartesianOrbit method getPositionDS.

/**
 * Get position with derivatives.
 * @return position with derivatives
 */
private FieldVector3D<DerivativeStructure> getPositionDS() {
    final Vector3D p = getPVCoordinates().getPosition();
    final Vector3D v = getPVCoordinates().getVelocity();
    return new FieldVector3D<>(FACTORY.build(p.getX(), v.getX()), FACTORY.build(p.getY(), v.getY()), FACTORY.build(p.getZ(), v.getZ()));
}

Example 94 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class BodyCenterPointingTest method doTestQDot.

private <T extends RealFieldElement<T>> void doTestQDot(final Field<T> field) throws OrekitException {
    final double ehMu = 3.9860047e14;
    final double ae = 6.378137e6;
    final double c20 = -1.08263e-3;
    final double c30 = 2.54e-6;
    final double c40 = 1.62e-6;
    final double c50 = 2.3e-7;
    final double c60 = -5.5e-7;
    // Satellite position as circular parameters
    T zero = field.getZero();
    final T a = zero.add(7178000.0);
    final T e = zero.add(7e-5);
    final T i = zero.add(FastMath.toRadians(50.));
    final T pa = zero.add(FastMath.toRadians(45.));
    final T raan = zero.add(FastMath.toRadians(270.));
    final T m = zero.add(FastMath.toRadians(5.3 - 270));
    // Computation date
    FieldAbsoluteDate<T> date_comp = new FieldAbsoluteDate<>(field, new DateComponents(2008, 04, 07), TimeComponents.H00, TimeScalesFactory.getUTC());
    // Orbit
    FieldKeplerianOrbit<T> circ = new FieldKeplerianOrbit<>(a, e, i, pa, raan, m, PositionAngle.MEAN, FramesFactory.getEME2000(), date_comp, ehMu);
    // WGS84 Earth model
    OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
    // Earth center pointing attitude provider
    BodyCenterPointing earthCenterAttitudeLaw = new BodyCenterPointing(circ.getFrame(), earth);
    final FieldAbsoluteDate<T> date = FieldAbsoluteDate.getJ2000Epoch(field).shiftedBy(584.);
    final FieldVector3D<T> position = new FieldVector3D<>(zero.add(3220103.), zero.add(69623.), zero.add(6449822.));
    final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(6414.7), zero.add(-2006.), zero.add(-3180.));
    final FieldCircularOrbit<T> initialOrbit = new FieldCircularOrbit<>(new FieldPVCoordinates<>(position, velocity), FramesFactory.getEME2000(), date, ehMu);
    FieldEcksteinHechlerPropagator<T> propagator = new FieldEcksteinHechlerPropagator<>(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);
    propagator.setAttitudeProvider(earthCenterAttitudeLaw);
    List<WeightedObservedPoint> w0 = new ArrayList<WeightedObservedPoint>();
    List<WeightedObservedPoint> w1 = new ArrayList<WeightedObservedPoint>();
    List<WeightedObservedPoint> w2 = new ArrayList<WeightedObservedPoint>();
    List<WeightedObservedPoint> w3 = new ArrayList<WeightedObservedPoint>();
    for (double dt = -1; dt < 1; dt += 0.01) {
        FieldRotation<T> rP = propagator.propagate(date.shiftedBy(dt)).getAttitude().getRotation();
        w0.add(new WeightedObservedPoint(1, dt, rP.getQ0().getReal()));
        w1.add(new WeightedObservedPoint(1, dt, rP.getQ1().getReal()));
        w2.add(new WeightedObservedPoint(1, dt, rP.getQ2().getReal()));
        w3.add(new WeightedObservedPoint(1, dt, rP.getQ3().getReal()));
    }
    double q0DotRef = PolynomialCurveFitter.create(2).fit(w0)[1];
    double q1DotRef = PolynomialCurveFitter.create(2).fit(w1)[1];
    double q2DotRef = PolynomialCurveFitter.create(2).fit(w2)[1];
    double q3DotRef = PolynomialCurveFitter.create(2).fit(w3)[1];
    FieldAttitude<T> a0 = propagator.propagate(date).getAttitude();
    T q0 = a0.getRotation().getQ0();
    T q1 = a0.getRotation().getQ1();
    T q2 = a0.getRotation().getQ2();
    T q3 = a0.getRotation().getQ3();
    T oX = a0.getSpin().getX();
    T oY = a0.getSpin().getY();
    T oZ = a0.getSpin().getZ();
    // first time-derivatives of the quaternion
    double q0Dot = 0.5 * MathArrays.linearCombination(-q1.getReal(), oX.getReal(), -q2.getReal(), oY.getReal(), -q3.getReal(), oZ.getReal());
    double q1Dot = 0.5 * MathArrays.linearCombination(q0.getReal(), oX.getReal(), -q3.getReal(), oY.getReal(), q2.getReal(), oZ.getReal());
    double q2Dot = 0.5 * MathArrays.linearCombination(q3.getReal(), oX.getReal(), q0.getReal(), oY.getReal(), -q1.getReal(), oZ.getReal());
    double q3Dot = 0.5 * MathArrays.linearCombination(-q2.getReal(), oX.getReal(), q1.getReal(), oY.getReal(), q0.getReal(), oZ.getReal());
    Assert.assertEquals(q0DotRef, q0Dot, 5.0e-9);
    Assert.assertEquals(q1DotRef, q1Dot, 5.0e-9);
    Assert.assertEquals(q2DotRef, q2Dot, 5.0e-9);
    Assert.assertEquals(q3DotRef, q3Dot, 5.0e-9);
}

Example 95 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class FieldAttitudeTest method doTestInterpolation.

private <T extends RealFieldElement<T>> void doTestInterpolation(final Field<T> field) throws OrekitException {
    T zero = field.getZero();
    Utils.setDataRoot("regular-data");
    final double ehMu = 3.9860047e14;
    final double ae = 6.378137e6;
    final double c20 = -1.08263e-3;
    final double c30 = 2.54e-6;
    final double c40 = 1.62e-6;
    final double c50 = 2.3e-7;
    final double c60 = -5.5e-7;
    final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field).shiftedBy(584.);
    final FieldVector3D<T> position = new FieldVector3D<>(zero.add(3220103.), zero.add(69623.), zero.add(6449822.));
    final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(6414.7), zero.add(-2006.), zero.add(-3180.));
    final FieldCircularOrbit<T> initialOrbit = new FieldCircularOrbit<>(new FieldPVCoordinates<>(position, velocity), FramesFactory.getEME2000(), date, ehMu);
    FieldEcksteinHechlerPropagator<T> propagator = new FieldEcksteinHechlerPropagator<>(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);
    OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
    propagator.setAttitudeProvider(new BodyCenterPointing(initialOrbit.getFrame(), earth));
    FieldAttitude<T> initialAttitude = propagator.propagate(initialOrbit.getDate()).getAttitude();
    // set up a 5 points sample
    List<FieldAttitude<T>> sample = new ArrayList<FieldAttitude<T>>();
    for (double dt = 0; dt < 251.0; dt += 60.0) {
        sample.add(propagator.propagate(date.shiftedBy(dt)).getAttitude());
    }
    // well inside the sample, interpolation should be better than quadratic shift
    double maxShiftAngleError = 0;
    double maxInterpolationAngleError = 0;
    double maxShiftRateError = 0;
    double maxInterpolationRateError = 0;
    for (double dt_R = 0; dt_R < 240.0; dt_R += 1.0) {
        T dt = zero.add(dt_R);
        FieldAbsoluteDate<T> t = initialOrbit.getDate().shiftedBy(dt);
        FieldAttitude<T> propagated = propagator.propagate(t).getAttitude();
        T shiftAngleError = FieldRotation.distance(propagated.getRotation(), initialAttitude.shiftedBy(dt).getRotation());
        T interpolationAngleError = FieldRotation.distance(propagated.getRotation(), initialAttitude.interpolate(t, sample).getRotation());
        T shiftRateError = FieldVector3D.distance(propagated.getSpin(), initialAttitude.shiftedBy(dt).getSpin());
        T interpolationRateError = FieldVector3D.distance(propagated.getSpin(), initialAttitude.interpolate(t, sample).getSpin());
        maxShiftAngleError = FastMath.max(maxShiftAngleError, shiftAngleError.getReal());
        maxInterpolationAngleError = FastMath.max(maxInterpolationAngleError, interpolationAngleError.getReal());
        maxShiftRateError = FastMath.max(maxShiftRateError, shiftRateError.getReal());
        maxInterpolationRateError = FastMath.max(maxInterpolationRateError, interpolationRateError.getReal());
    }
    Assert.assertTrue(maxShiftAngleError > 6.0e-6);
    Assert.assertTrue(maxInterpolationAngleError < 6.0e-15);
    Assert.assertTrue(maxShiftRateError > 7.0e-8);
    Assert.assertTrue(maxInterpolationRateError < 2.0e-16);
    // past sample end, interpolation error should increase, but still be far better than quadratic shift
    maxShiftAngleError = 0;
    maxInterpolationAngleError = 0;
    maxShiftRateError = 0;
    maxInterpolationRateError = 0;
    for (double dt_R = 250.0; dt_R < 300.0; dt_R += 1.0) {
        T dt = zero.add(dt_R);
        FieldAbsoluteDate<T> t = initialOrbit.getDate().shiftedBy(dt);
        FieldAttitude<T> propagated = propagator.propagate(t).getAttitude();
        T shiftAngleError = FieldRotation.distance(propagated.getRotation(), initialAttitude.shiftedBy(dt).getRotation());
        T interpolationAngleError = FieldRotation.distance(propagated.getRotation(), initialAttitude.interpolate(t, sample).getRotation());
        T shiftRateError = FieldVector3D.distance(propagated.getSpin(), initialAttitude.shiftedBy(dt).getSpin());
        T interpolationRateError = FieldVector3D.distance(propagated.getSpin(), initialAttitude.interpolate(t, sample).getSpin());
        maxShiftAngleError = FastMath.max(maxShiftAngleError, shiftAngleError.getReal());
        maxInterpolationAngleError = FastMath.max(maxInterpolationAngleError, interpolationAngleError.getReal());
        maxShiftRateError = FastMath.max(maxShiftRateError, shiftRateError.getReal());
        maxInterpolationRateError = FastMath.max(maxInterpolationRateError, interpolationRateError.getReal());
    }
    Assert.assertTrue(maxShiftAngleError > 1.0e-5);
    Assert.assertTrue(maxInterpolationAngleError < 8.0e-13);
    Assert.assertTrue(maxShiftRateError > 1.0e-7);
    Assert.assertTrue(maxInterpolationRateError < 6.0e-14);
}