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

Example 1 with FieldRotation

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

the class EstimationTestUtils method geoStationnaryContext.

public static Context geoStationnaryContext(final String dataRoot) throws OrekitException {
    Utils.setDataRoot(dataRoot);
    Context context = new Context();
    context.conventions = IERSConventions.IERS_2010;
    context.utc = TimeScalesFactory.getUTC();
    context.ut1 = TimeScalesFactory.getUT1(context.conventions, true);
    context.displacements = new StationDisplacement[0];
    String Myframename = "MyEarthFrame";
    final AbsoluteDate datedef = new AbsoluteDate(2000, 1, 1, 12, 0, 0.0, context.utc);
    final double omega = Constants.WGS84_EARTH_ANGULAR_VELOCITY;
    final Vector3D rotationRate = new Vector3D(0.0, 0.0, omega);
    TransformProvider MyEarthFrame = new TransformProvider() {

        private static final long serialVersionUID = 1L;

        public Transform getTransform(final AbsoluteDate date) {
            final double rotationduration = date.durationFrom(datedef);
            final Vector3D alpharot = new Vector3D(rotationduration, rotationRate);
            final Rotation rotation = new Rotation(Vector3D.PLUS_K, -alpharot.getZ(), RotationConvention.VECTOR_OPERATOR);
            return new Transform(date, rotation, rotationRate);
        }

        public <T extends RealFieldElement<T>> FieldTransform<T> getTransform(final FieldAbsoluteDate<T> date) {
            final T rotationduration = date.durationFrom(datedef);
            final FieldVector3D<T> alpharot = new FieldVector3D<>(rotationduration, rotationRate);
            final FieldRotation<T> rotation = new FieldRotation<>(FieldVector3D.getPlusK(date.getField()), alpharot.getZ().negate(), RotationConvention.VECTOR_OPERATOR);
            return new FieldTransform<>(date, rotation, new FieldVector3D<>(date.getField(), rotationRate));
        }
    };
    Frame FrameTest = new Frame(FramesFactory.getEME2000(), MyEarthFrame, Myframename, true);
    // Earth is spherical, rotating in one sidereal day
    context.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, 0.0, FrameTest);
    context.sun = CelestialBodyFactory.getSun();
    context.moon = CelestialBodyFactory.getMoon();
    context.radiationSensitive = new IsotropicRadiationClassicalConvention(2.0, 0.2, 0.8);
    context.dragSensitive = new IsotropicDrag(2.0, 1.2);
    GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
    AstronomicalAmplitudeReader aaReader = new AstronomicalAmplitudeReader("hf-fes2004.dat", 5, 2, 3, 1.0);
    DataProvidersManager.getInstance().feed(aaReader.getSupportedNames(), aaReader);
    Map<Integer, Double> map = aaReader.getAstronomicalAmplitudesMap();
    GravityFieldFactory.addOceanTidesReader(new FESCHatEpsilonReader("fes2004-7x7.dat", 0.01, FastMath.toRadians(1.0), OceanLoadDeformationCoefficients.IERS_2010, map));
    context.gravity = GravityFieldFactory.getNormalizedProvider(20, 20);
    // semimajor axis for a geostationnary satellite
    double da = FastMath.cbrt(context.gravity.getMu() / (omega * omega));
    // context.stations = Arrays.asList(context.createStation(  0.0,  0.0, 0.0, "Lat0_Long0"),
    // context.createStation( 62.29639,   -7.01250,  880.0, "Slættaratindur")
    // );
    context.stations = Arrays.asList(context.createStation(0.0, 0.0, 0.0, "Lat0_Long0"));
    // Station position & velocity in EME2000
    final Vector3D geovelocity = new Vector3D(0., 0., 0.);
    // Compute the frames transformation from station frame to EME2000
    Transform topoToEME = context.stations.get(0).getBaseFrame().getTransformTo(FramesFactory.getEME2000(), new AbsoluteDate(2000, 1, 1, 12, 0, 0.0, context.utc));
    // Station position in EME2000 at reference date
    Vector3D stationPositionEME = topoToEME.transformPosition(Vector3D.ZERO);
    // Satellite position and velocity in Station Frame
    final Vector3D sat_pos = new Vector3D(0., 0., da - stationPositionEME.getNorm());
    final Vector3D acceleration = new Vector3D(-context.gravity.getMu(), sat_pos);
    final PVCoordinates pv_sat_topo = new PVCoordinates(sat_pos, geovelocity, acceleration);
    // satellite position in EME2000
    final PVCoordinates pv_sat_iner = topoToEME.transformPVCoordinates(pv_sat_topo);
    // Geo-stationary Satellite Orbit, tightly above the station (l0-L0)
    context.initialOrbit = new KeplerianOrbit(pv_sat_iner, FramesFactory.getEME2000(), new AbsoluteDate(2000, 1, 1, 12, 0, 0.0, context.utc), context.gravity.getMu());
    context.stations = Arrays.asList(context.createStation(10.0, 45.0, 0.0, "Lat10_Long45"));
    // Turn-around range stations
    // Map entry = master station
    // Map value = slave station associated
    context.TARstations = new HashMap<GroundStation, GroundStation>();
    context.TARstations.put(context.createStation(41.977, 13.600, 671.354, "Fucino"), context.createStation(43.604, 1.444, 263.0, "Toulouse"));
    context.TARstations.put(context.createStation(49.867, 8.65, 144.0, "Darmstadt"), context.createStation(-25.885, 27.707, 1566.633, "Pretoria"));
    return context;
}

