Examples with PVCoordinates org.orekit.utils.PVCoordinates used on opensource projects

Example 26 with PVCoordinates

use of org.orekit.utils.PVCoordinates in project Orekit by CS-SI.

the class DSSTPropagatorTest method getLEOState.

private SpacecraftState getLEOState() throws IllegalArgumentException, OrekitException {
    final Vector3D position = new Vector3D(-6142438.668, 3492467.560, -25767.25680);
    final Vector3D velocity = new Vector3D(505.8479685, 942.7809215, 7435.922231);
    // Spring equinoxe 21st mars 2003 1h00m
    final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2003, 03, 21), new TimeComponents(1, 0, 0.), TimeScalesFactory.getUTC());
    return new SpacecraftState(new EquinoctialOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), initDate, 3.986004415E14));
}

Example 27 with PVCoordinates

use of org.orekit.utils.PVCoordinates in project Orekit by CS-SI.

the class OrbitDetermination method run.

private void run(final File input, final File home) throws IOException, IllegalArgumentException, OrekitException, ParseException {
    // read input parameters
    KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
    try (final FileInputStream fis = new FileInputStream(input)) {
        parser.parseInput(input.getAbsolutePath(), fis);
    }
    // log file
    final String baseName;
    final PrintStream logStream;
    if (parser.containsKey(ParameterKey.OUTPUT_BASE_NAME) && parser.getString(ParameterKey.OUTPUT_BASE_NAME).length() > 0) {
        baseName = parser.getString(ParameterKey.OUTPUT_BASE_NAME);
        logStream = new PrintStream(new File(home, baseName + "-log.out"), "UTF-8");
    } else {
        baseName = null;
        logStream = null;
    }
    final RangeLog rangeLog = new RangeLog(home, baseName);
    final RangeRateLog rangeRateLog = new RangeRateLog(home, baseName);
    final AzimuthLog azimuthLog = new AzimuthLog(home, baseName);
    final ElevationLog elevationLog = new ElevationLog(home, baseName);
    final PositionLog positionLog = new PositionLog(home, baseName);
    final VelocityLog velocityLog = new VelocityLog(home, baseName);
    try {
        // gravity field
        final NormalizedSphericalHarmonicsProvider gravityField = createGravityField(parser);
        // Orbit initial guess
        final Orbit initialGuess = createOrbit(parser, gravityField.getMu());
        // IERS conventions
        final IERSConventions conventions;
        if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
            conventions = IERSConventions.IERS_2010;
        } else {
            conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
        }
        // central body
        final OneAxisEllipsoid body = createBody(parser);
        // propagator builder
        final NumericalPropagatorBuilder propagatorBuilder = createPropagatorBuilder(parser, conventions, gravityField, body, initialGuess);
        // estimator
        final BatchLSEstimator estimator = createEstimator(parser, propagatorBuilder);
        // measurements
        final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
        for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES, ',')) {
            measurements.addAll(readMeasurements(new File(input.getParentFile(), fileName), createStationsData(parser, body), createPVData(parser), createSatRangeBias(parser), createWeights(parser), createRangeOutliersManager(parser), createRangeRateOutliersManager(parser), createAzElOutliersManager(parser), createPVOutliersManager(parser)));
        }
        for (ObservedMeasurement<?> measurement : measurements) {
            estimator.addMeasurement(measurement);
        }
        // estimate orbit
        estimator.setObserver(new BatchLSObserver() {

            private PVCoordinates previousPV;

            {
                previousPV = initialGuess.getPVCoordinates();
                final String header = "iteration evaluations      ΔP(m)        ΔV(m/s)           RMS          nb Range    nb Range-rate  nb Angular     nb PV%n";
                System.out.format(Locale.US, header);
                if (logStream != null) {
                    logStream.format(Locale.US, header);
                }
            }

            /**
             * {@inheritDoc}
             */
            @Override
            public void evaluationPerformed(final int iterationsCount, final int evaluationsCount, final Orbit[] orbits, final ParameterDriversList estimatedOrbitalParameters, final ParameterDriversList estimatedPropagatorParameters, final ParameterDriversList estimatedMeasurementsParameters, final EstimationsProvider evaluationsProvider, final LeastSquaresProblem.Evaluation lspEvaluation) {
                PVCoordinates currentPV = orbits[0].getPVCoordinates();
                final String format0 = "    %2d         %2d                                 %16.12f     %s       %s     %s     %s%n";
                final String format = "    %2d         %2d      %13.6f %12.9f %16.12f     %s       %s     %s     %s%n";
                final EvaluationCounter<Range> rangeCounter = new EvaluationCounter<Range>();
                final EvaluationCounter<RangeRate> rangeRateCounter = new EvaluationCounter<RangeRate>();
                final EvaluationCounter<AngularAzEl> angularCounter = new EvaluationCounter<AngularAzEl>();
                final EvaluationCounter<PV> pvCounter = new EvaluationCounter<PV>();
                for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
                    if (entry.getKey() instanceof Range) {
                        @SuppressWarnings("unchecked") EstimatedMeasurement<Range> evaluation = (EstimatedMeasurement<Range>) entry.getValue();
                        rangeCounter.add(evaluation);
                    } else if (entry.getKey() instanceof RangeRate) {
                        @SuppressWarnings("unchecked") EstimatedMeasurement<RangeRate> evaluation = (EstimatedMeasurement<RangeRate>) entry.getValue();
                        rangeRateCounter.add(evaluation);
                    } else if (entry.getKey() instanceof AngularAzEl) {
                        @SuppressWarnings("unchecked") EstimatedMeasurement<AngularAzEl> evaluation = (EstimatedMeasurement<AngularAzEl>) entry.getValue();
                        angularCounter.add(evaluation);
                    } else if (entry.getKey() instanceof PV) {
                        @SuppressWarnings("unchecked") EstimatedMeasurement<PV> evaluation = (EstimatedMeasurement<PV>) entry.getValue();
                        pvCounter.add(evaluation);
                    }
                }
                if (evaluationsCount == 1) {
                    System.out.format(Locale.US, format0, iterationsCount, evaluationsCount, lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
                    if (logStream != null) {
                        logStream.format(Locale.US, format0, iterationsCount, evaluationsCount, lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
                    }
                } else {
                    System.out.format(Locale.US, format, iterationsCount, evaluationsCount, Vector3D.distance(previousPV.getPosition(), currentPV.getPosition()), Vector3D.distance(previousPV.getVelocity(), currentPV.getVelocity()), lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
                    if (logStream != null) {
                        logStream.format(Locale.US, format, iterationsCount, evaluationsCount, Vector3D.distance(previousPV.getPosition(), currentPV.getPosition()), Vector3D.distance(previousPV.getVelocity(), currentPV.getVelocity()), lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
                    }
                }
                previousPV = currentPV;
            }
        });
        Orbit estimated = estimator.estimate()[0].getInitialState().getOrbit();
        // compute some statistics
        for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
            if (entry.getKey() instanceof Range) {
                @SuppressWarnings("unchecked") EstimatedMeasurement<Range> evaluation = (EstimatedMeasurement<Range>) entry.getValue();
                rangeLog.add(evaluation);
            } else if (entry.getKey() instanceof RangeRate) {
                @SuppressWarnings("unchecked") EstimatedMeasurement<RangeRate> evaluation = (EstimatedMeasurement<RangeRate>) entry.getValue();
                rangeRateLog.add(evaluation);
            } else if (entry.getKey() instanceof AngularAzEl) {
                @SuppressWarnings("unchecked") EstimatedMeasurement<AngularAzEl> evaluation = (EstimatedMeasurement<AngularAzEl>) entry.getValue();
                azimuthLog.add(evaluation);
                elevationLog.add(evaluation);
            } else if (entry.getKey() instanceof PV) {
                @SuppressWarnings("unchecked") EstimatedMeasurement<PV> evaluation = (EstimatedMeasurement<PV>) entry.getValue();
                positionLog.add(evaluation);
                velocityLog.add(evaluation);
            }
        }
        System.out.println("Estimated orbit: " + estimated);
        if (logStream != null) {
            logStream.println("Estimated orbit: " + estimated);
        }
        final ParameterDriversList orbitalParameters = estimator.getOrbitalParametersDrivers(true);
        final ParameterDriversList propagatorParameters = estimator.getPropagatorParametersDrivers(true);
        final ParameterDriversList measurementsParameters = estimator.getMeasurementsParametersDrivers(true);
        int length = 0;
        for (final ParameterDriver parameterDriver : orbitalParameters.getDrivers()) {
            length = FastMath.max(length, parameterDriver.getName().length());
        }
        for (final ParameterDriver parameterDriver : propagatorParameters.getDrivers()) {
            length = FastMath.max(length, parameterDriver.getName().length());
        }
        for (final ParameterDriver parameterDriver : measurementsParameters.getDrivers()) {
            length = FastMath.max(length, parameterDriver.getName().length());
        }
        displayParametersChanges(System.out, "Estimated orbital parameters changes: ", false, length, orbitalParameters);
        if (logStream != null) {
            displayParametersChanges(logStream, "Estimated orbital parameters changes: ", false, length, orbitalParameters);
        }
        displayParametersChanges(System.out, "Estimated propagator parameters changes: ", true, length, propagatorParameters);
        if (logStream != null) {
            displayParametersChanges(logStream, "Estimated propagator parameters changes: ", true, length, propagatorParameters);
        }
        displayParametersChanges(System.out, "Estimated measurements parameters changes: ", true, length, measurementsParameters);
        if (logStream != null) {
            displayParametersChanges(logStream, "Estimated measurements parameters changes: ", true, length, measurementsParameters);
        }
        System.out.println("Number of iterations: " + estimator.getIterationsCount());
        System.out.println("Number of evaluations: " + estimator.getEvaluationsCount());
        rangeLog.displaySummary(System.out);
        rangeRateLog.displaySummary(System.out);
        azimuthLog.displaySummary(System.out);
        elevationLog.displaySummary(System.out);
        positionLog.displaySummary(System.out);
        velocityLog.displaySummary(System.out);
        if (logStream != null) {
            logStream.println("Number of iterations: " + estimator.getIterationsCount());
            logStream.println("Number of evaluations: " + estimator.getEvaluationsCount());
            rangeLog.displaySummary(logStream);
            rangeRateLog.displaySummary(logStream);
            azimuthLog.displaySummary(logStream);
            elevationLog.displaySummary(logStream);
            positionLog.displaySummary(logStream);
            velocityLog.displaySummary(logStream);
        }
        rangeLog.displayResiduals();
        rangeRateLog.displayResiduals();
        azimuthLog.displayResiduals();
        elevationLog.displayResiduals();
        positionLog.displayResiduals();
        velocityLog.displayResiduals();
    } finally {
        if (logStream != null) {
            logStream.close();
        }
        rangeLog.close();
        rangeRateLog.close();
        azimuthLog.close();
        elevationLog.close();
        positionLog.close();
        velocityLog.close();
    }
}

