Examples with OrekitStepInterpolator org.orekit.propagation.sampling.OrekitStepInterpolator used on opensource projects

Example 6 with OrekitStepInterpolator

use of org.orekit.propagation.sampling.OrekitStepInterpolator in project Orekit by CS-SI.

the class RangeTest method genericTestParameterDerivatives.

void genericTestParameterDerivatives(final boolean isModifier, final boolean printResults, final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) throws OrekitException {
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 0.001);
    // Create perfect range measurements
    for (final GroundStation station : context.stations) {
        station.getEastOffsetDriver().setSelected(true);
        station.getNorthOffsetDriver().setSelected(true);
        station.getZenithOffsetDriver().setSelected(true);
    }
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context), 1.0, 3.0, 300.0);
    // List to store the results
    final List<Double> relErrorList = new ArrayList<Double>();
    // Set master mode
    // Use a lambda function to implement "handleStep" function
    propagator.setMasterMode((OrekitStepInterpolator interpolator, boolean isLast) -> {
        for (final ObservedMeasurement<?> measurement : measurements) {
            // Play test if the measurement date is between interpolator previous and current date
            if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) && (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
                // Add modifiers if test implies it
                final RangeTroposphericDelayModifier modifier = new RangeTroposphericDelayModifier(SaastamoinenModel.getStandardModel());
                if (isModifier) {
                    ((Range) measurement).addModifier(modifier);
                }
                // Parameter corresponding to station position offset
                final GroundStation stationParameter = ((Range) measurement).getStation();
                // We intentionally propagate to a date which is close to the
                // real spacecraft state but is *not* the accurate date, by
                // compensating only part of the downlink delay. This is done
                // in order to validate the partial derivatives with respect
                // to velocity. If we had chosen the proper state date, the
                // range would have depended only on the current position but
                // not on the current velocity.
                final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
                final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
                final SpacecraftState state = interpolator.getInterpolatedState(date);
                final ParameterDriver[] drivers = new ParameterDriver[] { stationParameter.getEastOffsetDriver(), stationParameter.getNorthOffsetDriver(), stationParameter.getZenithOffsetDriver() };
                if (printResults) {
                    String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
                    System.out.format(Locale.US, "%-15s  %-23s  %-23s  ", stationName, measurement.getDate(), date);
                }
                for (int i = 0; i < 3; ++i) {
                    final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
                    Assert.assertEquals(1, measurement.getDimension());
                    Assert.assertEquals(1, gradient.length);
                    // Compute a reference value using finite differences
                    final ParameterFunction dMkdP = Differentiation.differentiate(new ParameterFunction() {

                        /**
                         * {@inheritDoc}
                         */
                        @Override
                        public double value(final ParameterDriver parameterDriver) throws OrekitException {
                            return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
                        }
                    }, drivers[i], 3, 20.0);
                    final double ref = dMkdP.value(drivers[i]);
                    if (printResults) {
                        System.out.format(Locale.US, "%10.3e  %10.3e  ", gradient[0] - ref, FastMath.abs((gradient[0] - ref) / ref));
                    }
                    final double relError = FastMath.abs((ref - gradient[0]) / ref);
                    relErrorList.add(relError);
                // Assert.assertEquals(ref, gradient[0], 6.1e-5 * FastMath.abs(ref));
                }
                if (printResults) {
                    System.out.format(Locale.US, "%n");
                }
            }
        // End if measurement date between previous and current interpolator step
        }
    // End for loop on the measurements
    });
    // Rewind the propagator to initial date
    propagator.propagate(context.initialOrbit.getDate());
    // Sort measurements chronologically
    measurements.sort(new ChronologicalComparator());
    // Print results ? Header
    if (printResults) {
        System.out.format(Locale.US, "%-15s  %-23s  %-23s  " + "%10s  %10s  %10s  " + "%10s  %10s  %10s%n", "Station", "Measurement Date", "State Date", "ΔdQx", "rel ΔdQx", "ΔdQy", "rel ΔdQy", "ΔdQz", "rel ΔdQz");
    }
    // Propagate to final measurement's date
    propagator.propagate(measurements.get(measurements.size() - 1).getDate());
    // Convert error list to double[]
    final double[] relErrors = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
    // Compute statistics
    final double relErrorsMedian = new Median().evaluate(relErrors);
    final double relErrorsMean = new Mean().evaluate(relErrors);
    final double relErrorsMax = new Max().evaluate(relErrors);
    // Print the results on console ?
    if (printResults) {
        System.out.println();
        System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", relErrorsMedian, relErrorsMean, relErrorsMax);
    }
    Assert.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
    Assert.assertEquals(0.0, relErrorsMean, refErrorsMean);
    Assert.assertEquals(0.0, relErrorsMax, refErrorsMax);
}