Example 2 with FieldRotation

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

the class FixedRateTest method doTestNonZeroRate.

private <T extends RealFieldElement<T>> void doTestNonZeroRate(final Field<T> field) throws OrekitException {
    final T zero = field.getZero();
    FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 3, 2), new TimeComponents(13, 17, 7.865), TimeScalesFactory.getUTC());
    final T rate = zero.add(2 * FastMath.PI / (12 * 60));
    final Frame frame = FramesFactory.getEME2000();
    FixedRate law = new FixedRate(new Attitude(date.toAbsoluteDate(), frame, new Rotation(0.48, 0.64, 0.36, 0.48, false), new Vector3D(rate.getReal(), Vector3D.PLUS_K), Vector3D.ZERO));
    FieldPVCoordinates<T> pv = new FieldPVCoordinates<>(field.getOne(), new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0), new Vector3D(0, 0, 3680.853673522056)));
    FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, FramesFactory.getEME2000(), date, 3.986004415e14);
    FieldRotation<T> attitude0 = law.getAttitude(orbit, date, frame).getRotation();
    Assert.assertEquals(0, Rotation.distance(attitude0.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
    FieldRotation<T> attitude1 = law.getAttitude(orbit.shiftedBy(zero.add(10.0)), date.shiftedBy(10.0), frame).getRotation();
    Assert.assertEquals(10 * rate.getReal(), Rotation.distance(attitude1.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
    FieldRotation<T> attitude2 = law.getAttitude(orbit.shiftedBy(zero.add(-20.0)), date.shiftedBy(-20.0), frame).getRotation();
    Assert.assertEquals(20 * rate.getReal(), Rotation.distance(attitude2.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
    Assert.assertEquals(30 * rate.getReal(), Rotation.distance(attitude2.toRotation(), attitude1.toRotation()), 1.0e-10);
    FieldRotation<T> attitude3 = law.getAttitude(orbit.shiftedBy(zero.add(0.0)), date, frame).getRotation();
    Assert.assertEquals(0, Rotation.distance(attitude3.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
}

Example 3 with FieldRotation

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

the class FixedRateTest method doTestZeroRate.

private <T extends RealFieldElement<T>> void doTestZeroRate(final Field<T> field) throws OrekitException {
    final T zero = field.getZero();
    FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 3, 2), new TimeComponents(13, 17, 7.865), TimeScalesFactory.getUTC());
    final Frame frame = FramesFactory.getEME2000();
    FixedRate law = new FixedRate(new Attitude(date.toAbsoluteDate(), frame, new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO));
    FieldPVCoordinates<T> pv = new FieldPVCoordinates<>(field.getOne(), new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0), new Vector3D(0, 0, 3680.853673522056)));
    FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, frame, date, 3.986004415e14);
    FieldRotation<T> attitude0 = law.getAttitude(orbit, date, frame).getRotation();
    Assert.assertEquals(0, Rotation.distance(attitude0.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
    FieldRotation<T> attitude1 = law.getAttitude(orbit.shiftedBy(zero.add(10.0)), date.shiftedBy(10.0), frame).getRotation();
    Assert.assertEquals(0, Rotation.distance(attitude1.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
    FieldRotation<T> attitude2 = law.getAttitude(orbit.shiftedBy(zero.add(20.0)), date.shiftedBy(20.0), frame).getRotation();
    Assert.assertEquals(0, Rotation.distance(attitude2.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
}

Example 4 with FieldRotation

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

the class AbstractForceModelTest method checkStateJacobianVsFiniteDifferences.

protected void checkStateJacobianVsFiniteDifferences(final SpacecraftState state0, final ForceModel forceModel, final AttitudeProvider provider, final double dP, final double checkTolerance, final boolean print) throws OrekitException {
    double[][] finiteDifferencesJacobian = Differentiation.differentiate(state -> forceModel.acceleration(state, forceModel.getParameters()).toArray(), 3, provider, OrbitType.CARTESIAN, PositionAngle.MEAN, dP, 5).value(state0);
    DSFactory factory = new DSFactory(6, 1);
    Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldAbsoluteDate<DerivativeStructure> fDate = new FieldAbsoluteDate<>(field, state0.getDate());
    final Vector3D p = state0.getPVCoordinates().getPosition();
    final Vector3D v = state0.getPVCoordinates().getVelocity();
    final Vector3D a = state0.getPVCoordinates().getAcceleration();
    final TimeStampedFieldPVCoordinates<DerivativeStructure> fPVA = new TimeStampedFieldPVCoordinates<>(fDate, new FieldVector3D<>(factory.variable(0, p.getX()), factory.variable(1, p.getY()), factory.variable(2, p.getZ())), new FieldVector3D<>(factory.variable(3, v.getX()), factory.variable(4, v.getY()), factory.variable(5, v.getZ())), new FieldVector3D<>(factory.constant(a.getX()), factory.constant(a.getY()), factory.constant(a.getZ())));
    final TimeStampedFieldAngularCoordinates<DerivativeStructure> fAC = new TimeStampedFieldAngularCoordinates<>(fDate, new FieldRotation<>(field, state0.getAttitude().getRotation()), new FieldVector3D<>(field, state0.getAttitude().getSpin()), new FieldVector3D<>(field, state0.getAttitude().getRotationAcceleration()));
    final FieldSpacecraftState<DerivativeStructure> fState = new FieldSpacecraftState<>(new FieldCartesianOrbit<>(fPVA, state0.getFrame(), state0.getMu()), new FieldAttitude<>(state0.getFrame(), fAC), field.getZero().add(state0.getMass()));
    FieldVector3D<DerivativeStructure> dsJacobian = forceModel.acceleration(fState, forceModel.getParameters(fState.getDate().getField()));
    Vector3D dFdPXRef = new Vector3D(finiteDifferencesJacobian[0][0], finiteDifferencesJacobian[1][0], finiteDifferencesJacobian[2][0]);
    Vector3D dFdPXRes = new Vector3D(dsJacobian.getX().getPartialDerivative(1, 0, 0, 0, 0, 0), dsJacobian.getY().getPartialDerivative(1, 0, 0, 0, 0, 0), dsJacobian.getZ().getPartialDerivative(1, 0, 0, 0, 0, 0));
    Vector3D dFdPYRef = new Vector3D(finiteDifferencesJacobian[0][1], finiteDifferencesJacobian[1][1], finiteDifferencesJacobian[2][1]);
    Vector3D dFdPYRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 1, 0, 0, 0, 0), dsJacobian.getY().getPartialDerivative(0, 1, 0, 0, 0, 0), dsJacobian.getZ().getPartialDerivative(0, 1, 0, 0, 0, 0));
    Vector3D dFdPZRef = new Vector3D(finiteDifferencesJacobian[0][2], finiteDifferencesJacobian[1][2], finiteDifferencesJacobian[2][2]);
    Vector3D dFdPZRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 1, 0, 0, 0), dsJacobian.getY().getPartialDerivative(0, 0, 1, 0, 0, 0), dsJacobian.getZ().getPartialDerivative(0, 0, 1, 0, 0, 0));
    Vector3D dFdVXRef = new Vector3D(finiteDifferencesJacobian[0][3], finiteDifferencesJacobian[1][3], finiteDifferencesJacobian[2][3]);
    Vector3D dFdVXRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 1, 0, 0), dsJacobian.getY().getPartialDerivative(0, 0, 0, 1, 0, 0), dsJacobian.getZ().getPartialDerivative(0, 0, 0, 1, 0, 0));
    Vector3D dFdVYRef = new Vector3D(finiteDifferencesJacobian[0][4], finiteDifferencesJacobian[1][4], finiteDifferencesJacobian[2][4]);
    Vector3D dFdVYRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 0, 1, 0), dsJacobian.getY().getPartialDerivative(0, 0, 0, 0, 1, 0), dsJacobian.getZ().getPartialDerivative(0, 0, 0, 0, 1, 0));
    Vector3D dFdVZRef = new Vector3D(finiteDifferencesJacobian[0][5], finiteDifferencesJacobian[1][5], finiteDifferencesJacobian[2][5]);
    Vector3D dFdVZRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 0, 0, 1), dsJacobian.getY().getPartialDerivative(0, 0, 0, 0, 0, 1), dsJacobian.getZ().getPartialDerivative(0, 0, 0, 0, 0, 1));
    if (print) {
        System.out.println("dF/dPX ref: " + dFdPXRef.getX() + " " + dFdPXRef.getY() + " " + dFdPXRef.getZ());
        System.out.println("dF/dPX res: " + dFdPXRes.getX() + " " + dFdPXRes.getY() + " " + dFdPXRes.getZ());
        System.out.println("dF/dPY ref: " + dFdPYRef.getX() + " " + dFdPYRef.getY() + " " + dFdPYRef.getZ());
        System.out.println("dF/dPY res: " + dFdPYRes.getX() + " " + dFdPYRes.getY() + " " + dFdPYRes.getZ());
        System.out.println("dF/dPZ ref: " + dFdPZRef.getX() + " " + dFdPZRef.getY() + " " + dFdPZRef.getZ());
        System.out.println("dF/dPZ res: " + dFdPZRes.getX() + " " + dFdPZRes.getY() + " " + dFdPZRes.getZ());
        System.out.println("dF/dPX ref norm: " + dFdPXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPXRef, dFdPXRes) + ", rel error: " + (Vector3D.distance(dFdPXRef, dFdPXRes) / dFdPXRef.getNorm()));
        System.out.println("dF/dPY ref norm: " + dFdPYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPYRef, dFdPYRes) + ", rel error: " + (Vector3D.distance(dFdPYRef, dFdPYRes) / dFdPYRef.getNorm()));
        System.out.println("dF/dPZ ref norm: " + dFdPZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPZRef, dFdPZRes) + ", rel error: " + (Vector3D.distance(dFdPZRef, dFdPZRes) / dFdPZRef.getNorm()));
        System.out.println("dF/dVX ref norm: " + dFdVXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVXRef, dFdVXRes) + ", rel error: " + (Vector3D.distance(dFdVXRef, dFdVXRes) / dFdVXRef.getNorm()));
        System.out.println("dF/dVY ref norm: " + dFdVYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVYRef, dFdVYRes) + ", rel error: " + (Vector3D.distance(dFdVYRef, dFdVYRes) / dFdVYRef.getNorm()));
        System.out.println("dF/dVZ ref norm: " + dFdVZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVZRef, dFdVZRes) + ", rel error: " + (Vector3D.distance(dFdVZRef, dFdVZRes) / dFdVZRef.getNorm()));
    }
    checkdFdP(dFdPXRef, dFdPXRes, checkTolerance);
    checkdFdP(dFdPYRef, dFdPYRes, checkTolerance);
    checkdFdP(dFdPZRef, dFdPZRes, checkTolerance);
    checkdFdP(dFdVXRef, dFdVXRes, checkTolerance);
    checkdFdP(dFdVYRef, dFdVYRes, checkTolerance);
    checkdFdP(dFdVZRef, dFdVZRes, checkTolerance);
}