Example 28 with PVCoordinates

use of org.orekit.utils.PVCoordinates in project Orekit by CS-SI.

the class CartesianOrbit method shiftPVElliptic.

/**
 * Compute shifted position and velocity in elliptic case.
 * @param dt time shift
 * @return shifted position and velocity
 */
private PVCoordinates shiftPVElliptic(final double dt) {
    // preliminary computation
    final Vector3D pvP = getPVCoordinates().getPosition();
    final Vector3D pvV = getPVCoordinates().getVelocity();
    final double r2 = pvP.getNormSq();
    final double r = FastMath.sqrt(r2);
    final double rV2OnMu = r * pvV.getNormSq() / getMu();
    final double a = getA();
    final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(getMu() * a);
    final double eCE = rV2OnMu - 1;
    final double e2 = eCE * eCE + eSE * eSE;
    // we can use any arbitrary reference 2D frame in the orbital plane
    // in order to simplify some equations below, we use the current position as the u axis
    final Vector3D u = pvP.normalize();
    final Vector3D v = Vector3D.crossProduct(getPVCoordinates().getMomentum(), u).normalize();
    // the following equations rely on the specific choice of u explained above,
    // some coefficients that vanish to 0 in this case have already been removed here
    final double ex = (eCE - e2) * a / r;
    final double ey = -FastMath.sqrt(1 - e2) * eSE * a / r;
    final double beta = 1 / (1 + FastMath.sqrt(1 - e2));
    final double thetaE0 = FastMath.atan2(ey + eSE * beta * ex, r / a + ex - eSE * beta * ey);
    final double thetaM0 = thetaE0 - ex * FastMath.sin(thetaE0) + ey * FastMath.cos(thetaE0);
    // compute in-plane shifted eccentric argument
    final double thetaM1 = thetaM0 + getKeplerianMeanMotion() * dt;
    final double thetaE1 = meanToEccentric(thetaM1, ex, ey);
    final double cTE = FastMath.cos(thetaE1);
    final double sTE = FastMath.sin(thetaE1);
    // compute shifted in-plane Cartesian coordinates
    final double exey = ex * ey;
    final double exCeyS = ex * cTE + ey * sTE;
    final double x = a * ((1 - beta * ey * ey) * cTE + beta * exey * sTE - ex);
    final double y = a * ((1 - beta * ex * ex) * sTE + beta * exey * cTE - ey);
    final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
    final double xDot = factor * (-sTE + beta * ey * exCeyS);
    final double yDot = factor * (cTE - beta * ex * exCeyS);
    final Vector3D shiftedP = new Vector3D(x, u, y, v);
    final Vector3D shiftedV = new Vector3D(xDot, u, yDot, v);
    if (hasNonKeplerianAcceleration) {
        // extract non-Keplerian part of the initial acceleration
        final Vector3D nonKeplerianAcceleration = new Vector3D(1, getPVCoordinates().getAcceleration(), getMu() / (r2 * r), pvP);
        // add the quadratic motion due to the non-Keplerian acceleration to the Keplerian motion
        final Vector3D fixedP = new Vector3D(1, shiftedP, 0.5 * dt * dt, nonKeplerianAcceleration);
        final double fixedR2 = fixedP.getNormSq();
        final double fixedR = FastMath.sqrt(fixedR2);
        final Vector3D fixedV = new Vector3D(1, shiftedV, dt, nonKeplerianAcceleration);
        final Vector3D fixedA = new Vector3D(-getMu() / (fixedR2 * fixedR), shiftedP, 1, nonKeplerianAcceleration);
        return new PVCoordinates(fixedP, fixedV, fixedA);
    } else {
        // so the shifted orbit is not considered to have derivatives
        return new PVCoordinates(shiftedP, shiftedV);
    }
}

