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

Example 6 with OrekitInternalError

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

the class TimeStampedPVCoordinates method interpolate.

/**
 * Interpolate position-velocity.
 * <p>
 * The interpolated instance is created by polynomial Hermite interpolation
 * ensuring velocity remains the exact derivative of position.
 * </p>
 * <p>
 * Note that even if first time derivatives (velocities)
 * from sample can be ignored, the interpolated instance always includes
 * interpolated derivatives. This feature can be used explicitly to
 * compute these derivatives when it would be too complex to compute them
 * from an analytical formula: just compute a few sample points from the
 * explicit formula and set the derivatives to zero in these sample points,
 * then use interpolation to add derivatives consistent with the positions.
 * </p>
 * @param date interpolation date
 * @param filter filter for derivatives from the sample to use in interpolation
 * @param sample sample points on which interpolation should be done
 * @return a new position-velocity, interpolated at specified date
 * @since 9.0
 */
public static TimeStampedPVCoordinates interpolate(final AbsoluteDate date, final CartesianDerivativesFilter filter, final Stream<TimeStampedPVCoordinates> sample) {
    // set up an interpolator taking derivatives into account
    final HermiteInterpolator interpolator = new HermiteInterpolator();
    // add sample points
    switch(filter) {
        case USE_P:
            // populate sample with position data, ignoring velocity
            sample.forEach(pv -> {
                final Vector3D position = pv.getPosition();
                interpolator.addSamplePoint(pv.getDate().durationFrom(date), position.toArray());
            });
            break;
        case USE_PV:
            // populate sample with position and velocity data
            sample.forEach(pv -> {
                final Vector3D position = pv.getPosition();
                final Vector3D velocity = pv.getVelocity();
                interpolator.addSamplePoint(pv.getDate().durationFrom(date), position.toArray(), velocity.toArray());
            });
            break;
        case USE_PVA:
            // populate sample with position, velocity and acceleration data
            sample.forEach(pv -> {
                final Vector3D position = pv.getPosition();
                final Vector3D velocity = pv.getVelocity();
                final Vector3D acceleration = pv.getAcceleration();
                interpolator.addSamplePoint(pv.getDate().durationFrom(date), position.toArray(), velocity.toArray(), acceleration.toArray());
            });
            break;
        default:
            // this should never happen
            throw new OrekitInternalError(null);
    }
    // interpolate
    final double[][] p = interpolator.derivatives(0.0, 2);
    // build a new interpolated instance
    return new TimeStampedPVCoordinates(date, new Vector3D(p[0]), new Vector3D(p[1]), new Vector3D(p[2]));
}

Example 7 with OrekitInternalError

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

the class TimeStampedAngularCoordinates method interpolate.

/**
 * Interpolate angular coordinates.
 * <p>
 * The interpolated instance is created by polynomial Hermite interpolation
 * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation.
 * </p>
 * <p>
 * This method is based on Sergei Tanygin's paper <a
 * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude
 * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
 * vector as described in Malcolm D. Shuster's paper <a
 * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
 * Survey of Attitude Representations</a>. This change avoids the singularity at π.
 * There is still a singularity at 2π, which is handled by slightly offsetting all rotations
 * when this singularity is detected. Another change is that the mean linear motion
 * is first removed before interpolation and added back after interpolation. This allows
 * to use interpolation even when the sample covers much more than one turn and even
 * when sample points are separated by more than one turn.
 * </p>
 * <p>
 * Note that even if first and second time derivatives (rotation rates and acceleration)
 * from sample can be ignored, the interpolated instance always includes
 * interpolated derivatives. This feature can be used explicitly to
 * compute these derivatives when it would be too complex to compute them
 * from an analytical formula: just compute a few sample points from the
 * explicit formula and set the derivatives to zero in these sample points,
 * then use interpolation to add derivatives consistent with the rotations.
 * </p>
 * @param date interpolation date
 * @param filter filter for derivatives from the sample to use in interpolation
 * @param sample sample points on which interpolation should be done
 * @return a new position-velocity, interpolated at specified date
 * @exception OrekitException if the number of point is too small for interpolating
 */