Example 5 with FieldRotation

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

the class GroundStation method getOffsetToInertial.

/**
 * Get the transform between offset frame and inertial frame with derivatives.
 * <p>
 * As the East and North vector are not well defined at pole, the derivatives
 * of these two vectors diverge to infinity as we get closer to the pole.
 * So this method should not be used for stations less than 0.0001 degree from
 * either poles.
 * </p>
 * @param inertial inertial frame to transform to
 * @param date date of the transform
 * @param factory factory for the derivatives
 * @param indices indices of the estimated parameters in derivatives computations
 * @return offset frame defining vectors with derivatives
 * @exception OrekitException if some frame transforms cannot be computed
 * @since 9.0
 */
public FieldTransform<DerivativeStructure> getOffsetToInertial(final Frame inertial, final FieldAbsoluteDate<DerivativeStructure> date, final DSFactory factory, final Map<String, Integer> indices) throws OrekitException {
    final Field<DerivativeStructure> field = date.getField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    final FieldVector3D<DerivativeStructure> plusI = FieldVector3D.getPlusI(field);
    final FieldVector3D<DerivativeStructure> plusK = FieldVector3D.getPlusK(field);
    // take Earth offsets into account
    final FieldTransform<DerivativeStructure> intermediateToBody = estimatedEarthFrameProvider.getTransform(date, factory, indices).getInverse();
    // take station offset into account
    final DerivativeStructure x = parametricModel(factory, eastOffsetDriver, indices);
    final DerivativeStructure y = parametricModel(factory, northOffsetDriver, indices);
    final DerivativeStructure z = parametricModel(factory, zenithOffsetDriver, indices);
    final BodyShape baseShape = baseFrame.getParentShape();
    final Transform baseToBody = baseFrame.getTransformTo(baseShape.getBodyFrame(), (AbsoluteDate) null);
    FieldVector3D<DerivativeStructure> origin = baseToBody.transformPosition(new FieldVector3D<>(x, y, z));
    origin = origin.add(computeDisplacement(date.toAbsoluteDate(), origin.toVector3D()));
    final FieldGeodeticPoint<DerivativeStructure> originGP = baseShape.transform(origin, baseShape.getBodyFrame(), date);
    final FieldTransform<DerivativeStructure> offsetToIntermediate = new FieldTransform<>(date, new FieldTransform<>(date, new FieldRotation<>(plusI, plusK, originGP.getEast(), originGP.getZenith()), zero), new FieldTransform<>(date, origin));
    // combine all transforms together
    final FieldTransform<DerivativeStructure> bodyToInert = baseFrame.getParent().getTransformTo(inertial, date);
    return new FieldTransform<>(date, offsetToIntermediate, new FieldTransform<>(date, intermediateToBody, bodyToInert));
}