Example 29 with PVCoordinates

use of org.orekit.utils.PVCoordinates in project Orekit by CS-SI.

the class CartesianOrbit method addKeplerContribution.

/**
 * {@inheritDoc}
 */
public void addKeplerContribution(final PositionAngle type, final double gm, final double[] pDot) {
    final PVCoordinates pv = getPVCoordinates();
    // position derivative is velocity
    final Vector3D velocity = pv.getVelocity();
    pDot[0] += velocity.getX();
    pDot[1] += velocity.getY();
    pDot[2] += velocity.getZ();
    // velocity derivative is Newtonian acceleration
    final Vector3D position = pv.getPosition();
    final double r2 = position.getNormSq();
    final double coeff = -gm / (r2 * FastMath.sqrt(r2));
    pDot[3] += coeff * position.getX();
    pDot[4] += coeff * position.getY();
    pDot[5] += coeff * position.getZ();
}

Example 30 with PVCoordinates

use of org.orekit.utils.PVCoordinates in project Orekit by CS-SI.

the class KeplerianOrbit method computePVWithoutA.

/**
 * Compute position and velocity but not acceleration.
 */
private void computePVWithoutA() {
    if (partialPV != null) {
        // already computed
        return;
    }
    // preliminary variables
    final double cosRaan = FastMath.cos(raan);
    final double sinRaan = FastMath.sin(raan);
    final double cosPa = FastMath.cos(pa);
    final double sinPa = FastMath.sin(pa);
    final double cosI = FastMath.cos(i);
    final double sinI = FastMath.sin(i);
    final double crcp = cosRaan * cosPa;
    final double crsp = cosRaan * sinPa;
    final double srcp = sinRaan * cosPa;
    final double srsp = sinRaan * sinPa;
    // reference axes defining the orbital plane
    final Vector3D p = new Vector3D(crcp - cosI * srsp, srcp + cosI * crsp, sinI * sinPa);
    final Vector3D q = new Vector3D(-crsp - cosI * srcp, -srsp + cosI * crcp, sinI * cosPa);
    if (a > 0) {
        // elliptical case
        // elliptic eccentric anomaly
        final double uME2 = (1 - e) * (1 + e);
        final double s1Me2 = FastMath.sqrt(uME2);
        final double E = getEccentricAnomaly();
        final double cosE = FastMath.cos(E);
        final double sinE = FastMath.sin(E);
        // coordinates of position and velocity in the orbital plane
        final double x = a * (cosE - e);
        final double y = a * sinE * s1Me2;
        final double factor = FastMath.sqrt(getMu() / a) / (1 - e * cosE);
        final double xDot = -sinE * factor;
        final double yDot = cosE * s1Me2 * factor;
        final Vector3D position = new Vector3D(x, p, y, q);
        final Vector3D velocity = new Vector3D(xDot, p, yDot, q);
        partialPV = new PVCoordinates(position, velocity);
    } else {
        // hyperbolic case
        // compute position and velocity factors
        final double sinV = FastMath.sin(v);
        final double cosV = FastMath.cos(v);
        final double f = a * (1 - e * e);
        final double posFactor = f / (1 + e * cosV);
        final double velFactor = FastMath.sqrt(getMu() / f);
        final double x = posFactor * cosV;
        final double y = posFactor * sinV;
        final double xDot = -velFactor * sinV;
        final double yDot = velFactor * (e + cosV);
        final Vector3D position = new Vector3D(x, p, y, q);
        final Vector3D velocity = new Vector3D(xDot, p, yDot, q);
        partialPV = new PVCoordinates(position, velocity);
    }
}