Example 7 with OrekitStepInterpolator

use of org.orekit.propagation.sampling.OrekitStepInterpolator in project Orekit by CS-SI.

the class PropagatorsParallelizer method propagate.

/**
 * Propagate from a start date towards a target date.
 * @param start start date from which orbit state should be propagated
 * @param target target date to which orbit state should be propagated
 * @return propagated states
 * @exception OrekitException if state cannot be propagated
 */
public List<SpacecraftState> propagate(final AbsoluteDate start, final AbsoluteDate target) throws OrekitException {
    if (propagators.size() == 1) {
        // special handling when only one propagator is used
        propagators.get(0).setMasterMode(new SinglePropagatorHandler(globalHandler));
        return Collections.singletonList(propagators.get(0).propagate(start, target));
    }
    final double sign = FastMath.copySign(1.0, target.durationFrom(start));
    final int n = propagators.size();
    // set up queues for propagators synchronization
    // the main thread will let underlying propagators go forward
    // by consuming the step handling parameters they will put at each step
    final List<SynchronousQueue<SpacecraftState>> initQueues = new ArrayList<>(n);
    final List<SynchronousQueue<StepHandlingParameters>> shpQueues = new ArrayList<>(n);
    for (final Propagator propagator : propagators) {
        final SynchronousQueue<SpacecraftState> initQueue = new SynchronousQueue<>();
        initQueues.add(initQueue);
        final SynchronousQueue<StepHandlingParameters> shpQueue = new SynchronousQueue<>();
        shpQueues.add(shpQueue);
        propagator.setMasterMode(new MultiplePropagatorsHandler(initQueue, shpQueue));
    }
    // concurrently run all propagators
    final ExecutorService executorService = Executors.newFixedThreadPool(n);
    final List<Future<SpacecraftState>> futures = new ArrayList<>(n);
    final List<SpacecraftState> initialStates = new ArrayList<>(n);
    final List<StepHandlingParameters> stepHandlingParameters = new ArrayList<>(n);
    final List<OrekitStepInterpolator> restricted = new ArrayList<>(n);
    final List<SpacecraftState> finalStates = new ArrayList<>(n);
    for (int i = 0; i < n; ++i) {
        final Propagator propagator = propagators.get(i);
        final Future<SpacecraftState> future = executorService.submit(() -> propagator.propagate(start, target));
        futures.add(future);
        initialStates.add(getParameters(i, future, initQueues.get(i)));
        stepHandlingParameters.add(getParameters(i, future, shpQueues.get(i)));
        restricted.add(null);
        finalStates.add(null);
    }
    // main loop
    AbsoluteDate previousDate = start;
    globalHandler.init(initialStates, target);
    for (boolean isLast = false; !isLast; ) {
        // select the earliest ending propagator, according to propagation direction
        int selected = -1;
        AbsoluteDate selectedStepEnd = null;
        for (int i = 0; i < n; ++i) {
            final AbsoluteDate stepEnd = stepHandlingParameters.get(i).getDate();
            if (selected < 0 || sign * selectedStepEnd.durationFrom(stepEnd) > 0) {
                selected = i;
                selectedStepEnd = stepEnd;
            }
        }
        // restrict steps to a common time range
        for (int i = 0; i < n; ++i) {
            final OrekitStepInterpolator interpolator = stepHandlingParameters.get(i).interpolator;
            final SpacecraftState previousState = interpolator.getInterpolatedState(previousDate);
            final SpacecraftState currentState = interpolator.getInterpolatedState(selectedStepEnd);
            restricted.set(i, interpolator.restrictStep(previousState, currentState));
        }
        // will this be the last step?
        isLast = stepHandlingParameters.get(selected).isLast;
        // handle all states at once
        globalHandler.handleStep(restricted, isLast);
        if (!isLast) {
            // advance one step
            stepHandlingParameters.set(selected, getParameters(selected, futures.get(selected), shpQueues.get(selected)));
        }
        previousDate = selectedStepEnd;
    }
    // stop all remaining propagators
    executorService.shutdownNow();
    // extract the final states
    for (int i = 0; i < n; ++i) {
        try {
            finalStates.set(i, futures.get(i).get());
        } catch (InterruptedException | ExecutionException e) {
            // sort out if exception was intentional or not
            manageException(e);
            // this propagator was intentionally stopped,
            // we retrieve the final state from the last available interpolator
            finalStates.set(i, stepHandlingParameters.get(i).interpolator.getInterpolatedState(previousDate));
        }
    }
    return finalStates;
}

