Examples with OrekitExceptionWrapper org.orekit.errors.OrekitExceptionWrapper used on opensource projects

Example 21 with OrekitExceptionWrapper

use of org.orekit.errors.OrekitExceptionWrapper in project Orekit by CS-SI.

the class RangeRateMeasurementCreator method handleStep.

public void handleStep(final SpacecraftState currentState, final boolean isLast) throws OrekitException {
    for (final GroundStation station : context.stations) {
        final AbsoluteDate date = currentState.getDate();
        final Frame inertial = currentState.getFrame();
        final Vector3D position = currentState.getPVCoordinates().getPosition();
        final Vector3D velocity = currentState.getPVCoordinates().getVelocity();
        if (station.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) {
            final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
            final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {

                public double value(final double x) throws OrekitExceptionWrapper {
                    try {
                        final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x));
                        final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
                        return d - x * Constants.SPEED_OF_LIGHT;
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            }, -1.0, 1.0);
            final AbsoluteDate receptionDate = currentState.getDate().shiftedBy(downLinkDelay);
            final PVCoordinates stationAtReception = station.getOffsetToInertial(inertial, receptionDate).transformPVCoordinates(PVCoordinates.ZERO);
            // line of sight at reception
            final Vector3D receptionLOS = (position.subtract(stationAtReception.getPosition())).normalize();
            // relative velocity, spacecraft-station, at the date of reception
            final Vector3D deltaVr = velocity.subtract(stationAtReception.getVelocity());
            final double upLinkDelay = solver.solve(1000, new UnivariateFunction() {

                public double value(final double x) throws OrekitExceptionWrapper {
                    try {
                        final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(-x));
                        final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
                        return d - x * Constants.SPEED_OF_LIGHT;
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            }, -1.0, 1.0);
            final AbsoluteDate emissionDate = currentState.getDate().shiftedBy(-upLinkDelay);
            final PVCoordinates stationAtEmission = station.getOffsetToInertial(inertial, emissionDate).transformPVCoordinates(PVCoordinates.ZERO);
            // line of sight at emission
            final Vector3D emissionLOS = (position.subtract(stationAtEmission.getPosition())).normalize();
            // relative velocity, spacecraft-station, at the date of emission
            final Vector3D deltaVe = velocity.subtract(stationAtEmission.getVelocity());
            // range rate at the date of reception
            final double rr = twoWay ? 0.5 * (deltaVr.dotProduct(receptionLOS) + deltaVe.dotProduct(emissionLOS)) : deltaVr.dotProduct(receptionLOS);
            addMeasurement(new RangeRate(station, date, rr, 1.0, 10, twoWay));
        }
    }
}

Example 22 with OrekitExceptionWrapper

use of org.orekit.errors.OrekitExceptionWrapper in project Orekit by CS-SI.

the class Model method getEvolution.

/**
 * {@inheritDoc}
 */
@Override
public NonLinearEvolution getEvolution(final double previousTime, final RealVector previousState, final MeasurementDecorator measurement) throws OrekitExceptionWrapper {
    try {
        // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
        final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
        for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
            if (driver.getReferenceDate() == null) {
                driver.setReferenceDate(builder.getInitialOrbitDate());
            }
        }
        ++currentMeasurementNumber;
        currentDate = measurement.getObservedMeasurement().getDate();
        // Note:
        // - N = size of the current measurement
        // Example:
        // * 1 for Range, RangeRate and TurnAroundRange
        // * 2 for Angular (Azimuth/Elevation or Right-ascension/Declination)
        // * 6 for Position/Velocity
        // - M = size of the state vector. N = nbOrb + nbPropag + nbMeas
        // Propagate the reference trajectory to measurement date
        predictedSpacecraftStates[0] = referenceTrajectory.propagate(observedMeasurement.getDate());
        // Predict the state vector (Mx1)
        final RealVector predictedState = predictState(predictedSpacecraftStates[0].getOrbit());
        // Get the error state transition matrix (MxM)
        final RealMatrix stateTransitionMatrix = getErrorStateTransitionMatrix(predictedSpacecraftStates[0]);
        // Predict the measurement based on predicted spacecraft state
        // Compute the innovations (i.e. residuals of the predicted measurement)
        // ------------------------------------------------------------
        // Predicted measurement
        // Note: here the "iteration/evaluation" formalism from the batch LS method
        // is twisted to fit the need of the Kalman filter.
        // The number of "iterations" is actually the number of measurements processed by the filter
        // so far. We use this to be able to apply the OutlierFilter modifiers on the predicted measurement.
        predictedMeasurement = observedMeasurement.estimate(currentMeasurementNumber, currentMeasurementNumber, predictedSpacecraftStates);
        // Normalized measurement matrix (NxM)
        final RealMatrix measurementMatrix = getMeasurementMatrix(predictedSpacecraftStates[0]);
        // compute process noise matrix
        final SpacecraftState previous = correctedSpacecraftStates[0];
        final RealMatrix physicalProcessNoise = processNoiseMatrixProvider.getProcessNoiseMatrix(previous, predictedSpacecraftStates[0]);
        final RealMatrix normalizedProcessNoise = normalizeCovarianceMatrix(physicalProcessNoise);
        return new NonLinearEvolution(measurement.getTime(), predictedState, stateTransitionMatrix, normalizedProcessNoise, measurementMatrix);
    } catch (OrekitException oe) {
        throw new OrekitExceptionWrapper(oe);
    }
}