public static TimeStampedAngularCoordinates interpolate(final AbsoluteDate date, final AngularDerivativesFilter filter, final Collection<TimeStampedAngularCoordinates> sample) throws OrekitException {
    // set up safety elements for 2π singularity avoidance
    final double epsilon = 2 * FastMath.PI / sample.size();
    final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));
    // set up a linear model canceling mean rotation rate
    final Vector3D meanRate;
    if (filter != AngularDerivativesFilter.USE_R) {
        Vector3D sum = Vector3D.ZERO;
        for (final TimeStampedAngularCoordinates datedAC : sample) {
            sum = sum.add(datedAC.getRotationRate());
        }
        meanRate = new Vector3D(1.0 / sample.size(), sum);
    } else {
        if (sample.size() < 2) {
            throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, sample.size());
        }
        Vector3D sum = Vector3D.ZERO;
        TimeStampedAngularCoordinates previous = null;
        for (final TimeStampedAngularCoordinates datedAC : sample) {
            if (previous != null) {
                sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(), datedAC.date.durationFrom(previous.date)));
            }
            previous = datedAC;
        }
        meanRate = new Vector3D(1.0 / (sample.size() - 1), sum);
    }
    TimeStampedAngularCoordinates offset = new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.ZERO);
    boolean restart = true;
    for (int i = 0; restart && i < sample.size() + 2; ++i) {
        // offset adaptation parameters
        restart = false;
        // set up an interpolator taking derivatives into account
        final HermiteInterpolator interpolator = new HermiteInterpolator();
        // add sample points
        double sign = +1.0;
        Rotation previous = Rotation.IDENTITY;
        for (final TimeStampedAngularCoordinates ac : sample) {
            // remove linear offset from the current coordinates
            final double dt = ac.date.durationFrom(date);
            final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt));
            // make sure all interpolated points will be on the same branch
            final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(), fixed.getRotation().getQ1(), previous.getQ1(), fixed.getRotation().getQ2(), previous.getQ2(), fixed.getRotation().getQ3(), previous.getQ3());
            sign = FastMath.copySign(1.0, dot * sign);
            previous = fixed.getRotation();
            // check modified Rodrigues vector singularity
            if (fixed.getRotation().getQ0() * sign < threshold) {
                // the sample point is close to a modified Rodrigues vector singularity
                // we need to change the linear offset model to avoid this
                restart = true;
                break;
            }
            final double[][] rodrigues = fixed.getModifiedRodrigues(sign);
            switch(filter) {
                case USE_RRA:
                    // populate sample with rotation, rotation rate and acceleration data
                    interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
                    break;
                case USE_RR:
                    // populate sample with rotation and rotation rate data
                    interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
                    break;
                case USE_R:
                    // populate sample with rotation data only
                    interpolator.addSamplePoint(dt, rodrigues[0]);
                    break;
                default:
                    // this should never happen
                    throw new OrekitInternalError(null);
            }
        }
        if (restart) {
            // interpolation failed, some intermediate rotation was too close to 2π
            // we need to offset all rotations to avoid the singularity
            offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I, epsilon, RotationConvention.VECTOR_OPERATOR), Vector3D.ZERO, Vector3D.ZERO));
        } else {
            // interpolation succeeded with the current offset
            final double[][] p = interpolator.derivatives(0.0, 2);
            final AngularCoordinates ac = createFromModifiedRodrigues(p);
            return new TimeStampedAngularCoordinates(offset.getDate(), ac.getRotation(), ac.getRotationRate(), ac.getRotationAcceleration()).addOffset(offset);
        }
    }
    // this should never happen
    throw new OrekitInternalError(null);
}

Example 8 with OrekitInternalError

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

the class EOPHistory method interpolate.

/**
 * Interpolate a single EOP component.
 * <p>
 * This method should be called <em>only</em> when {@link #hasDataFor(AbsoluteDate)} returns true.
 * </p>
 * @param date interpolation date
 * @param selector selector for EOP entry component
 * @return interpolated value
 */