Example 8 with OrekitStepInterpolator

use of org.orekit.propagation.sampling.OrekitStepInterpolator in project Orekit by CS-SI.

the class AbstractAnalyticalPropagator method acceptStep.

/**
 * Accept a step, triggering events and step handlers.
 * @param interpolator interpolator for the current step
 * @param target final propagation time
 * @param epsilon threshold for end date detection
 * @return state at the end of the step
 * @exception OrekitException if the switching function cannot be evaluated
 * @exception MathRuntimeException if an event cannot be located
 */
protected SpacecraftState acceptStep(final OrekitStepInterpolator interpolator, final AbsoluteDate target, final double epsilon) throws OrekitException, MathRuntimeException {
    SpacecraftState previous = interpolator.getPreviousState();
    final SpacecraftState current = interpolator.getCurrentState();
    // initialize the events states if needed
    if (!statesInitialized) {
        if (!eventsStates.isEmpty()) {
            // initialize the events states
            for (final EventState<?> state : eventsStates) {
                state.reinitializeBegin(interpolator);
            }
        }
        statesInitialized = true;
    }
    // search for next events that may occur during the step
    final int orderingSign = interpolator.isForward() ? +1 : -1;
    final Queue<EventState<?>> occurringEvents = new PriorityQueue<>(new Comparator<EventState<?>>() {

        /**
         * {@inheritDoc}
         */
        @Override
        public int compare(final EventState<?> es0, final EventState<?> es1) {
            return orderingSign * es0.getEventDate().compareTo(es1.getEventDate());
        }
    });
    for (final EventState<?> state : eventsStates) {
        if (state.evaluateStep(interpolator)) {
            // the event occurs during the current step
            occurringEvents.add(state);
        }
    }
    OrekitStepInterpolator restricted = interpolator;
    do {
        eventLoop: while (!occurringEvents.isEmpty()) {
            // handle the chronologically first event
            final EventState<?> currentEvent = occurringEvents.poll();
            // get state at event time
            SpacecraftState eventState = restricted.getInterpolatedState(currentEvent.getEventDate());
            // try to advance all event states to current time
            for (final EventState<?> state : eventsStates) {
                if (state != currentEvent && state.tryAdvance(eventState, interpolator)) {
                    // we need to handle another event first
                    // remove event we just updated to prevent heap corruption
                    occurringEvents.remove(state);
                    // add it back to update its position in the heap
                    occurringEvents.add(state);
                    // re-queue the event we were processing
                    occurringEvents.add(currentEvent);
                    continue eventLoop;
                }
            }
            // all event detectors agree we can advance to the current event time
            final EventOccurrence occurrence = currentEvent.doEvent(eventState);
            final Action action = occurrence.getAction();
            isLastStep = action == Action.STOP;
            if (isLastStep) {
                // ensure the event is after the root if it is returned STOP
                // this lets the user integrate to a STOP event and then restart
                // integration from the same time.
                eventState = interpolator.getInterpolatedState(occurrence.getStopDate());
                restricted = restricted.restrictStep(previous, eventState);
            }
            // handle the first part of the step, up to the event
            if (getStepHandler() != null) {
                getStepHandler().handleStep(restricted, isLastStep);
            }
            if (isLastStep) {
                // the event asked to stop integration
                return eventState;
            }
            if (action == Action.RESET_DERIVATIVES || action == Action.RESET_STATE) {
                // some event handler has triggered changes that
                // invalidate the derivatives, we need to recompute them
                final SpacecraftState resetState = occurrence.getNewState();
                if (resetState != null) {
                    resetIntermediateState(resetState, interpolator.isForward());
                    return resetState;
                }
            }
            // at this point we know action == Action.CONTINUE
            // prepare handling of the remaining part of the step
            previous = eventState;
            restricted = new BasicStepInterpolator(restricted.isForward(), eventState, current);
            // check if the same event occurs again in the remaining part of the step
            if (currentEvent.evaluateStep(restricted)) {
                // the event occurs during the current step
                occurringEvents.add(currentEvent);
            }
        }
        // another event detector.
        for (final EventState<?> state : eventsStates) {
            if (state.tryAdvance(current, interpolator)) {
                occurringEvents.add(state);
            }
        }
    } while (!occurringEvents.isEmpty());
    isLastStep = target.equals(current.getDate());
    // handle the remaining part of the step, after all events if any
    if (getStepHandler() != null) {
        getStepHandler().handleStep(interpolator, isLastStep);
    }
    return current;
}