Example 23 with OrekitExceptionWrapper

use of org.orekit.errors.OrekitExceptionWrapper in project Orekit by CS-SI.

the class KalmanEstimator method estimationStep.

/**
 * Process a single measurement.
 * <p>
 * Update the filter with the new measurement by calling the estimate method.
 * </p>
 * @param observedMeasurement the measurement to process
 * @return estimated propagator
 * @throws OrekitException if an error occurred during the estimation
 */
public NumericalPropagator estimationStep(final ObservedMeasurement<?> observedMeasurement) throws OrekitException {
    try {
        final ProcessEstimate estimate = filter.estimationStep(decorate(observedMeasurement));
        processModel.finalizeEstimation(observedMeasurement, estimate);
        if (observer != null) {
            observer.evaluationPerformed(processModel);
        }
        return processModel.getEstimatedPropagator();
    } catch (MathRuntimeException mrte) {
        throw new OrekitException(mrte);
    } catch (OrekitExceptionWrapper oew) {
        throw oew.getException();
    }
}

Example 24 with OrekitExceptionWrapper

use of org.orekit.errors.OrekitExceptionWrapper in project Orekit by CS-SI.

the class AdapterPropagator method basicPropagate.

/**
 * {@inheritDoc}
 */
@Override
protected SpacecraftState basicPropagate(final AbsoluteDate date) throws OrekitException {
    try {
        // compute reference state
        SpacecraftState state = reference.propagate(date);
        final Map<String, double[]> before = state.getAdditionalStates();
        // add all the effects
        for (final DifferentialEffect effect : effects) {
            state = effect.apply(state);
        }
        // forward additional states from the reference propagator
        for (final Map.Entry<String, double[]> entry : before.entrySet()) {
            if (!state.hasAdditionalState(entry.getKey())) {
                state = state.addAdditionalState(entry.getKey(), entry.getValue());
            }
        }
        return state;
    } catch (OrekitExceptionWrapper oew) {
        throw new OrekitException(oew.getException());
    }
}

Example 25 with OrekitExceptionWrapper

use of org.orekit.errors.OrekitExceptionWrapper in project Orekit by CS-SI.

the class FieldSpacecraftState method interpolate.

/**
 * Get an interpolated instance.
 * <p>
 * The additional states that are interpolated are the ones already present
 * in the instance. The sample instances must therefore have at least the same
 * additional states has the instance. They may have more additional states,
 * but the extra ones will be ignored.
 * </p>
 * <p>
 * As this implementation of interpolation is polynomial, it should be used only
 * with small samples (about 10-20 points) in order to avoid <a
 * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
 * and numerical problems (including NaN appearing).
 * </p>
 * @param date interpolation date
 * @param sample sample points on which interpolation should be done
 * @return a new instance, interpolated at specified date
 * @exception OrekitException if the number of point is too small for interpolating
 */
public FieldSpacecraftState<T> interpolate(final FieldAbsoluteDate<T> date, final Stream<FieldSpacecraftState<T>> sample) throws OrekitException {
    // prepare interpolators
    final List<FieldOrbit<T>> orbits = new ArrayList<>();
    final List<FieldAttitude<T>> attitudes = new ArrayList<>();
    final FieldHermiteInterpolator<T> massInterpolator = new FieldHermiteInterpolator<>();
    final Map<String, FieldHermiteInterpolator<T>> additionalInterpolators = new HashMap<String, FieldHermiteInterpolator<T>>(additional.size());
    for (final String name : additional.keySet()) {
        additionalInterpolators.put(name, new FieldHermiteInterpolator<>());
    }
    // extract sample data
    try {
        sample.forEach(state -> {
            try {
                final T deltaT = state.getDate().durationFrom(date);
                orbits.add(state.getOrbit());
                attitudes.add(state.getAttitude());
                final T[] mm = MathArrays.buildArray(orbit.getA().getField(), 1);
                mm[0] = state.getMass();
                massInterpolator.addSamplePoint(deltaT, mm);
                for (final Map.Entry<String, FieldHermiteInterpolator<T>> entry : additionalInterpolators.entrySet()) {
                    entry.getValue().addSamplePoint(deltaT, state.getAdditionalState(entry.getKey()));
                }
            } catch (OrekitException oe) {
                throw new OrekitExceptionWrapper(oe);
            }
        });
    } catch (OrekitExceptionWrapper oew) {
        throw oew.getException();
    }
    // perform interpolations
    final FieldOrbit<T> interpolatedOrbit = orbit.interpolate(date, orbits);
    final FieldAttitude<T> interpolatedAttitude = attitude.interpolate(date, attitudes);
    final T interpolatedMass = massInterpolator.value(orbit.getA().getField().getZero())[0];
    final Map<String, T[]> interpolatedAdditional;
    if (additional.isEmpty()) {
        interpolatedAdditional = null;
    } else {
        interpolatedAdditional = new HashMap<String, T[]>(additional.size());
        for (final Map.Entry<String, FieldHermiteInterpolator<T>> entry : additionalInterpolators.entrySet()) {
            interpolatedAdditional.put(entry.getKey(), entry.getValue().value(orbit.getA().getField().getZero()));
        }
    }
    // create the complete interpolated state
    return new FieldSpacecraftState<>(interpolatedOrbit, interpolatedAttitude, interpolatedMass, interpolatedAdditional);
}