private double interpolate(final AbsoluteDate date, final Function<EOPEntry, Double> selector) {
    try {
        final HermiteInterpolator interpolator = new HermiteInterpolator();
        getNeighbors(date).forEach(entry -> interpolator.addSamplePoint(entry.getDate().durationFrom(date), new double[] { selector.apply(entry) }));
        return interpolator.value(0)[0];
    } catch (TimeStampedCacheException tce) {
        // this should not happen because of date check performed by caller
        throw new OrekitInternalError(tce);
    }
}

Example 9 with OrekitInternalError

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

the class MeasurementHandler method handleStep.

/**
 * {@inheritDoc}
 */
@Override
public void handleStep(final List<OrekitStepInterpolator> interpolators, final boolean isLast) throws OrekitException {
    while (number < precompensated.size()) {
        // Consider the next measurement to handle
        final PreCompensation next = precompensated.get(number);
        // Current state date for interpolator 0
        final AbsoluteDate currentDate = interpolators.get(0).getCurrentState().getDate();
        if ((model.isForwardPropagation() && (next.getDate().compareTo(currentDate) > 0)) || (!model.isForwardPropagation() && (next.getDate().compareTo(currentDate) < 0))) {
            // it will be picked-up in a future step
            if (isLast) {
                // this should never happen
                throw new OrekitInternalError(null);
            }
            return;
        }
        // get the observed measurement
        final ObservedMeasurement<?> observed = next.getMeasurement();
        // estimate the theoretical measurement
        final List<Integer> indices = observed.getPropagatorsIndices();
        final SpacecraftState[] states = new SpacecraftState[indices.size()];
        for (int i = 0; i < states.length; ++i) {
            states[i] = interpolators.get(i).getInterpolatedState(next.getDate());
        }
        final EstimatedMeasurement<?> estimated = observed.estimate(model.getIterationsCount(), model.getEvaluationsCount(), states);
        // fetch the evaluated measurement to the estimator
        model.fetchEvaluatedMeasurement(index, estimated);
        // prepare handling of next measurement
        ++number;
        index += observed.getDimension();
    }
}

Example 10 with OrekitInternalError

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

the class EcksteinHechlerPropagator method writeReplace.

/**
 * Replace the instance with a data transfer object for serialization.
 * @return data transfer object that will be serialized
 * @exception NotSerializableException if an additional state provider is not serializable
 */
private Object writeReplace() throws NotSerializableException {
    try {
        // managed states providers
        final List<AdditionalStateProvider> serializableProviders = new ArrayList<AdditionalStateProvider>();
        for (final AdditionalStateProvider provider : getAdditionalStateProviders()) {
            if (provider instanceof Serializable) {
                serializableProviders.add(provider);
            } else {
                throw new NotSerializableException(provider.getClass().getName());
            }
        }
        // states transitions
        final AbsoluteDate[] transitionDates;
        final CircularOrbit[] allOrbits;
        final double[] allMasses;
        final SortedSet<TimeSpanMap.Transition<EHModel>> transitions = models.getTransitions();
        if (transitions.size() == 1 && transitions.first().getBefore() == transitions.first().getAfter()) {
            // the single entry is a dummy one, without a real transition
            // we ignore it completely
            transitionDates = null;
            allOrbits = null;
            allMasses = null;
        } else {
            transitionDates = new AbsoluteDate[transitions.size()];
            allOrbits = new CircularOrbit[transitions.size() + 1];
            allMasses = new double[transitions.size() + 1];
            int i = 0;
            for (final TimeSpanMap.Transition<EHModel> transition : transitions) {
                if (i == 0) {
                    // model before the first transition
                    allOrbits[i] = transition.getBefore().mean;
                    allMasses[i] = transition.getBefore().mass;
                }
                transitionDates[i] = transition.getDate();
                allOrbits[++i] = transition.getAfter().mean;
                allMasses[i] = transition.getAfter().mass;
            }
        }
        return new DataTransferObject(getInitialState().getOrbit(), initialModel.mass, referenceRadius, mu, ck0, getAttitudeProvider(), transitionDates, allOrbits, allMasses, serializableProviders.toArray(new AdditionalStateProvider[serializableProviders.size()]));
    } catch (OrekitException orekitException) {
        // this should never happen
        throw new OrekitInternalError(null);
    }
}