Example 9 with OrekitStepInterpolator

use of org.orekit.propagation.sampling.OrekitStepInterpolator in project Orekit by CS-SI.

the class EventState method findRoot.

/**
 * Find a root in a bracketing interval.
 *
 * <p> When calling this method one of the following must be true. Either ga == 0, gb
 * == 0, (ga < 0  and gb > 0), or (ga > 0 and gb < 0).
 *
 * @param interpolator that covers the interval.
 * @param ta           earliest possible time for root.
 * @param ga           g(ta).
 * @param tb           latest possible time for root.
 * @param gb           g(tb).
 * @return if a zero crossing was found.
 * @throws OrekitException if the event detector throws one
 */
private boolean findRoot(final OrekitStepInterpolator interpolator, final AbsoluteDate ta, final double ga, final AbsoluteDate tb, final double gb) throws OrekitException {
    // check there appears to be a root in [ta, tb]
    check(ga == 0.0 || gb == 0.0 || (ga > 0.0 && gb < 0.0) || (ga < 0.0 && gb > 0.0));
    final double convergence = detector.getThreshold();
    final int maxIterationCount = detector.getMaxIterationCount();
    final BracketedUnivariateSolver<UnivariateFunction> solver = new BracketingNthOrderBrentSolver(0, convergence, 0, 5);
    // event time, just at or before the actual root.
    AbsoluteDate beforeRootT = null;
    double beforeRootG = Double.NaN;
    // time on the other side of the root.
    // Initialized the the loop below executes once.
    AbsoluteDate afterRootT = ta;
    double afterRootG = 0.0;
    // the ga == 0.0 case is handled by the loop below
    if (ta.equals(tb)) {
        // both non-zero but times are the same. Probably due to reset state
        beforeRootT = ta;
        beforeRootG = ga;
        afterRootT = shiftedBy(beforeRootT, convergence);
        afterRootG = g(interpolator.getInterpolatedState(afterRootT));
    } else if (ga != 0.0 && gb == 0.0) {
        // hard: ga != 0.0 and gb == 0.0
        // look past gb by up to convergence to find next sign
        // throw an exception if g(t) = 0.0 in [tb, tb + convergence]
        beforeRootT = tb;
        beforeRootG = gb;
        afterRootT = shiftedBy(beforeRootT, convergence);
        afterRootG = g(interpolator.getInterpolatedState(afterRootT));
    } else if (ga != 0.0) {
        final double newGa = g(interpolator.getInterpolatedState(ta));
        if (ga > 0 != newGa > 0) {
            // both non-zero, step sign change at ta, possibly due to reset state
            beforeRootT = ta;
            beforeRootG = newGa;
            afterRootT = minTime(shiftedBy(beforeRootT, convergence), tb);
            afterRootG = g(interpolator.getInterpolatedState(afterRootT));
        }
    }
    // loop to skip through "fake" roots, i.e. where g(t) = g'(t) = 0.0
    // executed once if we didn't hit a special case above
    AbsoluteDate loopT = ta;
    double loopG = ga;
    while ((afterRootG == 0.0 || afterRootG > 0.0 == g0Positive) && strictlyAfter(afterRootT, tb)) {
        if (loopG == 0.0) {
            // ga == 0.0 and gb may or may not be 0.0
            // handle the root at ta first
            beforeRootT = loopT;
            beforeRootG = loopG;
            afterRootT = minTime(shiftedBy(beforeRootT, convergence), tb);
            afterRootG = g(interpolator.getInterpolatedState(afterRootT));
        } else {
            // both non-zero, the usual case, use a root finder.
            try {
                // time zero for evaluating the function f. Needs to be final
                final AbsoluteDate fT0 = loopT;
                final UnivariateFunction f = dt -> {
                    try {
                        return g(interpolator.getInterpolatedState(fT0.shiftedBy(dt)));
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                };
                // tb as a double for use in f
                final double tbDouble = tb.durationFrom(fT0);
                if (forward) {
                    final Interval interval = solver.solveInterval(maxIterationCount, f, 0, tbDouble);
                    beforeRootT = fT0.shiftedBy(interval.getLeftAbscissa());
                    beforeRootG = interval.getLeftValue();
                    afterRootT = fT0.shiftedBy(interval.getRightAbscissa());
                    afterRootG = interval.getRightValue();
                } else {
                    final Interval interval = solver.solveInterval(maxIterationCount, f, tbDouble, 0);
                    beforeRootT = fT0.shiftedBy(interval.getRightAbscissa());
                    beforeRootG = interval.getRightValue();
                    afterRootT = fT0.shiftedBy(interval.getLeftAbscissa());
                    afterRootG = interval.getLeftValue();
                }
            } catch (OrekitExceptionWrapper oew) {
                throw oew.getException();
            }
        }
        // assume tolerance is 1 ulp
        if (beforeRootT.equals(afterRootT)) {
            afterRootT = nextAfter(afterRootT);
            afterRootG = g(interpolator.getInterpolatedState(afterRootT));
        }
        // check loop is making some progress
        check((forward && afterRootT.compareTo(beforeRootT) > 0) || (!forward && afterRootT.compareTo(beforeRootT) < 0));
        // setup next iteration
        loopT = afterRootT;
        loopG = afterRootG;
    }
    // figure out the result of root finding, and return accordingly
    if (afterRootG == 0.0 || afterRootG > 0.0 == g0Positive) {
        // loop gave up and didn't find any crossing within this step
        return false;
    } else {
        // real crossing
        check(beforeRootT != null && !Double.isNaN(beforeRootG));
        // variation direction, with respect to the integration direction
        increasing = !g0Positive;
        pendingEventTime = beforeRootT;
        stopTime = beforeRootG == 0.0 ? beforeRootT : afterRootT;
        pendingEvent = true;
        afterEvent = afterRootT;
        afterG = afterRootG;
        // check increasing set correctly
        check(afterG > 0 == increasing);
        check(increasing == gb >= ga);
        return true;
    }
}

Example 10 with OrekitStepInterpolator

use of org.orekit.propagation.sampling.OrekitStepInterpolator in project Orekit by CS-SI.

the class AbstractForceModelTest method checkStateJacobian.

protected void checkStateJacobian(NumericalPropagator propagator, SpacecraftState state0, AbsoluteDate targetDate, double hFactor, double[] integratorAbsoluteTolerances, double checkTolerance) throws OrekitException {
    propagator.setInitialState(state0);
    double[][] reference = new double[][] { jacobianColumn(propagator, state0, targetDate, 0, hFactor * integratorAbsoluteTolerances[0]), jacobianColumn(propagator, state0, targetDate, 1, hFactor * integratorAbsoluteTolerances[1]), jacobianColumn(propagator, state0, targetDate, 2, hFactor * integratorAbsoluteTolerances[2]), jacobianColumn(propagator, state0, targetDate, 3, hFactor * integratorAbsoluteTolerances[3]), jacobianColumn(propagator, state0, targetDate, 4, hFactor * integratorAbsoluteTolerances[4]), jacobianColumn(propagator, state0, targetDate, 5, hFactor * integratorAbsoluteTolerances[5]) };
    for (int j = 0; j < 6; ++j) {
        for (int k = j + 1; k < 6; ++k) {
            double tmp = reference[j][k];
            reference[j][k] = reference[k][j];
            reference[k][j] = tmp;
        }
    }
    final String name = "pde";
    PartialDerivativesEquations pde = new PartialDerivativesEquations(name, propagator);
    propagator.setInitialState(pde.setInitialJacobians(state0));
    final JacobiansMapper mapper = pde.getMapper();
    final double[][] dYdY0 = new double[6][6];
    propagator.setMasterMode(new OrekitStepHandler() {

        public void handleStep(OrekitStepInterpolator interpolator, boolean isLast) throws OrekitException {
            if (isLast) {
                // pick up final Jacobian
                mapper.getStateJacobian(interpolator.getCurrentState(), dYdY0);
            }
        }
    });
    propagator.propagate(targetDate);
    for (int j = 0; j < 6; ++j) {
        for (int k = 0; k < 6; ++k) {
            double scale = integratorAbsoluteTolerances[j] / integratorAbsoluteTolerances[k];
            Assert.assertEquals(reference[j][k], dYdY0[j][k], checkTolerance * scale);
        }
    }
}