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

Example 46 with Vector3D

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

the class AbstractLegacyForceModelTest method checkStateJacobianVs80Implementation.

protected void checkStateJacobianVs80Implementation(final SpacecraftState state, final ForceModel forceModel, final AttitudeProvider attitudeProvider, final double checkTolerance, final boolean print) throws OrekitException {
    FieldSpacecraftState<DerivativeStructure> fState = toDS(state, attitudeProvider);
    FieldVector3D<DerivativeStructure> dsNew = forceModel.acceleration(fState, forceModel.getParameters(fState.getDate().getField()));
    FieldVector3D<DerivativeStructure> dsOld = accelerationDerivatives(forceModel, fState.getDate().toAbsoluteDate(), fState.getFrame(), fState.getPVCoordinates().getPosition(), fState.getPVCoordinates().getVelocity(), fState.getAttitude().getRotation(), fState.getMass());
    Vector3D dFdPXRef = new Vector3D(dsOld.getX().getPartialDerivative(1, 0, 0, 0, 0, 0), dsOld.getY().getPartialDerivative(1, 0, 0, 0, 0, 0), dsOld.getZ().getPartialDerivative(1, 0, 0, 0, 0, 0));
    Vector3D dFdPXRes = new Vector3D(dsNew.getX().getPartialDerivative(1, 0, 0, 0, 0, 0), dsNew.getY().getPartialDerivative(1, 0, 0, 0, 0, 0), dsNew.getZ().getPartialDerivative(1, 0, 0, 0, 0, 0));
    Vector3D dFdPYRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 1, 0, 0, 0, 0), dsOld.getY().getPartialDerivative(0, 1, 0, 0, 0, 0), dsOld.getZ().getPartialDerivative(0, 1, 0, 0, 0, 0));
    Vector3D dFdPYRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 1, 0, 0, 0, 0), dsNew.getY().getPartialDerivative(0, 1, 0, 0, 0, 0), dsNew.getZ().getPartialDerivative(0, 1, 0, 0, 0, 0));
    Vector3D dFdPZRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 0, 1, 0, 0, 0), dsOld.getY().getPartialDerivative(0, 0, 1, 0, 0, 0), dsOld.getZ().getPartialDerivative(0, 0, 1, 0, 0, 0));
    Vector3D dFdPZRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 0, 1, 0, 0, 0), dsNew.getY().getPartialDerivative(0, 0, 1, 0, 0, 0), dsNew.getZ().getPartialDerivative(0, 0, 1, 0, 0, 0));
    Vector3D dFdVXRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 0, 0, 1, 0, 0), dsOld.getY().getPartialDerivative(0, 0, 0, 1, 0, 0), dsOld.getZ().getPartialDerivative(0, 0, 0, 1, 0, 0));
    Vector3D dFdVXRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 0, 0, 1, 0, 0), dsNew.getY().getPartialDerivative(0, 0, 0, 1, 0, 0), dsNew.getZ().getPartialDerivative(0, 0, 0, 1, 0, 0));
    Vector3D dFdVYRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 0, 0, 0, 1, 0), dsOld.getY().getPartialDerivative(0, 0, 0, 0, 1, 0), dsOld.getZ().getPartialDerivative(0, 0, 0, 0, 1, 0));
    Vector3D dFdVYRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 0, 0, 0, 1, 0), dsNew.getY().getPartialDerivative(0, 0, 0, 0, 1, 0), dsNew.getZ().getPartialDerivative(0, 0, 0, 0, 1, 0));
    Vector3D dFdVZRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 0, 0, 0, 0, 1), dsOld.getY().getPartialDerivative(0, 0, 0, 0, 0, 1), dsOld.getZ().getPartialDerivative(0, 0, 0, 0, 0, 1));
    Vector3D dFdVZRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 0, 0, 0, 0, 1), dsNew.getY().getPartialDerivative(0, 0, 0, 0, 0, 1), dsNew.getZ().getPartialDerivative(0, 0, 0, 0, 0, 1));
    if (print) {
        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 47 with Vector3D

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

the class BoxAndSolarArraySpacecraftTest method testNormalSunAlignedField.

@Test
public void testNormalSunAlignedField() throws OrekitException {
    BoxAndSolarArraySpacecraft s = new BoxAndSolarArraySpacecraft(0, 0, 0, (date, frame) -> new TimeStampedPVCoordinates(date, new Vector3D(0, 1e6, 0), Vector3D.ZERO), 20.0, Vector3D.PLUS_J, 0.0, 1.0, 0.0);
    Field<Decimal64> field = Decimal64Field.getInstance();
    FieldVector3D<Decimal64> normal = s.getNormal(FieldAbsoluteDate.getJ2000Epoch(field), FramesFactory.getEME2000(), FieldVector3D.getZero(field), FieldRotation.getIdentity(field));
    Assert.assertEquals(0, FieldVector3D.dotProduct(normal, Vector3D.PLUS_J).getReal(), 1.0e-16);
}

Example 48 with Vector3D

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

the class BoxAndSolarArraySpacecraftTest method testBackwardIllumination.

@Test
public void testBackwardIllumination() throws OrekitException {
    SpacecraftState state = propagator.getInitialState();
    CelestialBody sun = CelestialBodyFactory.getSun();
    BoxAndSolarArraySpacecraft s = new BoxAndSolarArraySpacecraft(0, 0, 0, sun, 20.0, Vector3D.PLUS_J, 0.0, 1.0, 0.0);
    Vector3D n = s.getNormal(state.getDate(), state.getFrame(), state.getPVCoordinates().getPosition(), state.getAttitude().getRotation());
    FieldVector3D<DerivativeStructure> aPlus = s.radiationPressureAcceleration(state.getDate(), state.getFrame(), state.getPVCoordinates().getPosition(), state.getAttitude().getRotation(), state.getMass(), n, getRadiationParameters(s), RadiationSensitive.ABSORPTION_COEFFICIENT);
    FieldVector3D<DerivativeStructure> aMinus = s.radiationPressureAcceleration(state.getDate(), state.getFrame(), state.getPVCoordinates().getPosition(), state.getAttitude().getRotation(), state.getMass(), n.negate(), getRadiationParameters(s), RadiationSensitive.ABSORPTION_COEFFICIENT);
    Assert.assertEquals(0.0, aPlus.add(aMinus).getNorm().getReal(), Double.MIN_VALUE);
}

Example 49 with Vector3D

use of org.hipparchus.geometry.euclidean.threed.Vector3D 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 50 with Vector3D

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

the class EstimationTestUtils method checkKalmanFit.

/**
 * Checker for Kalman estimator validation
 * @param context context used for the test
 * @param kalman Kalman filter
 * @param measurements List of observed measurements to be processed by the Kalman
 * @param refOrbit Reference orbit at last measurement date
 * @param expectedDeltaPos Expected position difference between estimated orbit and reference orbit
 * @param posEps Tolerance on expected position difference
 * @param expectedDeltaVel Expected velocity difference between estimated orbit and reference orbit
 * @param velEps Tolerance on expected velocity difference
 * @param expectedSigmasPos Expected values for covariance matrix on position
 * @param sigmaPosEps Tolerance on expected covariance matrix on position
 * @param expectedSigmasVel Expected values for covariance matrix on velocity
 * @param sigmaVelEps Tolerance on expected covariance matrix on velocity
 * @throws OrekitException
 */
public static void checkKalmanFit(final Context context, final KalmanEstimator kalman, final List<ObservedMeasurement<?>> measurements, final Orbit refOrbit, final PositionAngle positionAngle, final double expectedDeltaPos, final double posEps, final double expectedDeltaVel, final double velEps, final double[] expectedSigmasPos, final double sigmaPosEps, final double[] expectedSigmasVel, final double sigmaVelEps) throws OrekitException {
    // Add the measurements to the Kalman filter
    NumericalPropagator estimated = kalman.processMeasurements(measurements);
    // Check the number of measurements processed by the filter
    Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
    // Get the last estimation
    final Orbit estimatedOrbit = estimated.getInitialState().getOrbit();
    final Vector3D estimatedPosition = estimatedOrbit.getPVCoordinates().getPosition();
    final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
    // Get the last covariance matrix estimation
    final RealMatrix estimatedP = kalman.getPhysicalEstimatedCovarianceMatrix();
    // Convert the orbital part to Cartesian formalism
    // Assuming all 6 orbital parameters are estimated by the filter
    final double[][] dCdY = new double[6][6];
    estimatedOrbit.getJacobianWrtParameters(positionAngle, dCdY);
    final RealMatrix Jacobian = MatrixUtils.createRealMatrix(dCdY);
    final RealMatrix estimatedCartesianP = Jacobian.multiply(estimatedP.getSubMatrix(0, 5, 0, 5)).multiply(Jacobian.transpose());
    // Get the final sigmas (ie.sqrt of the diagonal of the Cartesian orbital covariance matrix)
    final double[] sigmas = new double[6];
    for (int i = 0; i < 6; i++) {
        sigmas[i] = FastMath.sqrt(estimatedCartesianP.getEntry(i, i));
    }
    // // FIXME: debug
    // final double dPos = Vector3D.distance(refOrbit.getPVCoordinates().getPosition(), estimatedPosition);
    // final double dVel = Vector3D.distance(refOrbit.getPVCoordinates().getVelocity(), estimatedVelocity);
    // System.out.println("Nb Meas = " + kalman.getCurrentMeasurementNumber());
    // System.out.println("dPos    = " + dPos + " m");
    // System.out.println("dVel    = " + dVel + " m/s");
    // System.out.println("sigmas  = " + sigmas[0] + " "
    // + sigmas[1] + " "
    // + sigmas[2] + " "
    // + sigmas[3] + " "
    // + sigmas[4] + " "
    // + sigmas[5]);
    // //debug
    // Check the final orbit estimation & PV sigmas
    Assert.assertEquals(expectedDeltaPos, Vector3D.distance(refOrbit.getPVCoordinates().getPosition(), estimatedPosition), posEps);
    Assert.assertEquals(expectedDeltaVel, Vector3D.distance(refOrbit.getPVCoordinates().getVelocity(), estimatedVelocity), velEps);
    for (int i = 0; i < 3; i++) {
        Assert.assertEquals(expectedSigmasPos[i], sigmas[i], sigmaPosEps);
        Assert.assertEquals(expectedSigmasVel[i], sigmas[i + 3], sigmaVelEps);
    }
}