From e0d759d86f693897000cb9d2a9fde1daeaa2be2a Mon Sep 17 00:00:00 2001 From: nfialton Date: Thu, 1 Apr 2021 14:39:39 +0200 Subject: [PATCH 01/26] Work in progress in field implementation of analytical propagators. --- .../propagation/FieldAbstractPropagator.java | 2 +- .../analytical/FieldAdapterPropagator.java | 168 ++++ .../analytical/FieldEphemeris.java | 418 +++++++++ .../FieldSmallManeuverAnalyticalModel.java | 387 ++++++++ .../gnss/AbstractGNSSPropagator.java | 1 + .../gnss/FieldAbstractGNSSPropagator.java | 388 ++++++++ .../analytical/gnss/FieldBeidouAlmanac.java | 315 +++++++ .../gnss/FieldBeidouOrbitalElements.java | 76 ++ .../gnss/FieldBeidouPropagator.java | 89 ++ .../FieldGLONASSAnalyticalPropagator.java | 860 ++++++++++++++++++ .../analytical/gnss/FieldGLONASSDate.java | 277 ++++++ .../gnss/FieldGLONASSOrbitalElements.java | 195 ++++ .../analytical/gnss/FieldGNSSDate.java | 435 +++++++++ .../gnss/FieldGNSSOrbitalElements.java | 183 ++++ .../analytical/gnss/FieldGPSAlmanac.java | 381 ++++++++ .../gnss/FieldGPSOrbitalElements.java | 67 ++ .../analytical/gnss/FieldGPSPropagator.java | 99 ++ .../analytical/gnss/FieldGalileoAlmanac.java | 367 ++++++++ .../gnss/FieldGalileoOrbitalElements.java | 57 ++ .../gnss/FieldGalileoPropagator.java | 105 +++ .../analytical/gnss/FieldIRNSSAlmanac.java | 267 ++++++ .../gnss/FieldIRNSSOrbitalElements.java | 32 + .../analytical/gnss/FieldIRNSSPropagator.java | 98 ++ .../analytical/gnss/FieldQZSSAlmanac.java | 343 +++++++ .../gnss/FieldQZSSOrbitalElements.java | 49 + .../analytical/gnss/FieldQZSSPropagator.java | 85 ++ .../gnss/FieldSBASOrbitalElements.java | 131 +++ .../analytical/gnss/FieldSBASPropagator.java | 257 ++++++ ...FieldSmallManeuverAnalyticalModelTest.java | 337 +++++++ .../FieldAdapterPropagatorTest.java | 338 +++++++ .../analytical/FieldEphemerisTest.java | 578 ++++++++++++ .../analytical/FieldJ2DifferentialEffect.java | 239 +++++ .../analytical/gnss/BeidouPropagatorTest.java | 559 ++++++------ .../gnss/FieldBeidouPropagatorTest.java | 436 +++++++++ .../gnss/FieldGPSPropagatorTest.java | 390 ++++++++ .../gnss/FieldGalileoPropagatorTest.java | 442 +++++++++ .../gnss/FieldIRNSSPropagatorTest.java | 181 ++++ .../gnss/FieldQZSSPropagatorTest.java | 389 ++++++++ .../gnss/FieldSBASPropagatorTest.java | 481 ++++++++++ .../analytical/gnss/GPSPropagatorTest.java | 812 ++++++++--------- 40 files changed, 10608 insertions(+), 706 deletions(-) create mode 100644 src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java create mode 100644 src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java create mode 100644 src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouOrbitalElements.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSOrbitalElements.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSDate.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSAlmanac.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSOrbitalElements.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagator.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASOrbitalElements.java create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagator.java create mode 100644 src/test/java/org/orekit/forces/maneuvers/FieldSmallManeuverAnalyticalModelTest.java create mode 100644 src/test/java/org/orekit/propagation/analytical/FieldAdapterPropagatorTest.java create mode 100644 src/test/java/org/orekit/propagation/analytical/FieldEphemerisTest.java create mode 100644 src/test/java/org/orekit/propagation/analytical/FieldJ2DifferentialEffect.java create mode 100644 src/test/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagatorTest.java create mode 100644 src/test/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagatorTest.java create mode 100644 src/test/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagatorTest.java create mode 100644 src/test/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagatorTest.java create mode 100644 src/test/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagatorTest.java create mode 100644 src/test/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagatorTest.java diff --git a/src/main/java/org/orekit/propagation/FieldAbstractPropagator.java b/src/main/java/org/orekit/propagation/FieldAbstractPropagator.java index e185a6eea..35df859b2 100644 --- a/src/main/java/org/orekit/propagation/FieldAbstractPropagator.java +++ b/src/main/java/org/orekit/propagation/FieldAbstractPropagator.java @@ -70,7 +70,7 @@ public abstract class FieldAbstractPropagator> imp private final Map> unmanagedStates; /** Field used.*/ - private final Field field; + public final Field field; /** Initial state. */ private FieldSpacecraftState initialState; diff --git a/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java b/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java new file mode 100644 index 000000000..750626d5f --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java @@ -0,0 +1,168 @@ +package org.orekit.propagation.analytical; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import java.util.Map; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.orbits.FieldOrbit; +import org.orekit.propagation.FieldPropagator; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.Propagator; +import org.orekit.propagation.SpacecraftState; +import org.orekit.propagation.analytical.AdapterPropagator.DifferentialEffect; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.utils.ParameterDriver; + +/** + * Orbit propagator that adapts an underlying propagator, adding + * {@link DifferentialEffect differential effects}. + *

+ * This propagator is used when a reference propagator does not handle some + * effects that we need. A typical example would be an ephemeris that was + * computed for a reference orbit, and we want to compute a station-keeping + * maneuver on top of this ephemeris, changing its final state. The principal is + * to add one or more + * {@link org.orekit.forces.maneuvers.SmallManeuverAnalyticalModel small + * maneuvers analytical models} to it and use it as a new propagator, which + * takes the maneuvers into account. + *

+ *

+ * From a space flight dynamics point of view, this is a differential correction + * approach. From a computer science point of view, this is a use of the + * decorator design pattern. + *

+ * + * @see Propagator + * @see org.orekit.forces.maneuvers.SmallManeuverAnalyticalModel + * @author Luc Maisonobe + * @author Nicolas Fialton (field translation) + */ +public class FieldAdapterPropagator> extends FieldAbstractAnalyticalPropagator { + /** Interface for orbit differential effects. */ + public interface FieldDifferentialEffect> { + + /** Apply the effect to a {@link SpacecraftState spacecraft state}. + *

+ * Applying the effect may be a no-op in some cases. A typical example + * is maneuvers, for which the state is changed only for time after + * the maneuver occurrence. + *

+ * @param original original state without the effect + * @return updated state at the same date, taking the effect + * into account if meaningful + */ + FieldSpacecraftState apply(FieldSpacecraftState original); + + } + + /** Underlying reference propagator. */ + private FieldPropagator reference; + + /** Effects to add. */ + private List> effects; + + /** Build a propagator from an underlying reference propagator. + *

The reference propagator can be almost anything, numerical, + * analytical, and even an ephemeris. It may already take some maneuvers + * into account.

+ * @param reference reference propagator + */ + public FieldAdapterPropagator(Field field, final FieldPropagator reference) { + super(field, reference.getAttitudeProvider()); + this.reference = reference; + this.effects = new ArrayList>(); + } + + /** Add a differential effect. + * @param effect differential effect + */ + public void addEffect(final FieldDifferentialEffect effect) { + effects.add(effect); + } + + /** Get the reference propagator. + * @return reference propagator + */ + public FieldPropagator getPropagator() { + return reference; + } + + /** Get the differential effects. + * @return differential effects models, as an unmodifiable list + */ + public List> getEffects() { + return Collections.unmodifiableList(effects); + } + + /** {@inheritDoc} */ + public FieldSpacecraftState getInitialState() { + return reference.getInitialState(); + } + + /** {@inheritDoc} */ + @Override + public void resetInitialState(final FieldSpacecraftState state) { + reference.resetInitialState(state); + } + + /** {@inheritDoc} */ + protected void resetIntermediateState(final FieldSpacecraftState state, final boolean forward) { + if (reference instanceof FieldAbstractAnalyticalPropagator) { + ((FieldAbstractAnalyticalPropagator) reference).resetIntermediateState(state, forward); + } else { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + } + + /** {@inheritDoc} */ + @Override + protected FieldSpacecraftState basicPropagate(final FieldAbsoluteDate date) { + + // compute reference state + FieldSpacecraftState state = reference.propagate(date); + final Map before = state.getAdditionalStates(); + + // add all the effects + for (final FieldDifferentialEffect effect : effects) { + state = effect.apply(state); + } + + // forward additional states from the reference propagator + for (final Map.Entry entry : before.entrySet()) { + if (!state.hasAdditionalState(entry.getKey())) { + state = state.addAdditionalState(entry.getKey(), entry.getValue()); + } + } + + return state; + + } + + /** {@inheritDoc} */ + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date) { + return basicPropagate(date).getOrbit(); + } + + /** {@inheritDoc}*/ + protected T getMass(final FieldAbsoluteDate date) { + return basicPropagate(date).getMass(); + } + + @Override + protected FieldOrbit propagateOrbit(FieldAbsoluteDate date, T[] parameters) { + // TODO Auto-generated method stub + return null; + } + + @Override + protected List getParametersDrivers() { + // TODO Auto-generated method stub + return null; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java b/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java new file mode 100644 index 000000000..e653a6f46 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java @@ -0,0 +1,418 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical; + +import java.io.Serializable; +import java.util.Collections; +import java.util.List; +import java.util.Set; +import java.util.stream.Collectors; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.exception.LocalizedCoreFormats; +import org.hipparchus.exception.MathIllegalArgumentException; +import org.hipparchus.util.FastMath; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.attitudes.FieldAttitude; +import org.orekit.data.DataContext; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.orbits.FieldOrbit; +import org.orekit.propagation.FieldBoundedPropagator; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.Propagator; +import org.orekit.propagation.SpacecraftState; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.utils.FieldPVCoordinatesProvider; +import org.orekit.utils.ImmutableTimeStampedCache; +import org.orekit.utils.ParameterDriver; +import org.orekit.utils.TimeStampedFieldPVCoordinates; + +/** + * This class is designed to accept and handle tabulated orbital entries. + * Tabulated entries are classified and then extrapolated in way to obtain + * continuous output, with accuracy and computation methods configured by the + * user. + * + * @author Fabien Maussion + * @author Véronique Pommier-Maurussane + * @author Luc Maisonobe + * @author Nicolas Fialton (field translation) + */ + +public class FieldEphemeris> extends FieldAbstractAnalyticalPropagator + implements FieldBoundedPropagator { + + /** + * Default extrapolation time threshold: 1ms. + * + * @since 9.0 + **/ + public static final double DEFAULT_EXTRAPOLATION_THRESHOLD_SEC = 1e-3; + + /** First date in range. */ + private final FieldAbsoluteDate minDate; + + /** Last date in range. */ + private final FieldAbsoluteDate maxDate; + + /** The extrapolation threshold beyond which the propagation will fail. **/ + private final T extrapolationThreshold; + + /** Reference frame. */ + private final Frame frame; + + /** Names of the additional states. */ + private final String[] additional; + + /** Local PV Provider used for computing attitude. **/ + private LocalPVProvider pvProvider; + + /** Thread-safe cache. */ + private final transient ImmutableTimeStampedCache cache; + + /** + * Constructor with tabulated states. + *

+ * This constructor allows extrapolating outside of the states time span by up + * to the 1ms {@link #DEFAULT_EXTRAPOLATION_THRESHOLD_SEC default extrapolation + * threshold}. + *

+ * + *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. + * + * @param states tabulates states + * @param interpolationPoints number of points to use in interpolation + * @exception MathIllegalArgumentException if the number of states is smaller + * than the number of points to use in + * interpolation + * @see #Ephemeris(List, int, double) + * @see #Ephemeris(List, int, double, AttitudeProvider) + */ + @DefaultDataContext + public FieldEphemeris(Field field, final List states, final int interpolationPoints) + throws MathIllegalArgumentException { + this(field, states, interpolationPoints, field.getZero().add(DEFAULT_EXTRAPOLATION_THRESHOLD_SEC)); + } + + /** + * Constructor with tabulated states. + * + *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. + * + * @param states tabulates states + * @param interpolationPoints number of points to use in interpolation + * @param extrapolationThreshold the largest time difference in seconds between + * the start or stop boundary of the ephemeris + * bounds to be doing extrapolation + * @exception MathIllegalArgumentException if the number of states is smaller + * than the number of points to use in + * interpolation + * @since 9.0 + * @see #Ephemeris(List, int, double, AttitudeProvider) + */ + @DefaultDataContext + public FieldEphemeris(Field field, final List states, final int interpolationPoints, + final T extrapolationThreshold) throws MathIllegalArgumentException { + this(field, states, interpolationPoints, extrapolationThreshold, + Propagator.getDefaultLaw(DataContext.getDefault().getFrames())); + } + + /** + * Constructor with tabulated states. + * + * @param states tabulates states + * @param interpolationPoints number of points to use in interpolation + * @param extrapolationThreshold the largest time difference in seconds between + * the start or stop boundary of the ephemeris + * bounds to be doing extrapolation + * @param attitudeProvider attitude law to use. + * @exception MathIllegalArgumentException if the number of states is smaller + * than the number of points to use in + * interpolation + * @since 10.1 + */ + public FieldEphemeris(Field field, final List states, final int interpolationPoints, + final T extrapolationThreshold, final AttitudeProvider attitudeProvider) + throws MathIllegalArgumentException { + + super(field, attitudeProvider); + + if (states.size() < interpolationPoints) { + throw new MathIllegalArgumentException(LocalizedCoreFormats.INSUFFICIENT_DIMENSION, states.size(), + interpolationPoints); + } + + final SpacecraftState s0 = states.get(0); + minDate = new FieldAbsoluteDate<>(field, s0.getDate()); + maxDate = new FieldAbsoluteDate<>(field, states.get(states.size() - 1).getDate()); + frame = s0.getFrame(); + + final Set names0 = s0.getAdditionalStates().keySet(); + additional = names0.toArray(new String[names0.size()]); + + // check all states handle the same additional states + for (final SpacecraftState state : states) { + s0.ensureCompatibleAdditionalStates(state); + } + + pvProvider = new LocalPVProvider<>(states, interpolationPoints, extrapolationThreshold); + + // user needs to explicitly set attitude provider if they want to use one + setAttitudeProvider(null); + + // set up cache + cache = new ImmutableTimeStampedCache(interpolationPoints, states); + + this.extrapolationThreshold = extrapolationThreshold; + } + + /** + * Get the first date of the range. + * + * @return the first date of the range + */ + public FieldAbsoluteDate getMinDate() { + return minDate; + } + + /** + * Get the last date of the range. + * + * @return the last date of the range + */ + public FieldAbsoluteDate getMaxDate() { + return maxDate; + } + + /** + * Get the maximum timespan outside of the stored ephemeris that is allowed for + * extrapolation. + * + * @return the extrapolation threshold in seconds + */ + public T getExtrapolationThreshold() { + return extrapolationThreshold; + } + + /** {@inheritDoc} */ + @Override + public Frame getFrame() { + return frame; + } + + /** {@inheritDoc} */ + @Override + public FieldSpacecraftState basicPropagate(final FieldAbsoluteDate date) { + final FieldSpacecraftState evaluatedState; + + final FieldAbsoluteDate central; + if (date.compareTo(minDate) < 0 + && FastMath.abs(date.durationFrom(minDate)).getReal() <= extrapolationThreshold.getReal()) { + // avoid TimeStampedCacheException as we are still within the tolerance before + // minDate + central = minDate; + } else if (date.compareTo(maxDate) > 0 + && FastMath.abs(date.durationFrom(maxDate)).getReal() <= extrapolationThreshold.getReal()) { + // avoid TimeStampedCacheException as we are still within the tolerance after + // maxDate + central = maxDate; + } else { + central = date; + } + final List> neighbors = cache.getNeighbors(central.toAbsoluteDate()) + .map(state -> new FieldSpacecraftState<>(date.getField(), state)) + .collect(Collectors.toList()); + evaluatedState = neighbors.get(0).interpolate(date, neighbors); + + final AttitudeProvider attitudeProvider = getAttitudeProvider(); + + if (attitudeProvider == null) { + return evaluatedState; + } else { + pvProvider.setCurrentState(evaluatedState); + final FieldAttitude calculatedAttitude = attitudeProvider.getAttitude(pvProvider, date, + evaluatedState.getFrame()); + + // Verify if orbit is defined + if (evaluatedState.isOrbitDefined()) { + return new FieldSpacecraftState(evaluatedState.getOrbit(), calculatedAttitude, + evaluatedState.getMass(), evaluatedState.getAdditionalStates()); + } else { + return new FieldSpacecraftState(evaluatedState.getAbsPVA(), calculatedAttitude, + evaluatedState.getMass(), evaluatedState.getAdditionalStates()); + } + + } + } + + /** {@inheritDoc} */ + @Override + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, final T[] parameters) { + return basicPropagate(date).getOrbit(); + } + + /** {@inheritDoc} */ + @Override + protected T getMass(final FieldAbsoluteDate date) { + return basicPropagate(date).getMass(); + } + + /** {@inheritDoc} */ + @Override + public TimeStampedFieldPVCoordinates getPVCoordinates(final FieldAbsoluteDate date, final Frame f) { + return propagate(date).getPVCoordinates(f); + } + + /** {@inheritDoc} */ + @Override + protected List getParametersDrivers() { + return Collections.emptyList(); + } + + /** + * Try (and fail) to reset the initial state. + *

+ * This method always throws an exception, as ephemerides cannot be reset. + *

+ * + * @param state new initial state to consider + */ + public void resetInitialState(final FieldSpacecraftState state) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected void resetIntermediateState(final FieldSpacecraftState state, final boolean forward) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + public FieldSpacecraftState getInitialState() { + return basicPropagate(getMinDate()); + } + + /** {@inheritDoc} */ + @Override + public boolean isAdditionalStateManaged(final String name) { + + // the additional state may be managed by a specific provider in the base class + if (super.isAdditionalStateManaged(name)) { + return true; + } + + // the additional state may be managed in the states sample + for (final String a : additional) { + if (a.equals(name)) { + return true; + } + } + + return false; + + } + + /** {@inheritDoc} */ + @Override + public String[] getManagedAdditionalStates() { + final String[] upperManaged = super.getManagedAdditionalStates(); + final String[] managed = new String[upperManaged.length + additional.length]; + System.arraycopy(upperManaged, 0, managed, 0, upperManaged.length); + System.arraycopy(additional, 0, managed, upperManaged.length, additional.length); + return managed; + } + + /** Internal PVCoordinatesProvider for attitude computation. */ + private static class LocalPVProvider> + implements FieldPVCoordinatesProvider, Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 20160115L; + + /** Current state. */ + private FieldSpacecraftState currentState; + + /** List of spacecraft states. */ + private List states; + + /** Interpolation points number. */ + private int interpolationPoints; + + /** Extrapolation threshold. */ + private T extrapolationThreshold; + + /** + * Constructor. + * + * @param states list of spacecraft states + * @param interpolationPoints interpolation points number + * @param extrapolationThreshold extrapolation threshold value + */ + LocalPVProvider(final List states, final int interpolationPoints, + final T extrapolationThreshold) { + + this.states = states; + this.interpolationPoints = interpolationPoints; + this.extrapolationThreshold = extrapolationThreshold; + } + + /** + * Get the current state. + * + * @return current state + */ + public FieldSpacecraftState getCurrentState() { + return currentState; + } + + /** + * Set the current state. + * + * @param state state to set + */ + public void setCurrentState(final FieldSpacecraftState state) { + this.currentState = state; + } + + /** {@inheritDoc} */ + public TimeStampedFieldPVCoordinates getPVCoordinates(final FieldAbsoluteDate date, final Frame f) { + final T dt = getCurrentState().getDate().durationFrom(date); + final double closeEnoughTimeInSec = 1e-9; + + if (FastMath.abs(dt).getReal() > closeEnoughTimeInSec) { + + // used in case of attitude transition, the attitude computed is not at the + // current date. + final FieldEphemeris ephemeris = new FieldEphemeris<>(date.getField(), states, interpolationPoints, + extrapolationThreshold, null); + return ephemeris.getPVCoordinates(date, f); + } + + return currentState.getPVCoordinates(f); + + } + + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java b/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java new file mode 100644 index 000000000..7986f0f3e --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java @@ -0,0 +1,387 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical; + +import java.util.Arrays; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.util.FastMath; +import org.hipparchus.util.MathArrays; +import org.orekit.frames.Frame; +import org.orekit.orbits.FieldOrbit; +import org.orekit.orbits.Orbit; +import org.orekit.orbits.OrbitType; +import org.orekit.orbits.PositionAngle; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.SpacecraftState; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.utils.Constants; + +/** + * Analytical model for small maneuvers. + *

+ * The aim of this model is to compute quickly the effect at date t₁ of a small + * maneuver performed at an earlier date t₀. Both the direct effect of the + * maneuver and the Jacobian of this effect with respect to maneuver parameters + * are available. + *

+ *

+ * These effect are computed analytically using two Jacobian matrices: + *

    + *
  1. J₀: Jacobian of Keplerian or equinoctial elements with respect to + * Cartesian parameters at date t₀ allows to compute maneuver effect as a change + * in orbital elements at maneuver date t₀,
    2. + *
  2. J1/0: Jacobian of Keplerian or equinoctial elements at date t₁ + * with respect to Keplerian or equinoctial elements at date t₀ allows to + * propagate the change in orbital elements to final date t₁.
    3. + *
+ * + *

+ * The second Jacobian, J1/0, is computed using a simple Keplerian + * model, i.e. it is the identity except for the mean motion row which also + * includes an off-diagonal element due to semi-major axis change. + *

+ *

+ * The orbital elements change at date t₁ can be added to orbital elements + * extracted from state, and the final elements taking account the changes are + * then converted back to appropriate type, which may be different from + * Keplerian or equinoctial elements. + *

+ *

+ * Note that this model takes only Keplerian effects into account. This + * means that using only this class to compute an inclination maneuver in Low + * Earth Orbit will not change ascending node drift rate despite + * inclination has changed (the same would be true for a semi-major axis change + * of course). In order to take this drift into account, an instance of + * {@link org.orekit.propagation.analytical.J2DifferentialEffect + * J2DifferentialEffect} must be used together with an instance of this class. + *

+ * + * @author Luc Maisonobe + * @author Nicolas Fialton (field translation) + */ +public class FieldSmallManeuverAnalyticalModel> + implements FieldAdapterPropagator.FieldDifferentialEffect { + /** State at maneuver date (before maneuver occurrence). */ + private final FieldSpacecraftState state0; + + /** Inertial velocity increment. */ + private final FieldVector3D inertialDV; + + /** Mass change ratio. */ + private final T massRatio; + + /** Type of orbit used for internal Jacobians. */ + private final OrbitType type; + + /** Initial Keplerian (or equinoctial) Jacobian with respect to maneuver. */ + private final T[][] j0; + + /** + * Time derivative of the initial Keplerian (or equinoctial) Jacobian with + * respect to maneuver. + */ + private T[][] j0Dot; + + /** Mean anomaly change factor. */ + private final T ksi; + + /** + * Build a maneuver defined in spacecraft frame. + * + * @param state0 state at maneuver date, before the maneuver is + * performed + * @param dV velocity increment in spacecraft frame + * @param isp engine specific impulse (s) + */ + public FieldSmallManeuverAnalyticalModel(Field field, final FieldSpacecraftState state0, + final FieldVector3D dV, final T isp) { + this(field, state0, state0.getFrame(), state0.getAttitude().getRotation().applyInverseTo(dV), isp); + } + + /** + * Build a maneuver defined in user-specified frame. + * + * @param state0 state at maneuver date, before the maneuver is + * performed + * @param frame frame in which velocity increment is defined + * @param dV velocity increment in specified frame + * @param isp engine specific impulse (s) + */ + public FieldSmallManeuverAnalyticalModel(Field field, final FieldSpacecraftState state0, final Frame frame, + final FieldVector3D dV, final T isp) { + final T zero = field.getZero(); + this.state0 = state0; + this.massRatio = FastMath.exp(zero.subtract(dV.getNorm()).divide(isp.multiply(Constants.G0_STANDARD_GRAVITY))); + + // use equinoctial orbit type if possible, Keplerian if nearly hyperbolic orbits + type = (state0.getE().getReal() < 0.9) ? OrbitType.EQUINOCTIAL : OrbitType.KEPLERIAN; + + // compute initial Jacobian + final T[][] fullJacobian = MathArrays.buildArray(field, 6, 6); + j0 = MathArrays.buildArray(field, 6, 3); + final FieldOrbit orbit0 = type.convertType(state0.getOrbit()); + orbit0.getJacobianWrtCartesian(PositionAngle.MEAN, fullJacobian); + for (int i = 0; i < j0.length; ++i) { + System.arraycopy(fullJacobian[i], 3, j0[i], 0, 3); + } + + // use lazy evaluation for j0Dot, as it is used only when Jacobians are + // evaluated + j0Dot = null; + + // compute maneuver effect on Keplerian (or equinoctial) elements + inertialDV = frame.getTransformTo(state0.getFrame(), state0.getDate()).transformVector(dV); + + // compute mean anomaly change: dM(t1) = dM(t0) + ksi * da * (t1 - t0) + final T mu = state0.getMu(); + final T a = state0.getA(); + ksi = zero.add(-1.5).multiply(FastMath.sqrt(zero.add(mu).divide(a)).divide(a.multiply(a))); + + } + + /** + * Get the date of the maneuver. + * + * @return date of the maneuver + */ + public FieldAbsoluteDate getDate() { + return state0.getDate(); + } + + /** + * Get the inertial velocity increment of the maneuver. + * + * @return velocity increment in a state-dependent inertial frame + * @see #getInertialFrame() + */ + public FieldVector3D getInertialDV() { + return inertialDV; + } + + /** + * Get the inertial frame in which the velocity increment is defined. + * + * @return inertial frame in which the velocity increment is defined + * @see #getInertialDV() + */ + public Frame getInertialFrame() { + return state0.getFrame(); + } + + /** + * Compute the effect of the maneuver on an orbit. + * + * @param orbit1 original orbit at t₁, without maneuver + * @return orbit at t₁, taking the maneuver into account if t₁ > t₀ + * @see #apply(SpacecraftState) + * @see #getJacobian(Orbit, PositionAngle, double[][]) + */ + public FieldOrbit apply(final FieldOrbit orbit1) { + + if (orbit1.getDate().compareTo(state0.getDate()) <= 0) { + // the maneuver has not occurred yet, don't change anything + return orbit1; + } + + return orbit1.getType().convertType(updateOrbit(orbit1)); + + } + + /** + * Compute the effect of the maneuver on a spacecraft state. + * + * @param state1 original spacecraft state at t₁, without maneuver + * @return spacecraft state at t₁, taking the maneuver into account if t₁ > + * t₀ + * @see #apply(Orbit) + * @see #getJacobian(Orbit, PositionAngle, double[][]) + */ + public FieldSpacecraftState apply(final FieldSpacecraftState state1) { + + if (state1.getDate().compareTo(state0.getDate()) <= 0) { + // the maneuver has not occurred yet, don't change anything + return state1; + } + + return new FieldSpacecraftState<>(state1.getOrbit().getType().convertType(updateOrbit(state1.getOrbit())), + state1.getAttitude(), updateMass(state1.getMass())); + + } + + /** + * Compute the effect of the maneuver on an orbit. + * + * @param orbit1 original orbit at t₁, without maneuver + * @return orbit at t₁, always taking the maneuver into account, always in the + * internal type + */ + private FieldOrbit updateOrbit(final FieldOrbit orbit1) { + + // compute maneuver effect + final T dt = orbit1.getDate().durationFrom(state0.getDate()); + final T x = inertialDV.getX(); + final T y = inertialDV.getY(); + final T z = inertialDV.getZ(); + final Field field = z.getField(); + final T[] delta = MathArrays.buildArray(field, 6); + for (int i = 0; i < delta.length; ++i) { + delta[i] = j0[i][0].multiply(x).add(j0[i][1].multiply(y).add(j0[i][2].multiply(z))); + } + delta[5] = delta[5].add(ksi.multiply(delta[0]).multiply(dt)); + + // convert current orbital state to Keplerian or equinoctial elements + final T[] parameters = MathArrays.buildArray(field, 6); + type.mapOrbitToArray(type.convertType(orbit1), PositionAngle.MEAN, parameters, null); + for (int i = 0; i < delta.length; ++i) { + parameters[i] = parameters[i].add(delta[i]); + } + + // build updated orbit as Keplerian or equinoctial elements + return type.mapArrayToOrbit(parameters, null, PositionAngle.MEAN, orbit1.getDate(), orbit1.getMu(), + orbit1.getFrame()); + + } + + /** + * Compute the Jacobian of the orbit with respect to maneuver parameters. + *

+ * The Jacobian matrix is a 6x4 matrix. Element jacobian[i][j] corresponds to + * the partial derivative of orbital parameter i with respect to maneuver + * parameter j. The rows order is the same order as used in + * {@link Orbit#getJacobianWrtCartesian(PositionAngle, double[][]) + * Orbit.getJacobianWrtCartesian} method. Columns (0, 1, 2) correspond to the + * velocity increment coordinates (ΔVx, ΔVy, + * ΔVz) in the inertial frame returned by + * {@link #getInertialFrame()}, and column 3 corresponds to the maneuver date + * t₀. + *

+ * + * @param orbit1 original orbit at t₁, without maneuver + * @param positionAngle type of the position angle to use + * @param jacobian placeholder 6x4 (or larger) matrix to be filled with the + * Jacobian, if matrix is larger than 6x4, only the 6x4 + * upper left corner will be modified + * @see #apply(Orbit) + */ + public void getJacobian(final FieldOrbit orbit1, final PositionAngle positionAngle, final T[][] jacobian) { + + final T dt = orbit1.getDate().durationFrom(state0.getDate()); + if (dt.getReal() < 0) { + // the maneuver has not occurred yet, Jacobian is null + for (int i = 0; i < 6; ++i) { + Arrays.fill(jacobian[i], 0, 4, 0.0); + } + return; + } + + // derivatives of Keplerian/equinoctial elements with respect to velocity + // increment + final T x = inertialDV.getX(); + final T y = inertialDV.getY(); + final T z = inertialDV.getZ(); + for (int i = 0; i < 6; ++i) { + System.arraycopy(j0[i], 0, jacobian[i], 0, 3); + } + for (int j = 0; j < 3; ++j) { + jacobian[5][j] = jacobian[5][j].add(ksi.multiply(dt).multiply(j0[0][j])); + } + + // derivatives of Keplerian/equinoctial elements with respect to date + evaluateJ0Dot(); + for (int i = 0; i < 6; ++i) { + jacobian[i][3] = j0Dot[i][0].multiply(x).add(j0Dot[i][1].multiply(y)).add(j0Dot[i][2].multiply(z)); + } + final T da = j0[0][0].multiply(x).add(j0[0][1].multiply(y)).add(j0[0][2].multiply(z)); + jacobian[5][3] = jacobian[5][3].add(ksi.multiply(jacobian[0][3].multiply(dt).subtract(da))); + + if (orbit1.getType() != type || positionAngle != PositionAngle.MEAN) { + + // convert to derivatives of Cartesian parameters + final Field field = x.getField(); + final T[][] j2 = MathArrays.buildArray(field, 6, 6); + final T[][] pvJacobian = MathArrays.buildArray(field, 6, 4); + final FieldOrbit updated = updateOrbit(orbit1); + updated.getJacobianWrtParameters(PositionAngle.MEAN, j2); + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 4; ++j) { + pvJacobian[i][j] = j2[i][0].multiply(jacobian[0][j]).add(j2[i][1].multiply(jacobian[1][j])) + .add(j2[i][2].multiply(jacobian[2][j])).add(j2[i][3].multiply(jacobian[3][j])) + .add(j2[i][4].multiply(jacobian[4][j])).add(j2[i][5].multiply(jacobian[5][j])); + } + } + + // convert to derivatives of specified parameters + final T[][] j3 = MathArrays.buildArray(field, 6, 6); + orbit1.getType().convertType(updated).getJacobianWrtCartesian(positionAngle, j3); + for (int j = 0; j < 4; ++j) { + for (int i = 0; i < 6; ++i) { + jacobian[i][j] = j3[i][0].multiply(pvJacobian[0][j]).add(j3[i][1].multiply(pvJacobian[1][j])) + .add(j3[i][2].multiply(pvJacobian[2][j])).add(j3[i][3].multiply(pvJacobian[3][j])) + .add(j3[i][4].multiply(pvJacobian[4][j])).add(j3[i][5].multiply(pvJacobian[5][j])); + } + } + + } + + } + + /** + * Lazy evaluation of the initial Jacobian time derivative. + */ + private void evaluateJ0Dot() { + + if (j0Dot == null) { + final Field field = massRatio.getField(); + final T zero = field.getZero(); + j0Dot = MathArrays.buildArray(field, 6, 3); + final T dt = zero.add(1.0e-5).divide(state0.getOrbit().getKeplerianMeanMotion()); + final FieldOrbit orbit = type.convertType(state0.getOrbit()); + + // compute shifted Jacobians + final T[][] j0m1 = MathArrays.buildArray(field, 6, 6); + orbit.shiftedBy(dt.multiply(-1)).getJacobianWrtCartesian(PositionAngle.MEAN, j0m1); + final T[][] j0p1 = MathArrays.buildArray(field, 6, 6); + orbit.shiftedBy(dt.multiply(+1)).getJacobianWrtCartesian(PositionAngle.MEAN, j0p1); + + // evaluate derivative by finite differences + for (int i = 0; i < j0Dot.length; ++i) { + final T[] m1Row = j0m1[i]; + final T[] p1Row = j0p1[i]; + final T[] j0DotRow = j0Dot[i]; + for (int j = 0; j < 3; ++j) { + j0DotRow[j] = (p1Row[j + 3].subtract(m1Row[j + 3])).divide(dt.multiply(2)); + } + } + + } + + } + + /** + * Update a spacecraft mass due to maneuver. + * + * @param mass masse before maneuver + * @return mass after maneuver + */ + public T updateMass(final T mass) { + return massRatio.multiply(mass); + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/AbstractGNSSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/AbstractGNSSPropagator.java index b70536681..8e33cedc8 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/AbstractGNSSPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/AbstractGNSSPropagator.java @@ -40,6 +40,7 @@ import org.orekit.utils.PVCoordinates; *

* @author Pascal Parraud */ + public abstract class AbstractGNSSPropagator extends AbstractAnalyticalPropagator { // Data used to solve Kepler's equation diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java new file mode 100644 index 000000000..c966649db --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java @@ -0,0 +1,388 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import java.util.Collections; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.util.FastMath; +import org.hipparchus.util.MathUtils; +import org.hipparchus.util.Precision; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.orbits.FieldCartesianOrbit; +import org.orekit.orbits.FieldOrbit; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.analytical.FieldAbstractAnalyticalPropagator; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.ParameterDriver; + +/** + * Common handling of {@link FieldAbstractAnalyticalPropagator} methods for GNSS + * propagators. + *

+ * This abstract class allows to provide easily a subset of + * {@link FieldAbstractAnalyticalPropagator} methods for specific GNSS propagators. + *

+ * + * @author Pascal Parraud + * @author Nicolas Fialton (field translation) + */ + +public abstract class FieldAbstractGNSSPropagator> + extends FieldAbstractAnalyticalPropagator { + + // Data used to solve Kepler's equation + /** First coefficient to compute Kepler equation solver starter. */ + private static final double A; + + /** Second coefficient to compute Kepler equation solver starter. */ + private static final double B; + + static { + final double k1 = 3 * FastMath.PI + 2; + final double k2 = FastMath.PI - 1; + final double k3 = 6 * FastMath.PI - 1; + A = 3 * k2 * k2 / k1; + B = k3 * k3 / (6 * k1); + } + + /** The GNSS orbital elements used. */ + private final FieldGNSSOrbitalElements gnssOrbit; + + /** Mean angular velocity of the Earth. */ + private final double av; + + /** Duration of the GNSS cycle in seconds. */ + private final double cycleDuration; + + /** The spacecraft mass (kg). */ + private final T mass; + + /** The Earth gravity coefficient used for GNSS propagation. */ + private final T mu; + + /** The ECI frame used for GNSS propagation. */ + private final Frame eci; + + /** The ECEF frame used for GNSS propagation. */ + private final Frame ecef; + + /** Build a new instance. + * @param gnssOrbit the common GNSS orbital elements to be used by the Abstract GNSS propagator + * @param attitudeProvider provider for attitude computation + * @param eci the ECI frame used for GNSS propagation + * @param ecef the ECEF frame used for GNSS propagation + * @param mass the spacecraft mass (kg) + * @param av mean angular velocity of the Earth (rad/s) + * @param cycleDuration duration of the GNSS cycle in seconds + * @param mu the Earth gravity coefficient used for GNSS propagation + */ + + protected FieldAbstractGNSSPropagator(final Field field, final FieldGNSSOrbitalElements gnssOrbit, + final AttitudeProvider attitudeProvider, + final Frame eci, final Frame ecef, final double mass, + final double av, final double cycleDuration, final double mu) { + super(field, attitudeProvider); + this.gnssOrbit = gnssOrbit; + this.av = av; + this.cycleDuration = cycleDuration; + this.mass = field.getZero().add(mass); + this.mu = field.getZero().add(mu); + // Sets the Earth Centered Inertial frame + this.eci = eci; + // Sets the Earth Centered Earth Fixed frame + this.ecef = ecef; + // Sets the start date as the date of the orbital elements + setStartDate(gnssOrbit.getDate()); + } + + /** Get the duration from GNSS Reference epoch. + *

This takes the GNSS week roll-over into account.

+ * @param date the considered date + * @return the duration from GNSS orbit Reference epoch (s) + */ + private T getTk(final FieldAbsoluteDate date) { + // Time from ephemeris reference epoch + T tk = date.durationFrom(gnssOrbit.getDate()); + // Adjusts the time to take roll over week into account + while (tk.getReal() > 0.5 * cycleDuration) { + tk = tk.subtract(cycleDuration); + } + while (tk.getReal() < -0.5 * cycleDuration) { + tk = tk.add(cycleDuration); + } + // Returns the time from ephemeris reference epoch + return tk; + } + + + /** + * Gets the PVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}. + * + *

The algorithm uses automatic differentiation to compute velocity and + * acceleration.

+ * + * @param date the computation date + * @return the GNSS SV PVCoordinates in {@link #getECEF() ECEF frame} + */ + public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { + // Field + final Field field = date.getField(); + // Duration from GNSS ephemeris Reference date + final FieldUnivariateDerivative2 tk = new FieldUnivariateDerivative2<>(getTk(date), field.getOne(), field.getZero()); + // Mean anomaly + final FieldUnivariateDerivative2 mk = tk.multiply(gnssOrbit.getMeanMotion()).add(gnssOrbit.getM0()); + // Eccentric Anomaly + final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk); + // True Anomaly + final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek); + // Argument of Latitude + final FieldUnivariateDerivative2 phik = vk.add(gnssOrbit.getPa()); + final FieldUnivariateDerivative2 twoPhik = phik.multiply(2); + final FieldUnivariateDerivative2 c2phi = twoPhik.cos(); + final FieldUnivariateDerivative2 s2phi = twoPhik.sin(); + // Argument of Latitude Correction + final FieldUnivariateDerivative2 dphik = c2phi.multiply(gnssOrbit.getCuc()).add(s2phi.multiply(gnssOrbit.getCus())); + // Radius Correction + final FieldUnivariateDerivative2 drk = c2phi.multiply(gnssOrbit.getCrc()).add(s2phi.multiply(gnssOrbit.getCrs())); + // Inclination Correction + final FieldUnivariateDerivative2 dik = c2phi.multiply(gnssOrbit.getCic()).add(s2phi.multiply(gnssOrbit.getCis())); + // Corrected Argument of Latitude + final FieldUnivariateDerivative2 uk = phik.add(dphik); + // Corrected Radius + final FieldUnivariateDerivative2 rk = ek.cos().multiply((gnssOrbit.getE()).negate()).add(1).multiply(gnssOrbit.getSma()).add(drk); + // Corrected Inclination + final FieldUnivariateDerivative2 ik = tk.multiply(gnssOrbit.getIDot()).add(gnssOrbit.getI0()).add(dik); + final FieldUnivariateDerivative2 cik = ik.cos(); + // Positions in orbital plane + final FieldUnivariateDerivative2 xk = uk.cos().multiply(rk); + final FieldUnivariateDerivative2 yk = uk.sin().multiply(rk); + // Corrected longitude of ascending node + final FieldUnivariateDerivative2 omk = tk.multiply((gnssOrbit.getOmegaDot()).subtract(av)). + add(gnssOrbit.getOmega0().subtract(gnssOrbit.getTime().multiply(av)) ); + final FieldUnivariateDerivative2 comk = omk.cos(); + final FieldUnivariateDerivative2 somk = omk.sin(); + // returns the Earth-fixed coordinates + final FieldVector3D> positionwithDerivatives = + new FieldVector3D<>(xk.multiply(comk).subtract(yk.multiply(somk).multiply(cik)), + xk.multiply(somk).add(yk.multiply(comk).multiply(cik)), + yk.multiply(ik.sin())); + return new FieldPVCoordinates(new FieldVector3D(positionwithDerivatives.getX().getValue(), + positionwithDerivatives.getY().getValue(), + positionwithDerivatives.getZ().getValue()), + new FieldVector3D(positionwithDerivatives.getX().getFirstDerivative(), + positionwithDerivatives.getY().getFirstDerivative(), + positionwithDerivatives.getZ().getFirstDerivative()), + new FieldVector3D(positionwithDerivatives.getX().getSecondDerivative(), + positionwithDerivatives.getY().getSecondDerivative(), + positionwithDerivatives.getZ().getSecondDerivative())); + } + + + + + + + /** + * Gets eccentric anomaly from mean anomaly. + *

The algorithm used to solve the Kepler equation has been published in: + * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. Gooding, + * Celestial Mechanics 38 (1986) 307-334

+ *

It has been copied from the OREKIT library (KeplerianOrbit class).

+ * + * @param mk the mean anomaly (rad) + * @return the eccentric anomaly (rad) + */ + private FieldUnivariateDerivative2 getEccentricAnomaly(final FieldUnivariateDerivative2 mk) { + + final T zero = mk.getValue().getField().getZero(); + // reduce M to [-PI PI] interval + final FieldUnivariateDerivative2 reducedM = new FieldUnivariateDerivative2(MathUtils.normalizeAngle(mk.getValue(), zero), + mk.getFirstDerivative(), + mk.getSecondDerivative()); + + // compute start value according to A. W. Odell and R. H. Gooding S12 starter + FieldUnivariateDerivative2 ek; + if (FastMath.abs(reducedM.getValue().getReal()) < 1.0 / 6.0) { + if (FastMath.abs(reducedM.getValue().getReal()) < Precision.SAFE_MIN) { + // this is an Orekit change to the S12 starter. + // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN appearing later in + // the computation. As in this case E and M are almost equal, we initialize ek with reducedM + ek = reducedM; + } else { + // this is the standard S12 starter + ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(gnssOrbit.getE())); + } + } else { + if (reducedM.getValue().getReal() < 0) { + final FieldUnivariateDerivative2 w = reducedM.add(FastMath.PI); + ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(gnssOrbit.getE())); + } else { + final FieldUnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI); + ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(gnssOrbit.getE())); + } + } + + final T e1 = gnssOrbit.getE().negate().add(1.0); + final boolean noCancellationRisk = (e1.getReal() + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; + + // perform two iterations, each consisting of one Halley step and one Newton-Raphson step + for (int j = 0; j < 2; ++j) { + final FieldUnivariateDerivative2 f; + FieldUnivariateDerivative2 fd; + final FieldUnivariateDerivative2 fdd = ek.sin().multiply(gnssOrbit.getE()); + final FieldUnivariateDerivative2 fddd = ek.cos().multiply(gnssOrbit.getE()); + if (noCancellationRisk) { + f = ek.subtract(fdd).subtract(reducedM); + fd = fddd.subtract(1).negate(); + } else { + f = eMeSinE(ek).subtract(reducedM); + final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); + fd = s.multiply(s).multiply(gnssOrbit.getE().multiply(2.0)).add(e1); + } + final FieldUnivariateDerivative2 dee = f.multiply(fd).divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd))); + + // update eccentric anomaly, using expressions that limit underflow problems + final FieldUnivariateDerivative2 w = fd.add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3)))); + fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); + ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd)); + } + + // expand the result back to original range + ek = ek.add((mk.getValue()).subtract(reducedM.getValue())); + + // Returns the eccentric anomaly + return ek; + } + + /** + * Accurate computation of E - e sin(E). + * + * @param E eccentric anomaly + * @return E - e sin(E) + */ + private FieldUnivariateDerivative2 eMeSinE(final FieldUnivariateDerivative2 E) { + FieldUnivariateDerivative2 x = E.sin().multiply(gnssOrbit.getE().negate().add(1)); + final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); + FieldUnivariateDerivative2 term = E; + FieldUnivariateDerivative2 d = E.getField().getZero(); + // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance + for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); !(x.getValue()).equals(x0.getValue());) { + d = d.add(2); + term = term.multiply(mE2.divide(d.multiply(d.add(1)))); + x0 = x; + x = x.subtract(term); + } + return x; + } + + /** Gets true anomaly from eccentric anomaly. + * + * @param ek the eccentric anomaly (rad) + * @return the true anomaly (rad) + */ + private FieldUnivariateDerivative2 getTrueAnomaly(final FieldUnivariateDerivative2 ek) { + final FieldUnivariateDerivative2 svk = ek.sin().multiply(FastMath.sqrt((gnssOrbit.getE().multiply(gnssOrbit.getE())).negate().add(1.0) )); + final FieldUnivariateDerivative2 cvk = ek.cos().subtract(gnssOrbit.getE()); + return svk.atan2(cvk); + } + + /** {@inheritDoc} */ + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, final T[] parameters) { + // Gets the PVCoordinates in ECEF frame + final FieldPVCoordinates pvaInECEF = propagateInEcef(date); + // Transforms the PVCoordinates to ECI frame + final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); + // Returns the Cartesian orbit + return new FieldCartesianOrbit(pvaInECI, eci, date, mu); + } + + /** + * Get the Earth gravity coefficient used for GNSS propagation. + * @return the Earth gravity coefficient. + */ + public T getMU() { + return mu; + } + + /** {@inheritDoc} */ + public Frame getFrame() { + return eci; + } + + /** {@inheritDoc} */ + protected T getMass(final FieldAbsoluteDate date) { + return mass; + } + + /** {@inheritDoc} */ + public void resetInitialState(final FieldSpacecraftState state) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected void resetIntermediateState(final FieldSpacecraftState state, final boolean forward) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + + + + + /** {@inheritDoc} */ + @Override + protected List getParametersDrivers() { + // GNSS propagation model does not have parameter drivers. + return Collections.emptyList(); + } + + + + + /** + * Gets the Earth Centered Inertial frame used to propagate the orbit. + * + * @return the ECI frame + */ + public Frame getECI() { + return eci; + } + + /** + * Gets the Earth Centered Earth Fixed frame used to propagate GNSS orbits according to the + * Interface Control Document. + * + * @return the ECEF frame + */ + public Frame getECEF() { + return ecef; + } + +} + + + + + diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java new file mode 100644 index 000000000..1934b7e31 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java @@ -0,0 +1,315 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; +import org.hipparchus.util.FastMath; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.data.DataContext; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.FieldAbsoluteDate; + +/** + * Class for BeiDou almanac. + * + * @see "BeiDou Navigation Satellite System, Signal In Space, Interface Control + * Document, Version 2.1, Table 5-12" + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + * + */ +public class FieldBeidouAlmanac> implements FieldBeidouOrbitalElements { + + /** PRN number. */ + private final int prn; + + /** Health status. */ + private final int health; + + /** BeiDou week. */ + private final int week; + + /** Time of applicability. */ + private final T toa; + + /** Semi-major axis. */ + private final T sma; + + /** Eccentricity. */ + private final T ecc; + + /** Inclination. */ + private final T inc; + + /** Longitude of Orbital Plane. */ + private final T om0; + + /** Rate of Right Ascension. */ + private final T dom; + + /** Argument of perigee. */ + private final T aop; + + /** Mean anomaly. */ + private final T anom; + + /** Zeroth order clock correction. */ + private final T af0; + + /** First order clock correction. */ + private final T af1; + + /** Date of validity. */ + private final FieldAbsoluteDate date; + + /** + * Build a new almanac. + * + *

+ * This method uses the {@link DataContext#getDefault() default data context}. + * + * @param prn the PRN number + * @param week the BeiDou week + * @param toa the Almanac Time of Applicability (s) + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc0 the orbit reference inclination 0.0 for GEO satellites and 0.30 + * * BEIDOU_PI for MEO/IGSO satellites (rad) + * @param dinc the correction of orbit reference inclination at reference time + * (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @see #BeidouAlmanac(int, int, T, T, T, T, T, T, T, T, T, T, T, int, + * AbsoluteDate) + */ + @DefaultDataContext + public FieldBeidouAlmanac(final int prn, final int week, final T toa, final T sqa, final T ecc, final T inc0, + final T dinc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, + final int health) { + this(prn, week, toa, sqa, ecc, inc0, dinc, om0, dom, aop, anom, af0, af1, health, + new FieldGNSSDate(week, toa.multiply(1000), SatelliteSystem.BEIDOU, DataContext.getDefault().getTimeScales()) + .getDate()); + } + + /** + * Build a new almanac. + * + * @param prn the PRN number + * @param week the BeiDou week + * @param toa the Almanac Time of Applicability (s) + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc0 the orbit reference inclination 0.0 for GEO satellites and 0.30 + * * BEIDOU_PI for MEO/IGSO satellites (rad) + * @param dinc the correction of orbit reference inclination at reference time + * (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @param date that corresponds to {@code week} and {@code toa}. + * @since 10.1 + */ + public FieldBeidouAlmanac(final int prn, final int week, final T toa, final T sqa, final T ecc, final T inc0, + final T dinc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, + final int health, final FieldAbsoluteDate date) { + this.prn = prn; + this.week = week; + this.toa = toa; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.inc = inc0.add(dinc); + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.af0 = af0; + this.af1 = af1; + this.health = health; + this.date = date; + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toa; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(date.getDate().getField().getZero().add(BEIDOU_MU).divide(absA)).divide(absA); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return date.getDate().getField().getZero(); + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCus() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCrc() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCrs() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCic() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCis() { + return date.getDate().getField().getZero(); + } + + @Override + public T getAf0() { + return af0; + } + + @Override + public T getAf1() { + return af1; + } + + /** + * Gets the Health status. + * + * @return the Health status + */ + public int getHealth() { + return health; + } + + @Override + public T getAf2() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public int getAODC() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public int getAODE() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public int getIOD() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getTGD1() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getTGD2() { + // TODO Auto-generated method stub + return null; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouOrbitalElements.java new file mode 100644 index 000000000..abd14faed --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouOrbitalElements.java @@ -0,0 +1,76 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; + +/** This interface provides the minimal set of orbital elements needed by the {@link BeidouPropagator}. +* +* @see Beidou Interface Control Document +* @author Bryan Cazabonne +* @author Nicolas Fialton (field translation) +*/ +public interface FieldBeidouOrbitalElements> extends FieldGNSSOrbitalElements { + + // Constants + /** Earth's universal gravitational parameter for Beidou user in m³/s². */ + double BEIDOU_MU = 3.986004418e+14; + + /** Value of Pi for conversion from semicircles to radian. */ + double BEIDOU_PI = 3.1415926535898; + + /** Duration of the Beidou week in seconds. */ + double BEIDOU_WEEK_IN_SECONDS = 604800.; + + /** Number of weeks in the Beidou cycle. */ + int BEIDOU_WEEK_NB = 8192; + + /** + * Gets the Age Of Data Clock (AODC). + * + * @return the Age Of Data Clock (AODC) + */ + int getAODC(); + + /** + * Gets the Age Of Data Ephemeris (AODE). + * + * @return the Age Of Data Ephemeris (AODE) + */ + int getAODE(); + + /** + * Gets the BeiDou Issue Of Data (IOD). + * + * @return the IOD + */ + int getIOD(); + + /** + * Gets the estimated group delay differential TGD1 for B1I signal. + * + * @return the estimated group delay differential TGD1 for B1I signal (s) + */ + T getTGD1(); + + /** + * Gets the estimated group delay differential TGD for B2I signal. + * + * @return the estimated group delay differential TGD2 for B2I signal (s) + */ + T getTGD2(); +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java new file mode 100644 index 000000000..e64f54cc7 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java @@ -0,0 +1,89 @@ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.data.DataContext; +import org.orekit.frames.Frame; +import org.orekit.frames.Frames; +import org.orekit.propagation.Propagator; +import org.orekit.utils.IERSConventions; + +/** + * This class aims at propagating a Beidou orbit from + * {@link FieldBeidouOrbitalElements}. + * + * @see Beidou + * Interface Control Document + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + */ +public class FieldBeidouPropagator> extends FieldAbstractGNSSPropagator { + + // Constants + /** Value of the earth's rotation rate in rad/s. */ + private static final double BEIDOU_AV = 7.2921150e-5; + + /** Duration of the Beidou cycle in seconds. */ + private static final double BEIDOU_CYCLE_DURATION = FieldBeidouOrbitalElements.BEIDOU_WEEK_IN_SECONDS + * FieldBeidouOrbitalElements.BEIDOU_WEEK_NB; + + // Fields + /** The Beidou orbital elements used. */ + private final FieldBeidouOrbitalElements bdsOrbit; + + /** + * Default constructor. + * + * @param field + * @param bdsOrbit + */ + @DefaultDataContext + public FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit) { + this(field, bdsOrbit, DataContext.getDefault().getFrames()); + } + + /** + * Constructor. + * + * @param field + * @param bdsOrbit + * @param frames + */ + public FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, + final Frames frames) { + this(field, bdsOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), + frames.getITRF(IERSConventions.IERS_2010, true)); + } + + /** + * Constructor. + * + * @param field + * @param bdsOrbit + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { + super(field, bdsOrbit, attitudeProvider, eci, ecef, mass, BEIDOU_AV, BEIDOU_CYCLE_DURATION, + FieldBeidouOrbitalElements.BEIDOU_MU); + // Stores the Beidou orbital elements + this.bdsOrbit = bdsOrbit; + } + + /** + * Get the underlying Beidou orbital elements. + * + * @return the underlying Beidou orbital elements + */ + public FieldBeidouOrbitalElements getFieldBeidouOrbitalElements() { + return bdsOrbit; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java new file mode 100644 index 000000000..690ebaf79 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java @@ -0,0 +1,860 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.util.FastMath; +import org.hipparchus.util.FieldSinCos; +import org.hipparchus.util.MathArrays; +import org.hipparchus.util.MathUtils; +import org.hipparchus.util.Precision; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.data.DataContext; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.frames.Frames; +import org.orekit.orbits.FieldCartesianOrbit; +import org.orekit.orbits.FieldOrbit; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.Propagator; +import org.orekit.propagation.SpacecraftState; +import org.orekit.propagation.analytical.FieldAbstractAnalyticalPropagator; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeScale; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.IERSConventions; +import org.orekit.utils.ParameterDriver; + +/** + * This class aims at propagating a GLONASS orbit from {@link GLONASSOrbitalElements}. + * + * @see + * GLONASS Interface Control Document + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + */ +public class FieldGLONASSAnalyticalPropagator> extends FieldAbstractAnalyticalPropagator { + + // Constants + /** Constant 7.0 / 3.0. */ + private static final double SEVEN_THIRD = 7.0 / 3.0; + + /** Constant 7.0 / 6.0. */ + private static final double SEVEN_SIXTH = 7.0 / 6.0; + + /** Constant 7.0 / 24.0. */ + private static final double SEVEN_24TH = 7.0 / 24.0; + + /** Constant 49.0 / 72.0. */ + private static final double FN_72TH = 49.0 / 72.0; + + /** Value of the earth's rotation rate in rad/s. */ + private static final double GLONASS_AV = 7.2921150e-5; + + /** Mean value of inclination for Glonass orbit is equal to 63°. */ + private static final double GLONASS_MEAN_INCLINATION = 64.8; + + /** Mean value of Draconian period for Glonass orbit is equal to 40544s : 11 hours 15 minutes 44 seconds. */ + private static final double GLONASS_MEAN_DRACONIAN_PERIOD = 40544; + + /** Second degree zonal coefficient of normal potential. */ + private static final double GLONASS_J20 = 1.08262575e-3; + + /** Equatorial radius of Earth (m). */ + private static final double GLONASS_EARTH_EQUATORIAL_RADIUS = 6378136; + + // Data used to solve Kepler's equation + /** First coefficient to compute Kepler equation solver starter. */ + private static final double A; + + /** Second coefficient to compute Kepler equation solver starter. */ + private static final double B; + + static { + final double k1 = 3 * FastMath.PI + 2; + final double k2 = FastMath.PI - 1; + final double k3 = 6 * FastMath.PI - 1; + A = 3 * k2 * k2 / k1; + B = k3 * k3 / (6 * k1); + } + + // Fields + /** The GLONASS orbital elements used. */ + private final FieldGLONASSOrbitalElements glonassOrbit; + + /** The spacecraft mass (kg). */ + private final double mass; + + /** The ECI frame used for GLONASS propagation. */ + private final Frame eci; + + /** The ECEF frame used for GLONASS propagation. */ + private final Frame ecef; + + /** Data context for propagation. */ + private final DataContext dataContext; + + private final Field field; + /** + * This nested class aims at building a GLONASSPropagator. + *

It implements the classical FieldBuilder pattern.

+ * + */ + public class FieldBuilder { + + // Required parameter + /** The GLONASS orbital elements. */ + private final FieldGLONASSOrbitalElements orbit; + + // Optional parameters + /** The attitude provider. */ + private AttitudeProvider attitudeProvider; + /** The mass. */ + private double mass = DEFAULT_MASS; + /** The ECI frame. */ + private Frame eci = null; + /** The ECEF frame. */ + private Frame ecef = null; + /** Data context. */ + private DataContext dataContext; + + /** Initializes the FieldBuilder. + *

The GLONASS orbital elements is the only requested parameter to build a GLONASSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The data context is by default to the + * {@link DataContext#getDefault() default data context}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldBuilder(final GLONASSOrbitalElements gpsOrbElt, final DataContext dataContext)}

+ * + * @param glonassOrbElt the GLONASS orbital elements to be used by the GLONASS propagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) + */ + @DefaultDataContext + public FieldBuilder(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt) { + this(field, glonassOrbElt, DataContext.getDefault()); + } + + /** Initializes the FieldBuilder. + *

The GLONASS orbital elements is the only requested parameter to build a GLONASSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#getDefaultLaw(Frames)}.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link Frames#getEME2000() EME2000 frame}.
+ * The ECEF frame is set by default to the + * {@link Frames#getITRF(IERSConventions, boolean) CIO/2010-based ITRF simple + * EOP}. + *

+ * + * @param glonassOrbElt the GLONASS orbital elements to be used by the GLONASS propagator. + * @param dataContext the data context to use for frames and time scales. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) + * @since 10.1 + */ + public FieldBuilder(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, + final DataContext dataContext) { + this.orbit = glonassOrbElt; + this.dataContext = dataContext; + final Frames frames = dataContext.getFrames(); + this.eci = frames.getEME2000(); + this.ecef = frames.getITRF(IERSConventions.IERS_2010, true); + attitudeProvider = Propagator.getDefaultLaw(frames); + } + + /** Sets the attitude provider. + * + * @param userProvider the attitude provider + * @return the updated FieldBuilder + */ + public FieldBuilder attitudeProvider(final AttitudeProvider userProvider) { + this.attitudeProvider = userProvider; + return this; + } + + /** Sets the mass. + * + * @param userMass the mass (in kg) + * @return the updated FieldBuilder + */ + public FieldBuilder mass(final double userMass) { + this.mass = userMass; + return this; + } + + /** Sets the Earth Centered Inertial frame used for propagation. + * + * @param inertial the ECI frame + * @return the updated FieldBuilder + */ + public FieldBuilder eci(final Frame inertial) { + this.eci = inertial; + return this; + } + + /** Sets the Earth Centered Earth Fixed frame assimilated to the WGS84 ECEF. + * + * @param bodyFixed the ECEF frame + * @return the updated FieldBuilder + */ + public FieldBuilder ecef(final Frame bodyFixed) { + this.ecef = bodyFixed; + return this; + } + + /** + * Sets the data context used by the propagator. Does not update the ECI or ECEF + * frames which must be done separately using {@link #eci(Frame)} and {@link + * #ecef(Frame)}. + * + * @param context used for propagation. + * @return the updated FieldBuilder. + */ + public FieldBuilder dataContext(final DataContext context) { + this.dataContext = context; + return this; + } + + /** Finalizes the build. + * + * @return the built GLONASSPropagator + */ + public FieldGLONASSAnalyticalPropagator build() { + return new FieldGLONASSAnalyticalPropagator<>(field, this); + } + + } + + /** + * Private constructor. + * @param FieldBuilder the FieldBuilder + */ + private FieldGLONASSAnalyticalPropagator(final Field field, final FieldBuilder FieldBuilder) { + super(field, FieldBuilder.attitudeProvider); + this.dataContext = FieldBuilder.dataContext; + // Stores the GLONASS orbital elements + this.glonassOrbit = FieldBuilder.orbit; + // Sets the start date as the date of the orbital elements + setStartDate(glonassOrbit.getDate()); + // Sets the mass + this.mass = FieldBuilder.mass; + // Sets the Earth Centered Inertial frame + this.eci = FieldBuilder.eci; + // Sets the Earth Centered Earth Fixed frame + this.ecef = FieldBuilder.ecef; + this.field = null; + } + + /** + * Gets the PVCoordinates of the GLONASS SV in {@link #getECEF() ECEF frame}. + * + *

The algorithm is defined at Appendix M.1 from GLONASS Interface Control Document, + * with automatic differentiation added to compute velocity and + * acceleration.

+ * + * @param date the computation date + * @return the GLONASS SV PVCoordinates in {@link #getECEF() ECEF frame} + */ + public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { + + // Interval of prediction dTpr + final FieldUnivariateDerivative2 dTpr = getdTpr(date); + + // Zero + final FieldUnivariateDerivative2 zero = dTpr.getField().getZero(); + + // The number of whole orbits "w" on a prediction interval + final FieldUnivariateDerivative2 w = FastMath.floor(dTpr.divide(glonassOrbit.getDeltaT().add(GLONASS_MEAN_DRACONIAN_PERIOD))); + + // Current inclination + final FieldUnivariateDerivative2 i = zero.add(zero.add(GLONASS_MEAN_INCLINATION / 180 * GLONASSOrbitalElements.GLONASS_PI).add( glonassOrbit.getDeltaI())); + + // Eccentricity + final FieldUnivariateDerivative2 e = zero.add(glonassOrbit.getE()); + + // Mean draconique period in orbite w+1 and mean motion + final FieldUnivariateDerivative2 tDR = w.multiply(2.0).add(1.0).multiply(glonassOrbit.getDeltaTDot()). + add(glonassOrbit.getDeltaT()). + add(GLONASS_MEAN_DRACONIAN_PERIOD); + final FieldUnivariateDerivative2 n = tDR.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI).reciprocal(); + + // Semi-major axis : computed by successive approximation + final FieldUnivariateDerivative2 sma = computeSma(tDR, i, e); + + // (ae / p)^2 term + final FieldUnivariateDerivative2 p = sma.multiply(e.multiply(e).negate().add(1.0)); + final FieldUnivariateDerivative2 aeop = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); + final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); + + // Current longitude of the ascending node + final FieldUnivariateDerivative2 lambda = computeLambda(dTpr, n, aeop2, i); + + // Current argument of perigee + final FieldUnivariateDerivative2 pa = computePA(dTpr, n, aeop2, i); + + // Mean longitude at the instant the spacecraft passes the current ascending node + final FieldUnivariateDerivative2 tanPAo2 = FastMath.tan(pa.divide(2.0)); + final FieldUnivariateDerivative2 coef = tanPAo2.multiply(FastMath.sqrt(e.negate().add(1.0).divide(e.add(1.0)))); + final FieldUnivariateDerivative2 e0 = FastMath.atan(coef).multiply(2.0).negate(); + final FieldUnivariateDerivative2 m1 = pa.add(e0).subtract(FastMath.sin(e0).multiply(e)); + + // Current mean longitude + final FieldUnivariateDerivative2 correction = dTpr. + subtract(w.multiply(zero.add(GLONASS_MEAN_DRACONIAN_PERIOD).add(glonassOrbit.getDeltaT()) )). + subtract(w.multiply(w).multiply(glonassOrbit.getDeltaTDot())); + final FieldUnivariateDerivative2 m = m1.add(n.multiply(correction)); + + // Take into consideration the periodic perturbations + final FieldSinCos> scPa = FastMath.sinCos(pa); + final FieldUnivariateDerivative2 h = e.multiply(scPa.sin()); + final FieldUnivariateDerivative2 l = e.multiply(scPa.cos()); + // δa1 + final FieldUnivariateDerivative2[] d1 = getParameterDifferentials(sma, i, h, l, m1); + // δa2 + final FieldUnivariateDerivative2[] d2 = getParameterDifferentials(sma, i, h, l, m); + // Apply corrections + final FieldUnivariateDerivative2 smaCorr = sma.add(d2[0]).subtract(d1[0]); + final FieldUnivariateDerivative2 hCorr = h.add(d2[1]).subtract(d1[1]); + final FieldUnivariateDerivative2 lCorr = l.add(d2[2]).subtract(d1[2]); + final FieldUnivariateDerivative2 lambdaCorr = lambda.add(d2[3]).subtract(d1[3]); + final FieldUnivariateDerivative2 iCorr = i.add(d2[4]).subtract(d1[4]); + final FieldUnivariateDerivative2 mCorr = m.add(d2[5]).subtract(d1[5]); + final FieldUnivariateDerivative2 eCorr = FastMath.sqrt(hCorr.multiply(hCorr).add(lCorr.multiply(lCorr))); + final FieldUnivariateDerivative2 paCorr; + if (eCorr.getValue().getReal() == 0.) { + paCorr = zero; + } else { + if (lCorr.getValue() == eCorr.getValue()) { + paCorr = zero.add(0.5 * GLONASSOrbitalElements.GLONASS_PI); + } else if (lCorr.getValue().getReal() == -eCorr.getValue().getReal()) { + paCorr = zero.add(-0.5 * GLONASSOrbitalElements.GLONASS_PI); + } else { + paCorr = FastMath.atan2(hCorr, lCorr); + } + } + + // Eccentric Anomaly + final FieldUnivariateDerivative2 mk = mCorr.subtract(paCorr); + final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk, eCorr); + + // True Anomaly + final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek, eCorr); + + // Argument of Latitude + final FieldUnivariateDerivative2 phik = vk.add(paCorr); + + // Corrected Radius + final FieldUnivariateDerivative2 pCorr = smaCorr.multiply(eCorr.multiply(eCorr).negate().add(1.0)); + final FieldUnivariateDerivative2 rk = pCorr.divide(eCorr.multiply(FastMath.cos(vk)).add(1.0)); + + // Positions in orbital plane + final FieldSinCos> scPhik = FastMath.sinCos(phik); + final FieldUnivariateDerivative2 xk = scPhik.cos().multiply(rk); + final FieldUnivariateDerivative2 yk = scPhik.sin().multiply(rk); + + // Coordinates of position + final FieldSinCos> scL = FastMath.sinCos(lambdaCorr); + final FieldSinCos> scI = FastMath.sinCos(iCorr); + final FieldVector3D> positionwithDerivatives = + new FieldVector3D<>(xk.multiply(scL.cos()).subtract(yk.multiply(scL.sin()).multiply(scI.cos())), + xk.multiply(scL.sin()).add(yk.multiply(scL.cos()).multiply(scI.cos())), + yk.multiply(scI.sin())); + + return new FieldPVCoordinates(new FieldVector3D(positionwithDerivatives.getX().getValue(), + positionwithDerivatives.getY().getValue(), + positionwithDerivatives.getZ().getValue()), + new FieldVector3D(positionwithDerivatives.getX().getFirstDerivative(), + positionwithDerivatives.getY().getFirstDerivative(), + positionwithDerivatives.getZ().getFirstDerivative()), + new FieldVector3D(positionwithDerivatives.getX().getSecondDerivative(), + positionwithDerivatives.getY().getSecondDerivative(), + positionwithDerivatives.getZ().getSecondDerivative())); + } + + /** + * Gets eccentric anomaly from mean anomaly. + *

The algorithm used to solve the Kepler equation has been published in: + * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. Gooding, + * Celestial Mechanics 38 (1986) 307-334

+ *

It has been copied from the OREKIT library (KeplerianOrbit class).

+ * + * @param mk the mean anomaly (rad) + * @param e the eccentricity + * @return the eccentric anomaly (rad) + */ + private FieldUnivariateDerivative2 getEccentricAnomaly(final FieldUnivariateDerivative2 mk, final FieldUnivariateDerivative2 e) { + + // reduce M to [-PI PI] interval + final T zero = mk.getValue().getField().getZero(); + final FieldUnivariateDerivative2 reducedM = new FieldUnivariateDerivative2(MathUtils.normalizeAngle(mk.getValue(), zero), + mk.getFirstDerivative(), + mk.getSecondDerivative()); + + // compute start value according to A. W. Odell and R. H. Gooding S12 starter + FieldUnivariateDerivative2 ek; + if (FastMath.abs(reducedM.getValue()).getReal() < 1.0 / 6.0) { + if (FastMath.abs(reducedM.getValue()).getReal() < Precision.SAFE_MIN) { + // this is an Orekit change to the S12 starter. + // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN appearing later in + // the computation. As in this case E and M are almost equal, we initialize ek with reducedM + ek = reducedM; + } else { + // this is the standard S12 starter + ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(e)); + } + } else { + if (reducedM.getValue().getReal() < 0) { + final FieldUnivariateDerivative2 w = reducedM.add(FastMath.PI); + ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(e)); + } else { + final FieldUnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI); + ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(e)); + } + } + + final FieldUnivariateDerivative2 e1 = e.negate().add(1.0); + final boolean noCancellationRisk = (e1.getReal() + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; + + // perform two iterations, each consisting of one Halley step and one Newton-Raphson step + for (int j = 0; j < 2; ++j) { + final FieldUnivariateDerivative2 f; + FieldUnivariateDerivative2 fd; + final FieldUnivariateDerivative2 fdd = ek.sin().multiply(e); + final FieldUnivariateDerivative2 fddd = ek.cos().multiply(e); + if (noCancellationRisk) { + f = ek.subtract(fdd).subtract(reducedM); + fd = fddd.subtract(1).negate(); + } else { + f = eMeSinE(ek, e).subtract(reducedM); + final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); + fd = s.multiply(s).multiply(e.multiply(2.0)).add(e1); + } + final FieldUnivariateDerivative2 dee = f.multiply(fd).divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd))); + + // update eccentric anomaly, using expressions that limit underflow problems + final FieldUnivariateDerivative2 w = fd.add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3)))); + fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); + ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd)); + } + + // expand the result back to original range + ek = ek.add(mk.getValue().subtract(reducedM.getValue()) ); + + // Returns the eccentric anomaly + return ek; + } + + /** + * Accurate computation of E - e sin(E). + * + * @param E eccentric anomaly + * @param ecc the eccentricity + * @return E - e sin(E) + */ + private FieldUnivariateDerivative2 eMeSinE(final FieldUnivariateDerivative2 E, final FieldUnivariateDerivative2 ecc) { + FieldUnivariateDerivative2 x = E.sin().multiply(ecc.negate().add(1.0)); + final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); + FieldUnivariateDerivative2 term = E; + FieldUnivariateDerivative2 d = E.getField().getZero(); + // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance + for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); !(x.getValue()).equals(x0.getValue());) { + d = d.add(2); + term = term.multiply(mE2.divide(d.multiply(d.add(1)))); + x0 = x; + x = x.subtract(term); + } + return x; + } + + /** Gets true anomaly from eccentric anomaly. + * + * @param ek the eccentric anomaly (rad) + * @param ecc the eccentricity + * @return the true anomaly (rad) + */ + private FieldUnivariateDerivative2 getTrueAnomaly(final FieldUnivariateDerivative2 ek, final FieldUnivariateDerivative2 ecc) { + final FieldUnivariateDerivative2 svk = ek.sin().multiply(FastMath.sqrt( ecc.multiply(ecc).negate().add(1.0))); + final FieldUnivariateDerivative2 cvk = ek.cos().subtract(ecc); + return svk.atan2(cvk); + } + + /** + * Get the interval of prediction. + * + * @param date the considered date + * @return the duration from GLONASS orbit Reference epoch (s) + */ + private FieldUnivariateDerivative2 getdTpr(final FieldAbsoluteDate date) { + final TimeScale glonass = dataContext.getTimeScales().getGLONASS(); + final FieldGLONASSDate tEnd = new FieldGLONASSDate(date, glonass); + final FieldGLONASSDate tSta = new FieldGLONASSDate(glonassOrbit.getDate(), glonass); + final int n = tEnd.getDayNumber(); + final int na = tSta.getDayNumber(); + final int deltaN; + if (na == 27) { + deltaN = n - na - FastMath.round((float) (n - na) / 1460) * 1460; + } else { + deltaN = n - na - FastMath.round((float) (n - na) / 1461) * 1461; + } + + final T zero = date.getField().getZero(); + final FieldUnivariateDerivative2 ti = new FieldUnivariateDerivative2(zero.add(tEnd.getSecInDay()), zero.add(1.0), zero.add(1.0)); + + return ti.subtract(glonassOrbit.getTime()).add(86400 * deltaN); + } + + /** + * Computes the semi-major axis of orbit using technique of successive approximations. + * @param tDR mean draconique period (s) + * @param i current inclination (rad) + * @param e eccentricity + * @return the semi-major axis (m). + */ + private FieldUnivariateDerivative2 computeSma(final FieldUnivariateDerivative2 tDR, + final FieldUnivariateDerivative2 i, + final FieldUnivariateDerivative2 e) { + + // Zero + final FieldUnivariateDerivative2 zero = tDR.getField().getZero(); + + // If one of the input parameter is equal to Double.NaN, an infinite loop can occur. + // In that case, we do not compute the value of the semi major axis. + // We decided to return a Double.NaN value instead. + if (Double.isNaN(tDR.getValue().getReal()) || Double.isNaN(i.getValue().getReal()) || Double.isNaN(e.getValue().getReal())) { + return zero.add(Double.NaN); + } + + // Common parameters + final FieldUnivariateDerivative2 sinI = FastMath.sin(i); + final FieldUnivariateDerivative2 sin2I = sinI.multiply(sinI); + final FieldUnivariateDerivative2 ome2 = e.multiply(e).negate().add(1.0); + final FieldUnivariateDerivative2 ome2Pow3o2 = FastMath.sqrt(ome2).multiply(ome2); + final FieldUnivariateDerivative2 pa = zero.add(glonassOrbit.getPa()); + final FieldUnivariateDerivative2 cosPA = FastMath.cos(pa); + final FieldUnivariateDerivative2 opecosPA = e.multiply(cosPA).add(1.0); + final FieldUnivariateDerivative2 opecosPAPow2 = opecosPA.multiply(opecosPA); + final FieldUnivariateDerivative2 opecosPAPow3 = opecosPAPow2.multiply(opecosPA); + + // Initial approximation + FieldUnivariateDerivative2 tOCK = tDR; + + // Successive approximations + // The process of approximation ends when fulfilling the following condition: |a(n+1) - a(n)| < 1cm + FieldUnivariateDerivative2 an = zero; + FieldUnivariateDerivative2 anp1 = zero; + boolean isLastStep = false; + while (!isLastStep) { + + // a(n+1) computation + final FieldUnivariateDerivative2 tOCKo2p = tOCK.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI); + final FieldUnivariateDerivative2 tOCKo2pPow2 = tOCKo2p.multiply(tOCKo2p); + anp1 = FastMath.cbrt(tOCKo2pPow2.multiply(GLONASSOrbitalElements.GLONASS_MU)); + + // p(n+1) computation + final FieldUnivariateDerivative2 p = anp1.multiply(ome2); + + // Tock(n+1) computation + final FieldUnivariateDerivative2 aeop = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); + final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); + final FieldUnivariateDerivative2 term1 = aeop2.multiply(GLONASS_J20).multiply(1.5); + final FieldUnivariateDerivative2 term2 = sin2I.multiply(2.5).negate().add(2.0); + final FieldUnivariateDerivative2 term3 = ome2Pow3o2.divide(opecosPAPow2); + final FieldUnivariateDerivative2 term4 = opecosPAPow3.divide(ome2); + tOCK = tDR.divide(term1.multiply(term2.multiply(term3).add(term4)).negate().add(1.0)); + + // Check convergence + if (FastMath.abs(anp1.subtract(an).getReal()) <= 0.01) { + isLastStep = true; + } + + an = anp1; + } + + return an; + + } + + /** + * Computes the current longitude of the ascending node. + * @param dTpr interval of prediction (s) + * @param n mean motion (rad/s) + * @param aeop2 square of the ratio between the radius of the ellipsoid and p, with p = sma * (1 - ecc²) + * @param i inclination (rad) + * @return the current longitude of the ascending node (rad) + */ + private FieldUnivariateDerivative2 computeLambda(final FieldUnivariateDerivative2 dTpr, + final FieldUnivariateDerivative2 n, + final FieldUnivariateDerivative2 aeop2, + final FieldUnivariateDerivative2 i) { + final FieldUnivariateDerivative2 cosI = FastMath.cos(i); + final FieldUnivariateDerivative2 precession = aeop2.multiply(n).multiply(cosI).multiply(1.5 * GLONASS_J20); + return dTpr.multiply(precession.add(GLONASS_AV)).negate().add(glonassOrbit.getLambda()); + } + + /** + * Computes the current argument of perigee. + * @param dTpr interval of prediction (s) + * @param n mean motion (rad/s) + * @param aeop2 square of the ratio between the radius of the ellipsoid and p, with p = sma * (1 - ecc²) + * @param i inclination (rad) + * @return the current argument of perigee (rad) + */ + private FieldUnivariateDerivative2 computePA(final FieldUnivariateDerivative2 dTpr, + final FieldUnivariateDerivative2 n, + final FieldUnivariateDerivative2 aeop2, + final FieldUnivariateDerivative2 i) { + final FieldUnivariateDerivative2 cosI = FastMath.cos(i); + final FieldUnivariateDerivative2 cos2I = cosI.multiply(cosI); + final FieldUnivariateDerivative2 precession = aeop2.multiply(n).multiply(cos2I.multiply(5.0).negate().add(1.0)).multiply(0.75 * GLONASS_J20); + return dTpr.multiply(precession).negate().add(glonassOrbit.getPa()); + } + + /** + * Computes the differentials δai. + *

+ * The value of i depends of the type of longitude (i = 2 for the current mean longitude; + * i = 1 for the mean longitude at the instant the spacecraft passes the current ascending node) + *

+ * @param a semi-major axis (m) + * @param i inclination (rad) + * @param h x component of the eccentricity (rad) + * @param l y component of the eccentricity (rad) + * @param m longitude (current or at the ascending node instant) + * @return the differentials of the orbital parameters + */ + private FieldUnivariateDerivative2[] getParameterDifferentials(final FieldUnivariateDerivative2 a, final FieldUnivariateDerivative2 i, + final FieldUnivariateDerivative2 h, final FieldUnivariateDerivative2 l, + final FieldUnivariateDerivative2 m) { + + // B constant + final FieldUnivariateDerivative2 aeoa = a.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); + final FieldUnivariateDerivative2 aeoa2 = aeoa.multiply(aeoa); + final FieldUnivariateDerivative2 b = aeoa2.multiply(1.5 * GLONASS_J20); + + // Commons Parameters + final FieldSinCos> scI = FastMath.sinCos(i); + final FieldSinCos> scLk = FastMath.sinCos(m); + final FieldSinCos> sc2Lk = FieldSinCos.sum(scLk, scLk); + final FieldSinCos> sc3Lk = FieldSinCos.sum(scLk, sc2Lk); + final FieldSinCos> sc4Lk = FieldSinCos.sum(sc2Lk, sc2Lk); + final FieldUnivariateDerivative2 cosI = scI.cos(); + final FieldUnivariateDerivative2 sinI = scI.sin(); + final FieldUnivariateDerivative2 cosI2 = cosI.multiply(cosI); + final FieldUnivariateDerivative2 sinI2 = sinI.multiply(sinI); + final FieldUnivariateDerivative2 cosLk = scLk.cos(); + final FieldUnivariateDerivative2 sinLk = scLk.sin(); + final FieldUnivariateDerivative2 cos2Lk = sc2Lk.cos(); + final FieldUnivariateDerivative2 sin2Lk = sc2Lk.sin(); + final FieldUnivariateDerivative2 cos3Lk = sc3Lk.cos(); + final FieldUnivariateDerivative2 sin3Lk = sc3Lk.sin(); + final FieldUnivariateDerivative2 cos4Lk = sc4Lk.cos(); + final FieldUnivariateDerivative2 sin4Lk = sc4Lk.sin(); + + // h*cos(nLk), l*cos(nLk), h*sin(nLk) and l*sin(nLk) + // n = 1 + final FieldUnivariateDerivative2 hCosLk = h.multiply(cosLk); + final FieldUnivariateDerivative2 hSinLk = h.multiply(sinLk); + final FieldUnivariateDerivative2 lCosLk = l.multiply(cosLk); + final FieldUnivariateDerivative2 lSinLk = l.multiply(sinLk); + // n = 2 + final FieldUnivariateDerivative2 hCos2Lk = h.multiply(cos2Lk); + final FieldUnivariateDerivative2 hSin2Lk = h.multiply(sin2Lk); + final FieldUnivariateDerivative2 lCos2Lk = l.multiply(cos2Lk); + final FieldUnivariateDerivative2 lSin2Lk = l.multiply(sin2Lk); + // n = 3 + final FieldUnivariateDerivative2 hCos3Lk = h.multiply(cos3Lk); + final FieldUnivariateDerivative2 hSin3Lk = h.multiply(sin3Lk); + final FieldUnivariateDerivative2 lCos3Lk = l.multiply(cos3Lk); + final FieldUnivariateDerivative2 lSin3Lk = l.multiply(sin3Lk); + // n = 4 + final FieldUnivariateDerivative2 hCos4Lk = h.multiply(cos4Lk); + final FieldUnivariateDerivative2 hSin4Lk = h.multiply(sin4Lk); + final FieldUnivariateDerivative2 lCos4Lk = l.multiply(cos4Lk); + final FieldUnivariateDerivative2 lSin4Lk = l.multiply(sin4Lk); + + // 1 - (3 / 2)*sin²i + final FieldUnivariateDerivative2 om3o2xSinI2 = sinI2.multiply(1.5).negate().add(1.0); + + // Compute Differentials + // δa + final FieldUnivariateDerivative2 dakT1 = b.multiply(2.0).multiply(om3o2xSinI2).multiply(lCosLk.add(hSinLk)); + final FieldUnivariateDerivative2 dakT2 = b.multiply(sinI2).multiply(hSinLk.multiply(0.5).subtract(lCosLk.multiply(0.5)). + add(cos2Lk).add(lCos3Lk.multiply(3.5)).add(hSin3Lk.multiply(3.5))); + final FieldUnivariateDerivative2 dak = dakT1.add(dakT2); + + // δh + final FieldUnivariateDerivative2 dhkT1 = b.multiply(om3o2xSinI2).multiply(sinLk.add(lSin2Lk.multiply(1.5)).subtract(hCos2Lk.multiply(1.5))); + final FieldUnivariateDerivative2 dhkT2 = b.multiply(sinI2).multiply(0.25).multiply(sinLk.subtract(sin3Lk.multiply(SEVEN_THIRD)).add(lSin2Lk.multiply(5.0)). + subtract(lSin4Lk.multiply(8.5)).add(hCos4Lk.multiply(8.5)).add(hCos2Lk)); + final FieldUnivariateDerivative2 dhkT3 = lSin2Lk.multiply(cosI2).multiply(b).multiply(0.5).negate(); + final FieldUnivariateDerivative2 dhk = dhkT1.subtract(dhkT2).add(dhkT3); + + // δl + final FieldUnivariateDerivative2 dlkT1 = b.multiply(om3o2xSinI2).multiply(cosLk.add(lCos2Lk.multiply(1.5)).add(hSin2Lk.multiply(1.5))); + final FieldUnivariateDerivative2 dlkT2 = b.multiply(sinI2).multiply(0.25).multiply(cosLk.negate().subtract(cos3Lk.multiply(SEVEN_THIRD)).subtract(hSin2Lk.multiply(5.0)). + subtract(lCos4Lk.multiply(8.5)).subtract(hSin4Lk.multiply(8.5)).add(lCos2Lk)); + final FieldUnivariateDerivative2 dlkT3 = hSin2Lk.multiply(cosI2).multiply(b).multiply(0.5); + final FieldUnivariateDerivative2 dlk = dlkT1.subtract(dlkT2).add(dlkT3); + + // δλ + final FieldUnivariateDerivative2 dokT1 = b.negate().multiply(cosI); + final FieldUnivariateDerivative2 dokT2 = lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)).subtract(sin2Lk.multiply(0.5)). + subtract(lSin3Lk.multiply(SEVEN_SIXTH)).add(hCos3Lk.multiply(SEVEN_SIXTH)); + final FieldUnivariateDerivative2 dok = dokT1.multiply(dokT2); + + // δi + final FieldUnivariateDerivative2 dik = b.multiply(sinI).multiply(cosI).multiply(0.5). + multiply(lCosLk.negate().add(hSinLk).add(cos2Lk).add(lCos3Lk.multiply(SEVEN_THIRD)).add(hSin3Lk.multiply(SEVEN_THIRD))); + + // δL + final FieldUnivariateDerivative2 dLkT1 = b.multiply(2.0).multiply(om3o2xSinI2).multiply(lSinLk.multiply(1.75).subtract(hCosLk.multiply(1.75))); + final FieldUnivariateDerivative2 dLkT2 = b.multiply(sinI2).multiply(3.0).multiply(hCosLk.multiply(SEVEN_24TH).negate().subtract(lSinLk.multiply(SEVEN_24TH)). + subtract(hCos3Lk.multiply(FN_72TH)).add(lSin3Lk.multiply(FN_72TH)).add(sin2Lk.multiply(0.25))); + final FieldUnivariateDerivative2 dLkT3 = b.multiply(cosI2).multiply(lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)).subtract(sin2Lk.multiply(0.5)). + subtract(lSin3Lk.multiply(SEVEN_SIXTH)).add(hCos3Lk.multiply(SEVEN_SIXTH))); + final FieldUnivariateDerivative2 dLk = dLkT1.add(dLkT2).add(dLkT3); + + // Final array + final FieldUnivariateDerivative2[] differentials = MathArrays.buildArray(a.getField(), 6); + differentials[0] = dak.multiply(a); + differentials[1] = dhk; + differentials[2] = dlk; + differentials[3] = dok; + differentials[4] = dik; + differentials[5] = dLk; + + return differentials; + } + + /** {@inheritDoc} */ + protected double getMass(final AbsoluteDate date) { + return mass; + } + + /** + * Get the Earth gravity coefficient used for GLONASS propagation. + * @return the Earth gravity coefficient. + */ + public T getMU() { + return field.getZero().add(GLONASSOrbitalElements.GLONASS_MU); + } + + /** + * Gets the underlying GLONASS orbital elements. + * + * @return the underlying GLONASS orbital elements + */ + public FieldGLONASSOrbitalElements getGLONASSOrbitalElements() { + return glonassOrbit; + } + + /** + * Gets the Earth Centered Inertial frame used to propagate the orbit. + * @return the ECI frame + */ + public Frame getECI() { + return eci; + } + + /** + * Gets the Earth Centered Earth Fixed frame used to propagate GLONASS orbits. + * @return the ECEF frame + */ + public Frame getECEF() { + return ecef; + } + + /** {@inheritDoc} */ + public Frame getFrame() { + return eci; + } + + /** {@inheritDoc} */ + public void resetInitialState(final SpacecraftState state) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected void resetIntermediateState(final SpacecraftState state, final boolean forward) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date) { + // Gets the PVCoordinates in ECEF frame + final FieldPVCoordinates pvaInECEF = propagateInEcef(date); + // Transforms the PVCoordinates to ECI frame + final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); + // Returns the Cartesian orbit + final T zero = date.getField().getZero(); + return new FieldCartesianOrbit(pvaInECI, eci, date, zero.add(FieldGLONASSOrbitalElements.GLONASS_MU)); + } + + @Override + protected T getMass(FieldAbsoluteDate date) { + // TODO Auto-generated method stub + return null; + } + + @Override + protected void resetIntermediateState(FieldSpacecraftState state, boolean forward) { + // TODO Auto-generated method stub + + } + + @Override + protected FieldOrbit propagateOrbit(FieldAbsoluteDate date, T[] parameters) { + // TODO Auto-generated method stub + return null; + } + + @Override + protected List getParametersDrivers() { + // TODO Auto-generated method stub + return null; + } + + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java new file mode 100644 index 000000000..6674b08a3 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java @@ -0,0 +1,277 @@ +package org.orekit.propagation.analytical.gnss; + +import java.io.Serializable; + +import org.hipparchus.Field; +import org.hipparchus.FieldElement; +import org.hipparchus.RealFieldElement; +import org.hipparchus.util.FastMath; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.data.DataContext; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.DateComponents; +import org.orekit.time.DateTimeComponents; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.FieldTimeStamped; +import org.orekit.time.GLONASSDate; +import org.orekit.time.TimeComponents; +import org.orekit.time.TimeScale; +import org.orekit.utils.Constants; + +public class FieldGLONASSDate> implements Serializable, FieldTimeStamped, Field { + + + /** Serializable UID. */ + private static final long serialVersionUID = 20190131L; + + /** Constant for date computation. */ + private static final int C1 = 44195; + + /** Constant for date computation. */ + private static final int C2 = 45290; + + /** The number of the current day in a four year interval Na. */ + private final int na; + + /** The number of the current four year interval N4. */ + private final int n4; + + /** Number of seconds since Na. */ + private final double secInNa; + + /** Current Julian date JD0. */ + private double jd0; + + /** Greenwich Mean Sidereal Time (rad). */ + private double gmst; + + /** Corresponding date. */ + private final transient FieldAbsoluteDate date; + + /** Build an instance corresponding to a GLONASS date. + * + *

This method uses the {@link DataContext#getDefault() default data context}. + * + * @param na the number of the current day in a four year interval + * @param n4 the number of the current four year interval + * @param secInNa the number of seconds since na start + * @see #GLONASSDate(int, int, double, TimeScale) + */ + @DefaultDataContext + public FieldGLONASSDate(final int na, final int n4, final double secInNa) { + this(na, n4, secInNa, DataContext.getDefault().getTimeScales().getGLONASS()); + } + + /** + * Build an instance corresponding to a GLONASS date. + * + * @param na the number of the current day in a four year interval + * @param n4 the number of the current four year interval + * @param secInNa the number of seconds since na start + * @param glonass time scale. + * @since 10.1 + */ + public FieldGLONASSDate(final int na, + final int n4, + final double secInNa, + final TimeScale glonass) { + this.na = na; + this.n4 = n4; + this.secInNa = secInNa; + // Compute JD0 + final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); + this.jd0 = 1461 * (n4 - 1) + na + 2450082.5 - ratio; + // GMST + this.gmst = computeGMST(); + this.date = computeDate(glonass); + } + + /** Build an instance from an absolute date. + * + *

This method uses the {@link DataContext#getDefault() default data context}. + * + * @param date absolute date to consider + * @see #GLONASSDate(AbsoluteDate, TimeScale) + */ + @DefaultDataContext + public FieldGLONASSDate(final FieldAbsoluteDate date) { + this(date, DataContext.getDefault().getTimeScales().getGLONASS()); + } + + /** + * Build an instance from an absolute date. + * + * @param date absolute date to consider + * @param glonass time scale. + * @since 10.1 + */ + public FieldGLONASSDate(final FieldAbsoluteDate date, final TimeScale glonass) { + final DateTimeComponents dateTime = date.getComponents(glonass); + // N4 + final int year = dateTime.getDate().getYear(); + this.n4 = ((int) (year - 1996) / 4) + 1; + // Na + final int start = 1996 + 4 * (n4 - 1); + + final Field field = null; + final double duration = date.durationFrom(new FieldAbsoluteDate(field, start, 1, 1, glonass)).getReal(); + + this.na = (int) (duration / 86400) + 1; + this.secInNa = dateTime.getTime().getSecondsInLocalDay(); + // Compute JD0 + final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); + this.jd0 = 1461 * (n4 - 1) + na + 2450082.5 - ratio; + // GMST + this.gmst = computeGMST(); + this.date = date; + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + /** Get the number of seconds since Na start. + * @return number of seconds since Na start + */ + public double getSecInDay() { + return secInNa; + } + + /** Get the number of the current day in a four year interval. + * @return the number of the current day in a four year interval + */ + public int getDayNumber() { + return na; + } + + /** Get the number of the current four year interval. + * @return the number of the current four year interval + */ + public int getIntervalNumber() { + return n4; + } + + /** Get the current Julian date JD0. + * @return the current date JD0 + */ + public double getJD0() { + return jd0; + } + + /** Get the Greenwich Mean Sidereal Time. + * @return the Greenwich Mean Sidereal Time (rad) + */ + public double getGMST() { + return gmst; + } + + /** Compute the Greenwich Mean Sidereal Time using the current Julian date JD0. + * @return the Greenwich Mean Sidereal Time (rad) + */ + private double computeGMST() { + final double ref = 2451545.0; + // Earth's rotation angle in radians + final double era = 2. * GLONASSOrbitalElements.GLONASS_PI * + (0.7790572732640 + 1.00273781191135448 * (jd0 - ref)); + // Time from Epoch 2000 (1st January, 00:00 UTC) till current Epoch in Julian centuries + final double time = (jd0 - ref) / Constants.JULIAN_CENTURY; + // Time to the power n + final double time2 = time * time; + final double time3 = time2 * time; + final double time4 = time2 * time2; + final double time5 = time2 * time3; + // GMST computation + final double gTime = era + 7.03270726e-8 + time * 2.23603658710194e-2 + + time2 * 6.7465784654e-6 - time3 * 2.1332e-12 - time4 * 1.452308e-10 - time5 * 1.784e-13; + return gTime; + } + + /** Compute the GLONASS date. + * @return the date + * @param glonass time scale. + */ + private FieldAbsoluteDate computeDate(final TimeScale glonass) { + // Compute the number of Julian day for the current date + final double jdn = jd0 + 0.5; + // Coefficients + final int a = (int) (jdn + 32044); + final int b = (4 * a + 3) / 146097; + final int c = a - (146097 * b) / 4; + final int d = (4 * c + 3) / 1461; + final int e = c - (1461 * d) / 4; + final int m = (5 * e + 2) / 153; + // Year, month and day + final int day = e - (153 * m + 2) / 5 + 1; + final int month = m + 3 - 12 * (m / 10); + final int year = 100 * b + d - 4800 + m / 10; + + + final Field field = null; + return new FieldAbsoluteDate(field, new DateComponents(year, month, day), + new TimeComponents(secInNa), + glonass); + } + + /** Replace the instance with a data transfer object for serialization. + * @return data transfer object that will be serialized + */ + @DefaultDataContext + private Object writeReplace() { + return new DataTransferObject(na, n4, secInNa); + } + + /** Internal class used only for serialization. */ + @DefaultDataContext + private static class DataTransferObject implements Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 20190131L; + + /** The number of the current day in a four year interval Na. */ + private final int na; + + /** The number of the current four year interval N4. */ + private final int n4; + + /** Number of seconds since Na. */ + private final double secInNa; + + /** Simple constructor. + * @param na the number of the current day in a four year interval + * @param n4 the number of the current four year interval + * @param secInNa the number of seconds since na start + */ + DataTransferObject(final int na, final int n4, final double secInNa) { + this.na = na; + this.n4 = n4; + this.secInNa = secInNa; + } + + /** Replace the deserialized data transfer object with a {@link GPSDate}. + * @return replacement {@link GPSDate} + */ + private Object readResolve() { + return new GLONASSDate(na, n4, secInNa); + } + + } + + @Override + public T getZero() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getOne() { + // TODO Auto-generated method stub + return null; + } + + @Override + public Class> getRuntimeClass() { + // TODO Auto-generated method stub + return null; + } +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSOrbitalElements.java new file mode 100644 index 000000000..7cf79b8ad --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSOrbitalElements.java @@ -0,0 +1,195 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; +import org.orekit.gnss.GLONASSAlmanac; +import org.orekit.gnss.GLONASSEphemeris; +import org.orekit.propagation.numerical.GLONASSNumericalPropagator; +import org.orekit.time.FieldTimeStamped; + +/** This interface provides the minimal set of orbital elements needed by the {@link GLONASSAnalyticalPropagator} and + * the {@link GLONASSNumericalPropagator}. + *

+ * Because input data are different between numerical and analytical GLONASS propagators the + * methods present in this interface are implemented by default. + * Depending if the user wants to use a {@link GLONASSNumericalPropagator} or a {@link GLONASSAnalyticalPropagator} + * he can create an instance of a {@link GLONASSEphemeris} or {@link GLONASSAlmanac}. + *

+ * + * @see + * GLONASS Interface Control Document + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + * + */ +public interface FieldGLONASSOrbitalElements> extends FieldTimeStamped { + // Constants + /** Value of the Earth's universal gravitational parameter for GLONASS user in m³/s². */ + double GLONASS_MU = 3.986004418e+14; + + /** Value of Pi for conversion from semicircles to radian. */ + double GLONASS_PI = 3.14159265358979; + + /** + * Get the number of the current day in a four year interval. + * + * @return the number of the current day in a four year interval + */ + int getNa(); + + /** + * Get the number of the current four year interval. + * + * @return the number of the current four year interval + */ + int getN4(); + + /** + * Get the Reference Time. + * + * @return the Reference Time (s) + */ + T getTime(); + + /** + * Get the longitude of ascending node of orbit. + * + * @return the longitude of ascending node of orbit (rad) + */ + T getLambda(); + + /** + * Get the Eccentricity. + * + * @return the Eccentricity + */ + T getE(); + + /** + * Get the Argument of Perigee. + * + * @return the Argument of Perigee (rad) + */ + T getPa(); + + /** + * Get the correction to the mean value of inlination. + * + * @return the correction to the mean value of inlination (rad) + */ + T getDeltaI(); + + /** + * Get the correction to the mean value of Draconian period. + * + * @return the correction to the mean value of Draconian period (s) + */ + T getDeltaT(); + + /** + * Get the rate of change of Draconian period. + * + * @return the rate of change of Draconian period + */ + T getDeltaTDot(); + + /** + * Get the relative deviation of predicted satellite carrier frequency from nominal value. + * + * @return the relative deviation of predicted satellite carrier frequency from nominal value + */ + T getGammaN() ; + + /** + * Get the correction to the satellite time relative to GLONASS system time. + * + * @return the correction to the satellite time relative to GLONASS system time (s) + * + */ + T getTN(); + + /** + * Get the ECEF-X component of satellite velocity vector in PZ-90 datum. + * + * @return the the ECEF-X component of satellite velocity vector in PZ-90 datum (m/s) + */ + T getXDot(); + + /** + * Get the ECEF-X component of satellite coordinates in PZ-90 datum. + * + * @return the ECEF-X component of satellite coordinates in PZ-90 datum (m) + */ + T getX(); + /** + * Get the GLONASS ECEF-X component of satellite acceleration vector in PZ-90 datum. + * + * @return the GLONASS ECEF-X component of satellite acceleration vector in PZ-90 datum (m/s²) + */ + T getXDotDot(); + + /** + * Get the ECEF-Y component of satellite velocity vector in PZ-90 datum. + * + * @return the ECEF-Y component of satellite velocity vector in PZ-90 datum (m/s) + */ + T getYDot(); + + /** + * Get the ECEF-Y component of satellite coordinates in PZ-90 datum. + * + * @return the ECEF-Y component of satellite coordinates in PZ-90 datum (m) + */ + T getY(); + + /** + * Get the GLONASS ECEF-Y component of satellite acceleration vector in PZ-90 datum. + * + * @return the GLONASS ECEF-Y component of satellite acceleration vector in PZ-90 datum (m/s²) + */ + T getYDotDot(); + + /** + * Get the ECEF-Z component of satellite velocity vector in PZ-90 datum. + * + * @return the the ECEF-Z component of satellite velocity vector in PZ-90 datum (m/s) + */ + T getZDot(); + + /** + * Get the ECEF-Z component of satellite coordinates in PZ-90 datum. + * + * @return the ECEF-Z component of satellite coordinates in PZ-90 datum (m) + */ + T getZ(); + + /** + * Get the GLONASS ECEF-Z component of satellite acceleration vector in PZ-90 datum. + * + * @return the GLONASS ECEF-Z component of satellite acceleration vector in PZ-90 datum (m/s²) + */ + T getZDotDot(); + + /** + * Gets the GLONASS Issue Of Data (IOD). + * + * @return the IOD + */ + int getIOD(); +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSDate.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSDate.java new file mode 100644 index 000000000..cc6fb2922 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSDate.java @@ -0,0 +1,435 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import java.io.Serializable; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.concurrent.atomic.AtomicReference; + +import org.hipparchus.RealFieldElement; +import org.hipparchus.util.FastMath; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.data.DataContext; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.EOPEntry; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.DateComponents; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.FieldTimeStamped; +import org.orekit.time.GNSSDate; +import org.orekit.time.TimeComponents; +import org.orekit.time.TimeScale; +import org.orekit.time.TimeScales; +import org.orekit.time.UT1Scale; +import org.orekit.utils.Constants; +import org.orekit.utils.IERSConventions; + +/** Container for date in GNSS form. + *

This class can be used to handle {@link SatelliteSystem#GPS GPS}, + * {@link SatelliteSystem#GALILEO Galileo}, {@link SatelliteSystem#BEIDOU BeiDou} + * and {@link SatelliteSystem#QZSS QZSS} dates.

+ * @author Luc Maisonobe (original code) + * @author Bryan Cazabonne (generalization to all GNSS constellations) + * @author Nicolas Fialton (field translation) + */ +public class FieldGNSSDate> implements Serializable, FieldTimeStamped { + + /** Serializable UID. */ + private static final long serialVersionUID = 201902141L; + + /** Duration of a week in days. */ + private static final int WEEK_D = 7; + + /** Duration of a week in seconds. */ + private static final double WEEK_S = WEEK_D * Constants.JULIAN_DAY; + + /** Conversion factor from seconds to milliseconds. */ + private static final double S_TO_MS = 1000.0; + + /** Reference date for ensuring continuity across GNSS week rollover. + * @since 9.3.1 + */ + private static AtomicReference rolloverReference = new AtomicReference(null); + + /** Week number since the GNSS reference epoch. */ + private final int weekNumber; + + /** Number of milliseconds since week start. */ + private final T milliInWeek; + + /** Satellite system to consider. */ + private final SatelliteSystem system; + + /** Corresponding date. */ + private final transient FieldAbsoluteDate date; + + /** Build an instance corresponding to a GNSS date. + *

+ * GNSS dates are provided as a week number starting at + * the GNSS reference epoch and as a number of milliseconds + * since week start. + *

+ *

+ * Many interfaces provide week number modulo the constellation week cycle. In order to cope with + * this, when the week number is smaller than the week cycle, this constructor assumes a modulo operation + * has been performed and it will fix the week number according to the reference date set up for + * handling rollover (see {@link #setRolloverReference(DateComponents) setRolloverReference(reference)}). + * If the week number is equal to the week cycle or larger, it will be used without any correction. + *

+ * + *

This method uses the {@link DataContext#getDefault() default data context}. + * + * @param weekNumber week number + * @param milliInWeek number of milliseconds since week start + * @param system satellite system to consider + * @see #GNSSDate(int, double, SatelliteSystem, TimeScales) + */ + @DefaultDataContext + public FieldGNSSDate(final int weekNumber, final T milliInWeek, + final SatelliteSystem system) { + this(weekNumber, milliInWeek, system, DataContext.getDefault().getTimeScales()); + } + + /** + * Build an instance corresponding to a GNSS date. + *

+ * GNSS dates are provided as a week number starting at the GNSS reference epoch and + * as a number of milliseconds since week start. + *

+ *

+ * Many interfaces provide week number modulo the constellation week cycle. In order + * to cope with this, when the week number is smaller than the week cycle, this + * constructor assumes a modulo operation has been performed and it will fix the week + * number according to the reference date set up for handling rollover (see {@link + * #setRolloverReference(DateComponents) setRolloverReference(reference)}). If the + * week number is equal to the week cycle or larger, it will be used without any + * correction. + *

+ * + * @param weekNumber week number + * @param milliInWeek number of milliseconds since week start + * @param system satellite system to consider + * @param timeScales the set of time scales. Used to retrieve the appropriate time + * scale for the given {@code system}. + * @since 10.1 + */ + public FieldGNSSDate(final int weekNumber, + final T milliInWeek, + final SatelliteSystem system, + final TimeScales timeScales) { + + final int day = (int) FastMath.floor(milliInWeek.getReal() / (Constants.JULIAN_DAY * S_TO_MS)); + final double secondsInDay = milliInWeek.getReal() / S_TO_MS - day * Constants.JULIAN_DAY; + + int w = weekNumber; + DateComponents dc = new DateComponents(getWeekReferenceDateComponents(system), weekNumber * 7 + day); + final int cycleW = GNSSDateType.getRollOverWeek(system); + if (weekNumber < cycleW) { + + DateComponents reference = rolloverReference.get(); + if (reference == null) { + // lazy setting of a default reference, using end of EOP entries + final UT1Scale ut1 = timeScales.getUT1(IERSConventions.IERS_2010, true); + final List eop = ut1.getEOPHistory().getEntries(); + final int lastMJD = eop.get(eop.size() - 1).getMjd(); + reference = new DateComponents(DateComponents.MODIFIED_JULIAN_EPOCH, lastMJD); + rolloverReference.compareAndSet(null, reference); + } + + // fix GNSS week rollover + final int cycleD = WEEK_D * cycleW; + while (dc.getJ2000Day() < reference.getJ2000Day() - cycleD / 2) { + dc = new DateComponents(dc, cycleD); + w += cycleW; + } + + } + + this.weekNumber = w; + this.milliInWeek = milliInWeek; + this.system = system; + + date = new FieldAbsoluteDate(milliInWeek.getField(), dc, new TimeComponents(secondsInDay), getTimeScale(system, timeScales)); + + } + + /** Build an instance from an absolute date. + * + *

This method uses the {@link DataContext#getDefault() default data context}. + * + * @param date absolute date to consider + * @param system satellite system to consider + * @see #GNSSDate(AbsoluteDate, SatelliteSystem, TimeScales) + */ + @DefaultDataContext + public FieldGNSSDate(final FieldAbsoluteDate date, final SatelliteSystem system) { + this(date, system, DataContext.getDefault().getTimeScales()); + } + + /** + * Build an instance from an absolute date. + * + * @param date absolute date to consider + * @param system satellite system to consider + * @param timeScales the set of time scales. Used to retrieve the appropriate time + * scale for the given {@code system}. + * @since 10.1 + */ + public FieldGNSSDate(final FieldAbsoluteDate date, + final SatelliteSystem system, + final TimeScales timeScales) { + + this.system = system; + final AbsoluteDate epoch = getWeekReferenceAbsoluteDate(system, timeScales); + this.weekNumber = (int) FastMath.floor(date.durationFrom(epoch).divide(WEEK_S).getReal()); + final AbsoluteDate weekStart = new AbsoluteDate(epoch, WEEK_S * weekNumber); + this.milliInWeek = date.durationFrom(weekStart).multiply(S_TO_MS); + this.date = date; + + } + + /** Set a reference date for ensuring continuity across GNSS week rollover. + *

+ * Instance created using the {@link #GNSSDate(int, double, SatelliteSystem) GNSSDate(weekNumber, milliInWeek, system)} + * constructor and with a week number between 0 and the constellation week cycle (cycleW) after this method has been called will + * fix the week number to ensure they correspond to dates between {@code reference - cycleW / 2 weeks} + * and {@code reference + cycleW / 2 weeks}. + *

+ *

+ * If this method is never called, a default reference date for rollover will be set using + * the date of the last known EOP entry retrieved from {@link UT1Scale#getEOPHistory() UT1} + * time scale. + *

+ * @param reference reference date for GNSS week rollover + * @see #getRolloverReference() + * @see #GNSSDate(int, double, SatelliteSystem) + * @since 9.3.1 + */ + public static void setRolloverReference(final DateComponents reference) { + rolloverReference.set(reference); + } + + /** Get the reference date ensuring continuity across GNSS week rollover. + * @return reference reference date for GNSS week rollover + * @see #setRolloverReference(DateComponents) + * @see #GNSSDate(int, double, SatelliteSystem) + * @since 9.3.1 + */ + public static DateComponents getRolloverReference() { + return rolloverReference.get(); + } + + /** Get the week number since the GNSS reference epoch. + *

+ * The week number returned here has been fixed for GNSS week rollover, i.e. + * it may be larger than the corresponding week cycle of the constellation. + *

+ * @return week number since since the GNSS reference epoch + */ + public int getWeekNumber() { + return weekNumber; + } + + /** Get the number of milliseconds since week start. + * @return number of milliseconds since week start + */ + public T getMilliInWeek() { + return milliInWeek; + } + + /** {@inheritDoc} */ + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + /** Get the time scale related to the given satellite system. + * @param satellite satellite system + * @param timeScales set of time scales. + * @return the time scale + */ + private TimeScale getTimeScale(final SatelliteSystem satellite, + final TimeScales timeScales) { + switch (satellite) { + case GPS : return timeScales.getGPS(); + case GALILEO : return timeScales.getGST(); + case QZSS : return timeScales.getQZSS(); + case BEIDOU : return timeScales.getBDT(); + case IRNSS : return timeScales.getIRNSS(); + case SBAS : return timeScales.getGPS(); + default : throw new OrekitException(OrekitMessages.INVALID_SATELLITE_SYSTEM, satellite); + } + } + + /** Get the reference epoch of the week number for the given satellite system. + *

Returned parameter is an AbsoluteDate.

+ * @param satellite satellite system + * @param timeScales set of time scales. + * @return the reference epoch + */ + private AbsoluteDate getWeekReferenceAbsoluteDate(final SatelliteSystem satellite, + final TimeScales timeScales) { + switch (satellite) { + case GPS : return timeScales.getGpsEpoch(); + case GALILEO : return timeScales.getGalileoEpoch(); + case QZSS : return timeScales.getQzssEpoch(); + case BEIDOU : return timeScales.getBeidouEpoch(); + case IRNSS : return timeScales.getIrnssEpoch(); + case SBAS : return timeScales.getGpsEpoch(); + default : throw new OrekitException(OrekitMessages.INVALID_SATELLITE_SYSTEM, satellite); + } + } + + /** Get the reference epoch of the week number for the given satellite system. + *

Returned parameter is a DateComponents.

+ * @param satellite satellite system + * @return the reference epoch + */ + private DateComponents getWeekReferenceDateComponents(final SatelliteSystem satellite) { + switch (satellite) { + case GPS : return DateComponents.GPS_EPOCH; + case GALILEO : return DateComponents.GALILEO_EPOCH; + case QZSS : return DateComponents.QZSS_EPOCH; + case BEIDOU : return DateComponents.BEIDOU_EPOCH; + case IRNSS : return DateComponents.IRNSS_EPOCH; + case SBAS : return DateComponents.GPS_EPOCH; + default : throw new OrekitException(OrekitMessages.INVALID_SATELLITE_SYSTEM, satellite); + } + } + + /** Replace the instance with a data transfer object for serialization. + * @return data transfer object that will be serialized + */ + @DefaultDataContext + private Object writeReplace() { + return new DataTransferObject(weekNumber, milliInWeek, system); + } + + /** Internal class used only for serialization. */ + @DefaultDataContext + private class DataTransferObject implements Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 201902141L; + + /** Week number since the GNSS reference epoch. */ + private final int weekNumber; + + /** Number of milliseconds since week start. */ + private final T milliInWeek; + + /** Satellite system to consider. */ + private final SatelliteSystem system; + + /** Simple constructor. + * @param weekNumber week number since the GNSS reference epoch + * @param milliInWeek number of milliseconds since week start + * @param system satellite system to consider + */ + DataTransferObject(final int weekNumber, final T milliInWeek, + final SatelliteSystem system) { + this.weekNumber = weekNumber; + this.milliInWeek = milliInWeek; + this.system = system; + } + + /** Replace the deserialized data transfer object with a {@link GNSSDate}. + * @return replacement {@link GNSSDate} + */ + private Object readResolve() { + return new FieldGNSSDate(weekNumber, milliInWeek, system, null); + } + + } + + /** Enumerate for GNSS data. */ + private enum GNSSDateType { + + /** GPS. */ + GPS(SatelliteSystem.GPS, 1024), + + /** Galileo. */ + GALILEO(SatelliteSystem.GALILEO, 4096), + + /** QZSS. */ + QZSS(SatelliteSystem.QZSS, 1024), + + /** BeiDou. */ + BEIDOU(SatelliteSystem.BEIDOU, 8192), + + /** IRNSS. */ + IRNSS(SatelliteSystem.IRNSS, 1024), + + /** SBAS. */ + SBAS(SatelliteSystem.SBAS, 1024); + + /** Map for the number of week in one GNSS rollover cycle. */ + private static final Map CYCLE_MAP = new HashMap(); + static { + for (final GNSSDateType type : values()) { + final int val = type.getRollOverCycle(); + final SatelliteSystem satellite = type.getSatelliteSystem(); + CYCLE_MAP.put(satellite, val); + } + } + + /** Number of week in one rollover cycle. */ + private final int numberOfWeek; + + /** Satellite system. */ + private final SatelliteSystem satelliteSystem; + + /** + * Build a new instance. + * + * @param system satellite system + * @param rollover number of week in one rollover cycle + */ + GNSSDateType(final SatelliteSystem system, final int rollover) { + this.satelliteSystem = system; + this.numberOfWeek = rollover; + } + + /** Get the number of week in one rollover cycle. + * @return the number of week in one rollover cycle + */ + private int getRollOverCycle() { + return numberOfWeek; + } + + /** Get the satellite system. + * @return the satellite system + */ + private SatelliteSystem getSatelliteSystem() { + return satelliteSystem; + } + + /** Get the number of week in one rollover cycle for the given satellite system. + * + * @param satellite satellite system + * @return the number of week in one rollover cycle for the given satellite system + */ + private static int getRollOverWeek(final SatelliteSystem satellite) { + return CYCLE_MAP.get(satellite); + } + + } +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java new file mode 100644 index 000000000..1ae26e458 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java @@ -0,0 +1,183 @@ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; +import org.orekit.time.FieldTimeStamped; + +/** + * This interface provides the minimal set of orbital elements needed by the {@link FieldAbstractGNSSPropagator}. + * @author nfialton + * + * @param + */ +public interface FieldGNSSOrbitalElements> extends FieldTimeStamped { + + + /** + * Gets the PRN number of the GNSS satellite. + * + * @return the PRN number of the GNSS satellite + */ + int getPRN(); + + /** + * Gets the Reference Week of the GNSS orbit. + * + * @return the Reference Week of the GNSS orbit within [0, 1024[ + */ + int getWeek(); + + /** + * Gets the Reference Time of the GNSS orbit as a duration from week start. + * + * @return the Reference Time of the GNSS orbit (s) + */ + T getTime(); + + /** + * Gets the Semi-Major Axis. + * + * @return the Semi-Major Axis (m) + */ + T getSma(); + + /** + * Gets the Mean Motion. + * + * @return the Mean Motion (rad/s) + */ + T getMeanMotion(); + + /** + * Gets the Eccentricity. + * + * @return the Eccentricity + */ + T getE(); + + /** + * Gets the Inclination Angle at Reference Time. + * + * @return the Inclination Angle at Reference Time (rad) + */ + T getI0(); + + /** + * Gets the Rate of Inclination Angle. + * + * @return the Rate of Inclination Angle (rad/s) + */ + T getIDot(); + + /** + * Gets the Longitude of Ascending Node of Orbit Plane at Weekly Epoch. + * + * @return the Longitude of Ascending Node of Orbit Plane at Weekly Epoch (rad) + */ + T getOmega0(); + + /** + * Gets the Rate of Right Ascension. + * + * @return the Rate of Right Ascension (rad/s) + */ + T getOmegaDot(); + + /** + * Gets the Argument of Perigee. + * + * @return the Argument of Perigee (rad) + */ + T getPa(); + + /** + * Gets the Mean Anomaly at Reference Time. + * + * @return the Mean Anomaly at Reference Time (rad) + */ + T getM0(); + + /** + * Gets the Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude. + * + * @return the Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude (rad) + */ + T getCuc(); + + /** + * Gets the Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude. + * + * @return the Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude (rad) + */ + T getCus(); + + /** + * Gets the Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius. + * + * @return the Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius (m) + */ + T getCrc(); + + /** + * Gets the Amplitude of the Sine Harmonic Correction Term to the Orbit Radius. + * + * @return the Amplitude of the Sine Harmonic Correction Term to the Orbit Radius (m) + */ + T getCrs(); + + /** + * Gets the Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination. + * + * @return the Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination (rad) + */ + T getCic(); + + /** + * Gets the Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination. + * + * @return the Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination (rad) + */ + T getCis(); + + /** + * Gets the Zeroth Order Clock Correction. + * + * @return the Zeroth Order Clock Correction (s) + * @see #getAf1() + * @see #getAf2() + * @see #getToc() + */ + T getAf0(); + + /** + * Gets the First Order Clock Correction. + * + * @return the First Order Clock Correction (s/s) + * @see #getAf0() + * @see #getAf2() + * @see #getToc() + * @since 9.3 + */ + T getAf1(); + /** + * Gets the Second Order Clock Correction. + * + * @return the Second Order Clock Correction (s/s²) + * @see #getAf0() + * @see #getAf1() + * @see #getToc() + * @since 9.3 + */ + T getAf2(); + + /** + * Gets the clock correction reference time toc. + * + * @return the clock correction reference time (s) + * @see #getAf0() + * @see #getAf1() + * @see #getAf2() + * @since 9.3 + */ + T getToc(); + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java new file mode 100644 index 000000000..80f92068d --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java @@ -0,0 +1,381 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.util.FastMath; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.data.DataContext; +import org.orekit.gnss.GPSAlmanac; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.GNSSDate; + +/** + * This class holds a GPS almanac as read from SEM or YUMA files. + * + *

+ * Depending on the source (SEM or YUMA), some fields may be filled in or not. + * An almanac read from a YUMA file doesn't hold SVN number, average URA and + * satellite configuration. + *

+ * + * @author Pascal Parraud + * @author Nicolas Fialton (field translation) + * + */ +public class FieldGPSAlmanac> implements FieldGPSOrbitalElements { + + private final T zero; + // Fields + /** Source of the almanac. */ + private final String src; + /** PRN number. */ + private final int prn; + /** SVN number. */ + private final int svn; + /** Health status. */ + private final int health; + /** Average URA. */ + private final int ura; + /** Satellite configuration. */ + private final int config; + /** GPS week. */ + private final int week; + /** Time of applicability. */ + private final T toa; + /** Semi-major axis. */ + private final T sma; + /** Eccentricity. */ + private final T ecc; + /** Inclination. */ + private final T inc; + /** Longitude of Orbital Plane. */ + private final T om0; + /** Rate of Right Ascension. */ + private final T dom; + /** Argument of perigee. */ + private final T aop; + /** Mean anomaly. */ + private final T anom; + /** Zeroth order clock correction. */ + private final T af0; + /** First order clock correction. */ + private final T af1; + /** Date of validity. */ + private final FieldAbsoluteDate date; + + /** + * Constructor. + * + *

+ * This method uses the {@link DataContext#getDefault() default data context}. + * + * @param source the source of the almanac (SEM, YUMA, user defined) + * @param prn the PRN number + * @param svn the SVN number + * @param week the GPS week + * @param toa the Time of Applicability + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc the inclination (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @param ura the average URA + * @param config the satellite configuration + * @see #FieldGPSAlmanac(String, int, int, int, T, T, T, T, T, T, T, T, T, T, + * int, int, int, FieldAbsoluteDate) + */ + @DefaultDataContext + public FieldGPSAlmanac(final String source, final int prn, final int svn, final int week, final T toa, final T sqa, + final T ecc, final T inc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, + final int health, final int ura, final int config) { + this(source, prn, svn, week, toa, sqa, ecc, inc, om0, dom, aop, anom, af0, af1, health, ura, config, + new FieldGNSSDate<>(week, toa.multiply(1000), SatelliteSystem.GPS, + DataContext.getDefault().getTimeScales()).getDate()); + } + + /** + * Constructor. + * + * @param source the source of the almanac (SEM, YUMA, user defined) + * @param prn the PRN number + * @param svn the SVN number + * @param week the GPS week + * @param toa the Time of Applicability + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc the inclination (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @param ura the average URA + * @param config the satellite configuration + * @param date built from the {@code week} and {@code toa}: + * {@code new GNSSDate(week, + * toa * 1000., SatelliteSystem.GPS, timeScales).getDate()} + * @since 10.1 + */ + public FieldGPSAlmanac(final String source, final int prn, final int svn, final int week, final T toa, final T sqa, + final T ecc, final T inc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, + final int health, final int ura, final int config, final FieldAbsoluteDate date) { + this.zero = toa.getField().getZero(); + this.src = source; + this.prn = prn; + this.svn = svn; + this.week = week; + this.toa = toa; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.inc = inc; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.af0 = af0; + this.af1 = af1; + this.health = health; + this.ura = ura; + this.config = config; + this.date = date; + } + + /** + * Constructor + * + * @param field + * @param almanac + */ + public FieldGPSAlmanac(Field field, GPSAlmanac almanac) { + this.zero = field.getZero(); + this.src = almanac.getSource(); + this.prn = almanac.getPRN(); + this.svn = almanac.getSVN(); + this.week = almanac.getWeek(); + this.toa = zero.add(almanac.getTime()); + this.sma = zero.add(almanac.getSma()); + this.ecc = zero.add(almanac.getE()); + this.inc = zero.add(almanac.getI0()); + this.om0 = zero.add(almanac.getOmega0()); + this.dom = zero.add(almanac.getOmegaDot()); + this.aop = zero.add(almanac.getPa()); + this.anom = zero.add(almanac.getM0()); + this.af0 = zero.add(almanac.getAf0()); + this.af1 = zero.add(almanac.getAf1()); + this.health = almanac.getHealth(); + this.ura = almanac.getURA(); + this.config = almanac.getSatConfiguration(); + this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + /** + * Gets the source of this GPS almanac. + *

+ * Sources can be SEM or YUMA, when the almanac is read from a file. + *

+ * + * @return the source of this GPS almanac + */ + public String getSource() { + return src; + } + + @Override + public int getPRN() { + return prn; + } + + /** + * Gets the satellite "SVN" reference number. + * + * @return the satellite "SVN" reference number + */ + public int getSVN() { + return svn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toa; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(absA.getField().getZero().add(GPS_MU).divide(absA)).divide(absA); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return zero; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return zero; + } + + @Override + public T getCus() { + return zero; + } + + @Override + public T getCrc() { + return zero; + } + + @Override + public T getCrs() { + return zero; + } + + @Override + public T getCic() { + return zero; + } + + @Override + public T getCis() { + return zero; + } + + @Override + public T getAf0() { + return af0; + } + + @Override + public T getAf1() { + return af1; + } + + /** + * Gets the Health status. + * + * @return the Health status + */ + public int getHealth() { + return health; + } + + /** + * Gets the average URA number. + * + * @return the average URA number + */ + public int getURA() { + return ura; + } + + /** + * Gets the satellite configuration. + * + * @return the satellite configuration + */ + public int getSatConfiguration() { + return config; + } + + @Override + public T getAf2() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public int getIODC() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public int getIODE() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getTGD() { + // TODO Auto-generated method stub + return null; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java new file mode 100644 index 000000000..c04e01dc4 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java @@ -0,0 +1,67 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; + +/** This interface provides the minimal set of orbital elements needed by the {@link GPSPropagator}. +* +* @see GPS Interface Specification +* @author Pascal Parraud +* @author Nicolas Fialton (field translation) +*/ + +public interface FieldGPSOrbitalElements> extends FieldGNSSOrbitalElements { + + // Constants + /** WGS 84 value of the Earth's universal gravitational parameter for GPS user in m³/s². */ + double GPS_MU = 3.986005e+14; + + /** Value of Pi for conversion from semicircles to radian. */ + double GPS_PI = 3.1415926535898; + + /** Duration of the GPS week in seconds. */ + double GPS_WEEK_IN_SECONDS = 604800.; + + /** Number of weeks in the GPS cycle. */ + int GPS_WEEK_NB = 1024; + + /** + * Gets the Issue Of Data Clock (IODC). + * + * @return the Issue Of Data Clock (IODC) + * @since 9.3 + */ + int getIODC(); + + /** + * Gets the Issue Of Data Ephemeris (IODE). + * + * @return the Issue Of Data Ephemeris (IODE) + * @since 9.3 + */ + int getIODE(); + + /** + * Gets the estimated group delay differential TGD for L1-L2 correction. + * + * @return the estimated group delay differential TGD for L1-L2 correction (s) + * @since 9.3 + */ + T getTGD(); + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java new file mode 100644 index 000000000..e8427ff8f --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java @@ -0,0 +1,99 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.data.DataContext; +import org.orekit.frames.Frame; +import org.orekit.frames.Frames; +import org.orekit.propagation.Propagator; +import org.orekit.utils.IERSConventions; + +/** + * This class aims at propagating a GPS orbit from {@link GPSOrbitalElements}. + * + * @see GPS + * Interface Specification + * @author Pascal Parraud + * @author Nicolas Fialton (field translation) + */ +public class FieldGPSPropagator> extends FieldAbstractGNSSPropagator { + + // Constants + /** WGS 84 value of the earth's rotation rate in rad/s. */ + private static final double GPS_AV = 7.2921151467e-5; + + /** Duration of the GPS cycle in seconds. */ + private static final double GPS_CYCLE_DURATION = FieldGPSOrbitalElements.GPS_WEEK_IN_SECONDS + * FieldGPSOrbitalElements.GPS_WEEK_NB; + + /** The GPS orbital elements used. */ + private final FieldGPSOrbitalElements gpsOrbit; + + /** + * Default constructor. + * + * @param field + * @param gpsOrbit + */ + @DefaultDataContext + public FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit) { + this(field, gpsOrbit, DataContext.getDefault().getFrames()); + } + + /** + * Constructor. + * + * @param field + * @param gpsOrbit + * @param frames + */ + public FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, final Frames frames) { + this(field, gpsOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), + frames.getITRF(IERSConventions.IERS_2010, true)); + } + + /** + * Constructor. + * + * @param field + * @param gpsOrbit + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { + super(field, gpsOrbit, attitudeProvider, eci, ecef, mass, GPS_AV, GPS_CYCLE_DURATION, + FieldGPSOrbitalElements.GPS_MU); + // Stores the GPS orbital elements + this.gpsOrbit = gpsOrbit; + } + + /** + * Get the underlying GPS orbital elements. + * + * @return the underlying GPS orbital elements + */ + public FieldGPSOrbitalElements getFieldGPSOrbitalElements() { + return gpsOrbit; + } +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java new file mode 100644 index 000000000..eab9b66f3 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java @@ -0,0 +1,367 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; +import org.hipparchus.util.FastMath; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.data.DataContext; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.FieldAbsoluteDate; + +/** + * Class for Galileo almanac. + * + * @see "European GNSS (Galileo) Open Service, Signal In Space, Interface + * Control Document, Table 75" + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + * + */ +public class FieldGalileoAlmanac> implements FieldGalileoOrbitalElements { + + private final T zero; + + // Nominal parameters + /** Nominal inclination (Ref: Galileo ICD - Table 75). */ + private final static double I0 = FastMath.toRadians(56.0); + + /** Nominal semi-major axis in meters (Ref: Galileo ICD - Table 75). */ + private final static double A0 = 29600000; + + /** PRN number. */ + private final int prn; + + /** Satellite E5a signal health status. */ + private final int healthE5a; + + /** Satellite E5b signal health status. */ + private final int healthE5b; + + /** Satellite E1-B/C signal health status. */ + private final int healthE1; + + /** Galileo week. */ + private final int week; + + /** Time of applicability. */ + private final T toa; + + /** Semi-major axis. */ + private final T sma; + + /** Eccentricity. */ + private final T ecc; + + /** Inclination. */ + private final T inc; + + /** Longitude of Orbital Plane. */ + private final T om0; + + /** Rate of Right Ascension. */ + private final T dom; + + /** Argument of perigee. */ + private final T aop; + + /** Mean anomaly. */ + private final T anom; + + /** Zeroth order clock correction. */ + private final T af0; + + /** First order clock correction. */ + private final T af1; + + /** Almanac Issue Of Data. */ + private final int iod; + + private final FieldAbsoluteDate date; + + + /** + * Build a new almanac. + * + *

This method uses the {@link DataContext#getDefault() default data context}. + * + * @param prn the PRN number + * @param week the Galileo week + * @param toa the Almanac Time of Applicability (s) + * @param dsqa difference between the square root of the semi-major axis + * and the square root of the nominal semi-major axis + * @param ecc the eccentricity + * @param dinc the correction of orbit reference inclination at reference time (rad) + * @param iod the issue of data + * @param om0 the geographic longitude of the orbital plane at the weekly epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param healthE5a the E5a signal health status + * @param healthE5b the E5b signal health status + * @param healthE1 the E1-B/C signal health status + * @see #GalileoAlmanac(int, int, T, T, T, T, int, T, T, + * T, T, T, T, int, int, int, AbsoluteDate) + */ + @DefaultDataContext + public FieldGalileoAlmanac(final int prn, final int week, final T toa, + final T dsqa, final T ecc, final T dinc, + final int iod, final T om0, final T dom, + final T aop, final T anom, final T af0, + final T af1, final int healthE5a, final int healthE5b, + final int healthE1) { + this(prn, week, toa, dsqa, ecc, dinc, iod, om0, dom, aop, anom, af0, af1, + healthE5a, healthE5b, healthE1, + new FieldGNSSDate<>(week, toa.multiply(1000), SatelliteSystem.GALILEO, + DataContext.getDefault().getTimeScales()).getDate()); + } + + /** + * Build a new almanac. + * + * @param prn the PRN number + * @param week the Galileo week + * @param toa the Almanac Time of Applicability (s) + * @param dsqa difference between the square root of the semi-major axis + * and the square root of the nominal semi-major axis + * @param ecc the eccentricity + * @param dinc the correction of orbit reference inclination at reference time (rad) + * @param iod the issue of data + * @param om0 the geographic longitude of the orbital plane at the weekly epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param healthE5a the E5a signal health status + * @param healthE5b the E5b signal health status + * @param healthE1 the E1-B/C signal health status + * @param date corresponding to {@code week} and {@code toa}. + * @since 10.1 + */ + public FieldGalileoAlmanac(final int prn, final int week, final T toa, + final T dsqa, final T ecc, final T dinc, + final int iod, final T om0, final T dom, + final T aop, final T anom, final T af0, + final T af1, final int healthE5a, final int healthE5b, + final int healthE1, final FieldAbsoluteDate date) { + this.zero = toa.getField().getZero(); + this.prn = prn; + this.week = week; + this.toa = toa; + this.ecc = ecc; + this.inc = dinc.add(I0); + this.iod = iod; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.af0 = af0; + this.af1 = af1; + this.healthE1 = healthE1; + this.healthE5a = healthE5a; + this.healthE5b = healthE5b; + this.date = date; + + // semi-major axis computation + final T sqa = dsqa.add(FastMath.sqrt(A0)); + this.sma = sqa.multiply(sqa); + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toa; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(zero.add(GALILEO_MU).divide(absA)).divide(absA); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return zero; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return zero; + } + + @Override + public T getCus() { + return zero; + } + + @Override + public T getCrc() { + return zero; + } + + @Override + public T getCrs() { + return zero; + } + + @Override + public T getCic() { + return zero; + } + + @Override + public T getCis() { + return zero; + } + + @Override + public T getAf0() { + return af0; + } + + @Override + public T getAf1() { + return af1; + } + + /** + * Get the Issue of Data (IOD). + * + * @return the Issue Of Data + */ + public int getIOD() { + return iod; + } + + /** + * Gets the E1-B/C signal health status. + * + * @return the E1-B/C signal health status + */ + public int getHealthE1() { + return healthE1; + } + + /** + * Gets the E5a signal health status. + * + * @return the E5a signal health status + */ + public int getHealthE5a() { + return healthE5a; + } + + /** + * Gets the E5b signal health status. + * + * @return the E5b signal health status + */ + public int getHealthE5b() { + return healthE5b; + } + + @Override + public T getAf2() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public int getIODNav() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getBGD() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getBGDE1E5a() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getBGDE5bE1() { + // TODO Auto-generated method stub + return null; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java new file mode 100644 index 000000000..ed56f2a27 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java @@ -0,0 +1,57 @@ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; + +/** This interface provides the minimal set of orbital elements needed by the {@link GalileoPropagator}. +* +* @see +* Galileo Interface Control Document +* +* @author Bryan Cazabonne +* @author Nicolas Fialton (field translation) +* +*/ + +public interface FieldGalileoOrbitalElements> extends FieldGNSSOrbitalElements { + + // Constants + /** Earth's universal gravitational parameter for Galileo user in m³/s². */ + double GALILEO_MU = 3.986004418e+14; + + /** Value of Pi for conversion from semicircles to radian. */ + double GALILEO_PI = 3.1415926535898; + + /** Duration of the Galileo week in seconds. */ + double GALILEO_WEEK_IN_SECONDS = 604800.; + + /** Number of weeks in the Galileo cycle. */ + int GALILEO_WEEK_NB = 4096; + + /** + * Gets the Issue Of Data (IOD). + * + * @return the Issue Of Data (IOD) + */ + int getIODNav(); + + /** + * Gets the estimated broadcast group delay differential. + * + * @return the estimated broadcast group delay differential(s) + */ + T getBGD(); + + /** + * Gets the E1/E5a broadcast group delay. + * + * @return the E1/E5a broadcast group delay (s) + */ + T getBGDE1E5a(); + + /** + * Gets the Broadcast Group Delay E5b/E1. + * + * @return the Broadcast Group Delay E5b/E1 (s) + */ + T getBGDE5bE1(); +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java new file mode 100644 index 000000000..508a0252a --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java @@ -0,0 +1,105 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.data.DataContext; +import org.orekit.frames.Frame; +import org.orekit.frames.Frames; +import org.orekit.propagation.Propagator; +import org.orekit.utils.IERSConventions; + +/** + * This class aims at propagating a Galileo orbit from + * {@link GalileoOrbitalElements}. + * + * @see Galileo + * Interface Control Document + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + */ +public class FieldGalileoPropagator> extends FieldAbstractGNSSPropagator { + + // Constants + /** Value of the earth's rotation rate in rad/s. */ + private static final double GALILEO_AV = 7.2921151467e-5; + + /** Duration of the Galileo cycle in seconds. */ + private static final double GALILEO_CYCLE_DURATION = GalileoOrbitalElements.GALILEO_WEEK_IN_SECONDS + * GalileoOrbitalElements.GALILEO_WEEK_NB; + + // Fields + /** The Galileo orbital elements used. */ + private final FieldGalileoOrbitalElements galileoOrbit; + + /** + * Default constructor + * + * @param field + * @param galileoOrbit + */ + @DefaultDataContext + public FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit) { + this(field, galileoOrbit, DataContext.getDefault().getFrames()); + } + + /** + * Constructor + * + * @param field + * @param galileoOrbit + * @param frames + */ + public FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, + final Frames frames) { + this(field, galileoOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), + frames.getITRF(IERSConventions.IERS_2010, true)); + } + + /** + * Constructor + * + * @param field + * @param galileoOrbit + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { + super(field, galileoOrbit, attitudeProvider, eci, ecef, mass, GALILEO_AV, GALILEO_CYCLE_DURATION, + FieldGalileoOrbitalElements.GALILEO_MU); + // Stores the Beidou orbital elements + this.galileoOrbit = galileoOrbit; + } + + /** + * Get the underlying Beidou orbital elements. + * + * @return the underlying Beidou orbital elements + */ + public FieldGalileoOrbitalElements getFieldGalileoOrbitalElements() { + return galileoOrbit; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java new file mode 100644 index 000000000..19271b643 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java @@ -0,0 +1,267 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.util.FastMath; +import org.orekit.gnss.IRNSSAlmanac; +import org.orekit.time.FieldAbsoluteDate; + +/** + * Class for IRNSS almanac. + * + * @see "Indian Regiona Navigation Satellite System, Signal In Space ICD for + * standard positioning service, version 1.1 - Table 28" + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + * + */ +public class FieldIRNSSAlmanac> implements FieldIRNSSOrbitalElements { + + private final T zero; + /** PRN number. */ + private final int prn; + + /** IRNSS week. */ + private final int week; + + /** Time of applicability. */ + private final T toa; + + /** Semi-major axis. */ + private final T sma; + + /** Eccentricity. */ + private final T ecc; + + /** Inclination. */ + private final T inc; + + /** Longitude of Orbital Plane. */ + private final T om0; + + /** Rate of Right Ascension. */ + private final T dom; + + /** Argument of perigee. */ + private final T aop; + + /** Mean anomaly. */ + private final T anom; + + /** Zeroth order clock correction. */ + private final T af0; + + /** First order clock correction. */ + private final T af1; + + /** Date of aplicability. */ + private final FieldAbsoluteDate date; + + /** + * Constructor. + * + * @param prn the PRN number + * @param week the GPS week + * @param toa the Time of Applicability + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc the inclination (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly epoch + * (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param date of applicability corresponding to {@code toa}. + */ + public FieldIRNSSAlmanac(final int prn, final int week, final T toa, final T sqa, final T ecc, final T inc, + final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, + final FieldAbsoluteDate date) { + this.zero = toa.getField().getZero(); + this.prn = prn; + this.week = week; + this.toa = toa; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.inc = inc; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.af0 = af0; + this.af1 = af1; + this.date = date; + } + + /** + * Constructor + * + * @param field + * @param almanac + */ + public FieldIRNSSAlmanac(Field field, IRNSSOrbitalElements almanac) { + this.zero = field.getZero(); + this.prn = almanac.getPRN(); + this.week = almanac.getWeek(); + this.toa = zero.add(almanac.getTime()); + this.sma = zero.add(almanac.getSma()); + this.ecc = zero.add(almanac.getE()); + this.inc = zero.add(almanac.getI0()); + this.om0 = zero.add(almanac.getOmega0()); + this.dom = zero.add(almanac.getOmegaDot()); + this.aop = zero.add(almanac.getPa()); + this.anom = zero.add(almanac.getM0()); + this.af0 = zero.add(almanac.getAf0()); + this.af1 = zero.add(almanac.getAf1()); + this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toa; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(absA.getField().getZero().add(IRNSS_MU).divide(absA)).divide(absA); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return zero; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return zero; + } + + @Override + public T getCus() { + return zero; + } + + @Override + public T getCrc() { + return zero; + } + + @Override + public T getCrs() { + return zero; + } + + @Override + public T getCic() { + return zero; + } + + @Override + public T getCis() { + return zero; + } + + @Override + public T getAf0() { + return af0; + } + + @Override + public T getAf1() { + return af1; + } + + @Override + public T getAf2() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public int getIODEC() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getTGD() { + // TODO Auto-generated method stub + return null; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java new file mode 100644 index 000000000..315f37180 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java @@ -0,0 +1,32 @@ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; + +public interface FieldIRNSSOrbitalElements> extends FieldGNSSOrbitalElements { + + /** WGS 84 value of the Earth's universal gravitational parameter for IRNSS user in m³/s². */ + double IRNSS_MU = 3.986005e+14; + + /** Value of Pi for conversion from semicircles to radian. */ + double IRNSS_PI = 3.1415926535898; + + /** Duration of the IRNSS week in seconds. */ + double IRNSS_WEEK_IN_SECONDS = 604800.; + + /** Number of weeks in the IRNSS cycle. */ + int IRNSS_WEEK_NB = 1024; + + /** + * Gets the Issue Of Data Ephemeris and Clock (IODEC). + * + * @return the Issue Of Data Ephemeris and Clock (IODEC) + */ + int getIODEC(); + + /** + * Gets the estimated group delay differential TGD for L5-S correction. + * + * @return the estimated group delay differential TGD for L5-S correction (s) + */ + T getTGD(); +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java new file mode 100644 index 000000000..fbcc53f02 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java @@ -0,0 +1,98 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.data.DataContext; +import org.orekit.frames.Frame; +import org.orekit.frames.Frames; +import org.orekit.propagation.Propagator; +import org.orekit.utils.IERSConventions; + +/** + * This class aims at propagating a IRNSS orbit from + * {@link FieldIRNSSOrbitalElements}. + * + * @see "Indian Regional Navigation Satellite System, Signal In Space ICD for + * standard positioning service, version 1.1" + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + */ +public class FieldIRNSSPropagator> extends FieldAbstractGNSSPropagator { + // Constants + /** WGS 84 value of the earth's rotation rate in rad/s. */ + private static final double IRNSS_AV = 7.2921151467e-5; + + /** Duration of the IRNSS cycle in seconds. */ + private static final double IRNSS_CYCLE_DURATION = FieldIRNSSOrbitalElements.IRNSS_WEEK_IN_SECONDS + * FieldIRNSSOrbitalElements.IRNSS_WEEK_NB; + + // Fields + /** The IRNSS orbital elements used. */ + private final FieldIRNSSOrbitalElements irnssOrbit; + + /** + * Default constructor. + * + * @param field + * @param irnssOrbit + */ + @DefaultDataContext + public FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit) { + this(field, irnssOrbit, DataContext.getDefault().getFrames()); + } + + /** + * Constructor. + * + * @param field + * @param irnssOrbit + * @param frames + */ + public FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit, + final Frames frames) { + this(field, irnssOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), + frames.getITRF(IERSConventions.IERS_2010, true)); + } + + /** + * Constructor. + * + * @param field + * @param irnssOrbit + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { + super(field, irnssOrbit, attitudeProvider, eci, ecef, mass, IRNSS_AV, IRNSS_CYCLE_DURATION, + FieldIRNSSOrbitalElements.IRNSS_MU); + // Stores the IRNSS orbital elements + this.irnssOrbit = irnssOrbit; + } + + public FieldIRNSSOrbitalElements getFieldIRNSSOrbitalElements() { + return irnssOrbit; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSAlmanac.java new file mode 100644 index 000000000..5bb338271 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSAlmanac.java @@ -0,0 +1,343 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.util.FastMath; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.data.DataContext; +import org.orekit.gnss.QZSSAlmanac; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.FieldAbsoluteDate; + +/** + * This class holds a QZSS almanac as read from YUMA files. + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + * + */ +public class FieldQZSSAlmanac> implements FieldQZSSOrbitalElements { + private final T zero; + // Fields + /** Source of the almanac. */ + private final String src; + + /** PRN number. */ + private final int prn; + + /** Health status. */ + private final int health; + + /** QZSS week. */ + private final int week; + + /** Time of applicability. */ + private final T toa; + + /** Semi-major axis. */ + private final T sma; + + /** Eccentricity. */ + private final T ecc; + + /** Inclination. */ + private final T inc; + + /** Longitude of Orbital Plane. */ + private final T om0; + + /** Rate of Right Ascension. */ + private final T dom; + + /** Argument of perigee. */ + private final T aop; + + /** Mean anomaly. */ + private final T anom; + + /** Zeroth order clock correction. */ + private final T af0; + + /** First order clock correction. */ + private final T af1; + + /** Date of validity. */ + private final FieldAbsoluteDate date; + + /** + * Constructor. + * + *

+ * This method uses the {@link DataContext#getDefault() default data context}. + * + * @param source the source of the almanac (SEM, YUMA, user defined) + * @param prn the PRN number + * @param week the QZSS week + * @param toa the Time of Applicability + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc the inclination (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @see #FieldQZSSAlmanac(String, int, int, T, T, T, T, T, T, T, T, T, T, int, + * FieldAbsoluteDate) + */ + @DefaultDataContext + public FieldQZSSAlmanac(final String source, final int prn, final int week, final T toa, final T sqa, final T ecc, + final T inc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, + final int health) { + this(source, prn, week, toa, sqa, ecc, inc, om0, dom, aop, anom, af0, af1, health, new FieldGNSSDate<>(week, + toa.multiply(1000), SatelliteSystem.QZSS, DataContext.getDefault().getTimeScales()).getDate()); + } + + /** + * Constructor. + * + * @param source the source of the almanac (SEM, YUMA, user defined) + * @param prn the PRN number + * @param week the QZSS week + * @param toa the Time of Applicability + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc the inclination (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @param date of applicability corresponding to {@code week} and {@code toa}. + * @since 10.1 + */ + public FieldQZSSAlmanac(final String source, final int prn, final int week, final T toa, final T sqa, final T ecc, + final T inc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, + final int health, final FieldAbsoluteDate date) { + this.zero = toa.getField().getZero(); + this.src = source; + this.prn = prn; + this.week = week; + this.toa = toa; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.inc = inc; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.af0 = af0; + this.af1 = af1; + this.health = health; + this.date = date; + } + + /** + * Constructor + * + * @param field + * @param almanac + */ + public FieldQZSSAlmanac(Field field, QZSSAlmanac almanac) { + this.zero = field.getZero(); + this.src = almanac.getSource(); + this.prn = almanac.getPRN(); + this.week = almanac.getWeek(); + this.toa = zero.add(almanac.getTime()); + this.sma = zero.add(almanac.getSma()); + this.ecc = zero.add(almanac.getE()); + this.inc = zero.add(almanac.getI0()); + this.om0 = zero.add(almanac.getOmega0()); + this.dom = zero.add(almanac.getOmegaDot()); + this.aop = zero.add(almanac.getPa()); + this.anom = zero.add(almanac.getM0()); + this.af0 = zero.add(almanac.getAf0()); + this.af1 = zero.add(almanac.getAf1()); + this.health = almanac.getHealth(); + this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); + } + + /** + * Gets the source of this QZSS almanac. + * + * @return the source of this QZSS almanac + */ + public String getSource() { + return src; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toa; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(absA.getField().getZero().add(QZSS_MU).divide(absA)).divide(absA); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getAf0() { + return af0; + } + + @Override + public T getAf1() { + return af1; + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + /** + * Gets the Health status. + * + * @return the Health status + */ + public int getHealth() { + return health; + } + + @Override + public T getIDot() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getCuc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getCus() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getCrc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getCrs() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getCic() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getCis() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAf2() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public int getIODC() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public int getIODE() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getTGD() { + // TODO Auto-generated method stub + return null; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSOrbitalElements.java new file mode 100644 index 000000000..4f839e3f2 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSOrbitalElements.java @@ -0,0 +1,49 @@ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; + +/** This interface provides the minimal set of orbital elements needed by the {@link QZSSPropagator}. +* +* @see +* QZSS Interface Specification +* +* @author Bryan Cazabonne +* @author Nicolas Fialton (field translation) +* +*/ +public interface FieldQZSSOrbitalElements> extends FieldGNSSOrbitalElements { + + // Constants + /** WGS 84 value of the Earth's universal gravitational parameter for QZSS user in m³/s². */ + double QZSS_MU = 3.986005e+14; + + /** Value of Pi for conversion from semicircles to radian. */ + double QZSS_PI = 3.1415926535898; + + /** Duration of the QZSS week in seconds. */ + double QZSS_WEEK_IN_SECONDS = 604800.; + + /** Number of weeks in the QZSS cycle. */ + int QZSS_WEEK_NB = 1024; + + /** + * Gets the Issue Of Data Clock (IODC). + * + * @return the Issue Of Data Clock (IODC) + */ + int getIODC(); + + /** + * Gets the Issue Of Data Ephemeris (IODE). + * + * @return the Issue Of Data Ephemeris (IODE) + */ + int getIODE(); + + /** + * Gets the estimated group delay differential TGD between SV clock and L1C/A. + * + * @return the estimated group delay differential TGD between SV clock and L1C/A (s) + */ + T getTGD(); +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagator.java new file mode 100644 index 000000000..303e26823 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagator.java @@ -0,0 +1,85 @@ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.data.DataContext; +import org.orekit.frames.Frame; +import org.orekit.frames.Frames; +import org.orekit.propagation.Propagator; +import org.orekit.utils.IERSConventions; +/** + * This class aims at propagating a QZSS orbit from {@link FieldQZSSOrbitalElements}. + * + * @see + * QZSS Interface Specification + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + */ +public class FieldQZSSPropagator> extends FieldAbstractGNSSPropagator { + + // Constants + /** WGS 84 value of the earth's rotation rate in rad/s. */ + private static final double QZSS_AV = 7.2921151467e-5; + + /** Duration of the QZSS cycle in seconds. */ + private static final double QZSS_CYCLE_DURATION = FieldQZSSOrbitalElements.QZSS_WEEK_IN_SECONDS * + FieldQZSSOrbitalElements.QZSS_WEEK_NB; + + // Fields + /** The QZSS orbital elements used. */ + private final FieldQZSSOrbitalElements qzssOrbit; + + /** + * Default constructor. + * + * @param field + * @param qzssOrbit + */ + @DefaultDataContext + public FieldQZSSPropagator(final Field field, final FieldQZSSOrbitalElements qzssOrbit) { + this(field, qzssOrbit, DataContext.getDefault().getFrames()); + } + + /** + * Constructor. + * + * @param field + * @param qzssOrbit + * @param frames + */ + public FieldQZSSPropagator(final Field field, final FieldQZSSOrbitalElements qzssOrbit, + final Frames frames) { + this(field, qzssOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), + frames.getITRF(IERSConventions.IERS_2010, true)); + } + + /** + * Constructor. + * + * @param field + * @param qzssOrbit + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldQZSSPropagator(final Field field, final FieldQZSSOrbitalElements qzssOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { + super(field, qzssOrbit, attitudeProvider, eci, ecef, mass, QZSS_AV, QZSS_CYCLE_DURATION, + FieldQZSSOrbitalElements.QZSS_MU); + // Stores the QZSS orbital elements + this.qzssOrbit = qzssOrbit; + } + + /** + * Get the underlying QZSS orbital elements. + * + * @return the underlying QZSS orbital elements + */ + public FieldQZSSOrbitalElements getFieldQZSSOrbitalElements() { + return qzssOrbit; + } +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASOrbitalElements.java new file mode 100644 index 000000000..06a1884c4 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASOrbitalElements.java @@ -0,0 +1,131 @@ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.RealFieldElement; +import org.orekit.time.FieldTimeStamped; + +/** This interface provides the minimal set of orbital elements needed by the {@link SBASPropagator}. +* +* @author Bryan Cazabonne +* @author Nicolas Fialton (field translation) +* +*/ + +public interface FieldSBASOrbitalElements> extends FieldTimeStamped { + + /** WGS 84 value of the Earth's universal gravitational parameter for SBAS user in m³/s². */ + double SBAS_MU = 3.986005e+14; + + /** + * Gets the PRN number of the SBAS satellite. + * + * @return the PRN number of the SBAS satellite + */ + int getPRN(); + + /** + * Gets the Reference Week of the SBAS orbit. + * + * @return the Reference Week of the SBAS orbit + */ + int getWeek(); + + /** + * Gets the Reference Time of the SBAS orbit in GPS seconds of the week. + * + * @return the Reference Time of the SBAS orbit (s) + */ + T getTime(); + + /** + * Get the ECEF-X component of satellite coordinates. + * + * @return the ECEF-X component of satellite coordinates (m) + */ + T getX(); + + /** + * Get the ECEF-X component of satellite velocity vector. + * + * @return the the ECEF-X component of satellite velocity vector (m/s) + */ + T getXDot(); + + /** + * Get the ECEF-X component of satellite acceleration vector. + * + * @return the GLONASS ECEF-X component of satellite acceleration vector (m/s²) + */ + T getXDotDot(); + + /** + * Get the ECEF-Y component of satellite coordinates. + * + * @return the ECEF-Y component of satellite coordinates (m) + */ + T getY(); + + /** + * Get the ECEF-Y component of satellite velocity vector. + * + * @return the ECEF-Y component of satellite velocity vector (m/s) + */ + T getYDot(); + + /** + * Get the ECEF-Y component of satellite acceleration vector. + * + * @return the ECEF-Y component of satellite acceleration vector (m/s²) + */ + T getYDotDot(); + + /** + * Get the ECEF-Z component of satellite coordinates. + * + * @return the ECEF-Z component of satellite coordinates (m) + */ + T getZ(); + + /** + * Get the ECEF-Z component of satellite velocity vector. + * + * @return the the ECEF-Z component of satellite velocity vector (m/s) + */ + T getZDot(); + + /** + * Get the ECEF-Z component of satellite acceleration vector. + * + * @return the ECEF-Z component of satellite acceleration vector (m/s²) + */ + T getZDotDot(); + + /** + * Gets the Issue Of Data Navigation (IODN). + * + * @return the IODN + */ + int getIODN(); + + /** + * Gets the Zeroth Order Clock Correction. + * + * @return the Zeroth Order Clock Correction (s) + */ + T getAGf0(); + + /** + * Gets the First Order Clock Correction. + * + * @return the First Order Clock Correction (s/s) + */ + T getAGf1(); + + /** + * Gets the clock correction reference time toc. + * + * @return the clock correction reference time (s) + */ + T getToc(); + +} + diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagator.java new file mode 100644 index 000000000..822b716c4 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagator.java @@ -0,0 +1,257 @@ +package org.orekit.propagation.analytical.gnss; + +import java.util.Collections; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.data.DataContext; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.frames.Frames; +import org.orekit.orbits.FieldCartesianOrbit; +import org.orekit.orbits.FieldOrbit; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.Propagator; +import org.orekit.propagation.SpacecraftState; +import org.orekit.propagation.analytical.FieldAbstractAnalyticalPropagator; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.IERSConventions; +import org.orekit.utils.ParameterDriver; + +/** + * + * This class aims at propagating a SBAS orbit from {@link SBASOrbitalElements}. + * + * @see "Tyler Reid, Todd Walker, Per Enge, L1/L5 SBAS MOPS Ephemeris Message to + * Support Multiple Orbit Classes, ION ITM, 2013" + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + */ +public class FieldSBASPropagator> extends FieldAbstractAnalyticalPropagator { + /** The SBAS orbital elements used. */ + private final FieldSBASOrbitalElements sbasOrbit; + + /** The spacecraft mass (kg). */ + private final T mass; + + /** The Earth gravity coefficient used for SBAS propagation. */ + private final T mu; + + /** The ECI frame used for SBAS propagation. */ + private final Frame eci; + + /** The ECEF frame used for SBAS propagation. */ + private final Frame ecef; + + /** Initial state. */ + private FieldSpacecraftState initialState; + + /** + * Default constructor. + * + * @param field + * @param sbasOrbit + */ + @DefaultDataContext + public FieldSBASPropagator(final Field field, final FieldSBASOrbitalElements sbasOrbit) { + this(field, sbasOrbit, DataContext.getDefault().getFrames()); + } + + /** + * Constructor. + * + * @param field + * @param sbasOrbit + * @param frames + */ + public FieldSBASPropagator(final Field field, final FieldSBASOrbitalElements sbasOrbit, final Frames frames) { + this(field, sbasOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), + frames.getITRF(IERSConventions.IERS_2010, true)); + } + + /** + * Constructor. + * + * @param field + * @param sbasOrbit + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldSBASPropagator(final Field field, final FieldSBASOrbitalElements sbasOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef /*, final FieldSpacecraftState initialState*/) { + super(field, attitudeProvider); + // Stores the SBAS orbital elements + this.sbasOrbit = sbasOrbit; + // Sets the start date as the date of the orbital elements + setStartDate(sbasOrbit.getDate()); + // Sets the mu + final T zero = field.getZero(); + this.mu = zero.add(FieldSBASOrbitalElements.SBAS_MU); + // Sets the mass + this.mass = zero.add(mass); + // Sets the Earth Centered Inertial frame + this.eci = eci; + // Sets the Earth Centered Earth Fixed frame + this.ecef = ecef; + //this.initialState = initialState; + } + + /** + * Get the underlying SBAS orbital elements. + * + * @return the underlying SBAS orbital elements + */ + public FieldSBASOrbitalElements getFieldSBASOrbitalElements() { + return sbasOrbit; + } + + /** + * Gets the PVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}. + * + *

+ * The algorithm uses automatic differentiation to compute velocity and + * acceleration. + *

+ * + * @param date the computation date + * @return the GNSS SV PVCoordinates in {@link #getECEF() ECEF frame} + */ + public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { + // Duration from SBAS ephemeris Reference date + final T zero = date.getField().getZero(); + final FieldUnivariateDerivative2 dt = new FieldUnivariateDerivative2(zero.add(getDT(date)), zero.add(1.0), + zero.add(1.0)); + // Satellite coordinates + final FieldUnivariateDerivative2 x = dt + .multiply(dt.multiply(sbasOrbit.getXDotDot().multiply(0.5)).add(sbasOrbit.getXDot())) + .add(sbasOrbit.getX()); + final FieldUnivariateDerivative2 y = dt + .multiply(dt.multiply(sbasOrbit.getYDotDot().multiply(0.5)).add(sbasOrbit.getYDot())) + .add(sbasOrbit.getY()); + final FieldUnivariateDerivative2 z = dt + .multiply(dt.multiply(sbasOrbit.getZDotDot().multiply(0.5)).add(sbasOrbit.getZDot())) + .add(sbasOrbit.getZ()); + // Returns the Earth-fixed coordinates + final FieldVector3D> positionwithDerivatives = new FieldVector3D<>(x, y, z); + return new FieldPVCoordinates( + new FieldVector3D(positionwithDerivatives.getX().getValue(), + positionwithDerivatives.getY().getValue(), positionwithDerivatives.getZ().getValue()), + new FieldVector3D(positionwithDerivatives.getX().getFirstDerivative(), + positionwithDerivatives.getY().getFirstDerivative(), + positionwithDerivatives.getZ().getFirstDerivative()), + new FieldVector3D(positionwithDerivatives.getX().getSecondDerivative(), + positionwithDerivatives.getY().getSecondDerivative(), + positionwithDerivatives.getZ().getSecondDerivative())); + } + + /** {@inheritDoc} */ + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date) { + // Gets the PVCoordinates in ECEF frame + final FieldPVCoordinates pvaInECEF = propagateInEcef(date); + // Transforms the PVCoordinates to ECI frame + final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); + // Returns the Cartesian orbit + final T zero = date.getField().getZero(); + return new FieldCartesianOrbit(pvaInECI, eci, date, zero.add(mu)); + } + + /** + * Get the Earth gravity coefficient used for SBAS propagation. + * + * @return the Earth gravity coefficient. + */ + public T getMU() { + return mu; + } + + /** + * Gets the Earth Centered Inertial frame used to propagate the orbit. + * + * @return the ECI frame + */ + public Frame getECI() { + return eci; + } + + /** + * Gets the Earth Centered Earth Fixed frame used to propagate GNSS orbits. + * + * @return the ECEF frame + */ + public Frame getECEF() { + return ecef; + } + + /** + * Get the underlying SBAS orbital elements. + * + * @return the underlying SBAS orbital elements + */ + public FieldSBASOrbitalElements getSBASOrbitalElements() { + return sbasOrbit; + } + + /** {@inheritDoc} */ + public Frame getFrame() { + return eci; + } + + /** {@inheritDoc} */ + public void resetInitialState(final SpacecraftState state) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected T getMass(final FieldAbsoluteDate date) { + return mass; + } + + /** {@inheritDoc} */ + protected void resetIntermediateState(final SpacecraftState state, final boolean forward) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** + * Get the duration from SBAS Reference epoch. + * + * @param date the considered date + * @return the duration from SBAS orbit Reference epoch (s) + */ + private T getDT(final FieldAbsoluteDate date) { + // Time from ephemeris reference epoch + return date.durationFrom(sbasOrbit.getDate()); + } + + @Override + protected void resetIntermediateState(FieldSpacecraftState state, boolean forward) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + + } + + @Override + protected FieldOrbit propagateOrbit(FieldAbsoluteDate date, T[] parameters) { + // Gets the PVCoordinates in ECEF frame + final FieldPVCoordinates pvaInECEF = propagateInEcef(date); + // Transforms the PVCoordinates to ECI frame + final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); + // Returns the Cartesian orbit + return new FieldCartesianOrbit(pvaInECI, eci, date, mu); + } + + @Override + protected List getParametersDrivers() { + + return Collections.emptyList(); + } + +} diff --git a/src/test/java/org/orekit/forces/maneuvers/FieldSmallManeuverAnalyticalModelTest.java b/src/test/java/org/orekit/forces/maneuvers/FieldSmallManeuverAnalyticalModelTest.java new file mode 100644 index 000000000..2078866ca --- /dev/null +++ b/src/test/java/org/orekit/forces/maneuvers/FieldSmallManeuverAnalyticalModelTest.java @@ -0,0 +1,337 @@ +package org.orekit.forces.maneuvers; + +import java.lang.reflect.Array; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator; +import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.hipparchus.util.MathArrays; +import org.junit.Assert; +import org.junit.Before; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.attitudes.LofOffset; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.frames.LOFType; +import org.orekit.orbits.FieldCircularOrbit; +import org.orekit.orbits.FieldKeplerianOrbit; +import org.orekit.orbits.FieldOrbit; +import org.orekit.orbits.OrbitType; +import org.orekit.orbits.PositionAngle; +import org.orekit.propagation.FieldBoundedPropagator; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.analytical.FieldSmallManeuverAnalyticalModel; +import org.orekit.propagation.numerical.FieldNumericalPropagator; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.DateComponents; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeComponents; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldPVCoordinates; + +public class FieldSmallManeuverAnalyticalModelTest { + + @Test + public void testLowEarthOrbit1() { + doTestLowEarthOrbit1(Decimal64Field.getInstance()); + } + + @Test + public void testLowEarthOrbit2() { + doTestLowEarthOrbit2(Decimal64Field.getInstance()); + } + + @Test + public void testEccentricOrbit() { + doTestEccentricOrbit(Decimal64Field.getInstance()); + } + + @Test + public void testJacobian() { + doTestJacobian(Decimal64Field.getInstance()); + } + + public > void doTestLowEarthOrbit1(Field field) { + + final T zero = field.getZero(); + FieldOrbit leo = new FieldCircularOrbit<>(zero.add(7200000.0), zero.add(-1.0e-5), zero.add(2.0e-4), + zero.add(FastMath.toRadians(98.0)), zero.add(FastMath.toRadians(123.456)), zero.add(0.0), + PositionAngle.MEAN, FramesFactory.getEME2000(), + new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 01), + new TimeComponents(23, 30, 00.000), TimeScalesFactory.getUTC())), + zero.add(Constants.EIGEN5C_EARTH_MU)); + T mass = zero.add(5600.0); + FieldAbsoluteDate t0 = leo.getDate().shiftedBy(1000.0); + FieldVector3D dV = new FieldVector3D<>(zero.add(-0.01), zero.add(0.02), zero.add(0.03)); + T f = zero.add(20.0); + T isp = zero.add(315.0); + FieldBoundedPropagator withoutManeuver = getEphemeris(leo, mass, t0, FieldVector3D.getZero(field), f, isp); + FieldBoundedPropagator withManeuver = getEphemeris(leo, mass, t0, dV, f, isp); + FieldSmallManeuverAnalyticalModel model = new FieldSmallManeuverAnalyticalModel<>(field, + withoutManeuver.propagate(t0), dV, isp); + Assert.assertEquals(t0, model.getDate()); + + for (FieldAbsoluteDate t = withoutManeuver.getMinDate(); t.compareTo(withoutManeuver.getMaxDate()) < 0; t = t + .shiftedBy(60.0)) { + FieldPVCoordinates pvWithout = withoutManeuver.getPVCoordinates(t, leo.getFrame()); + FieldPVCoordinates pvWith = withManeuver.getPVCoordinates(t, leo.getFrame()); + FieldPVCoordinates pvModel = model.apply(withoutManeuver.propagate(t)).getPVCoordinates(leo.getFrame()); + T nominalDeltaP = new FieldPVCoordinates<>(pvWith, pvWithout).getPosition().getNorm(); + T modelError = new FieldPVCoordinates<>(pvWith, pvModel).getPosition().getNorm(); + if (t.compareTo(t0) < 0) { +// before maneuver, all positions should be equal + Assert.assertEquals(0, nominalDeltaP.getReal(), 1.0e-10); + Assert.assertEquals(0, modelError.getReal(), 1.0e-10); + } else { +// after maneuver, model error should be less than 0.8m, +// despite nominal deltaP exceeds 1 kilometer after less than 3 orbits + if (t.durationFrom(t0).getReal() > 0.1 * leo.getKeplerianPeriod().getReal()) { + Assert.assertTrue(modelError.getReal() < 0.009 * nominalDeltaP.getReal()); + } + Assert.assertTrue(modelError.getReal() < 0.8); + } + } + } + + public > void doTestLowEarthOrbit2(Field field) { + + final T zero = field.getZero(); + FieldOrbit leo = new FieldCircularOrbit<>(zero.add(7200000.0), zero.add(-1.0e-5), zero.add(2.0e-4), + zero.add(FastMath.toRadians(98.0)), zero.add(FastMath.toRadians(123.456)), zero.add(0.0), + PositionAngle.MEAN, FramesFactory.getEME2000(), + new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 01), + new TimeComponents(23, 30, 00.000), TimeScalesFactory.getUTC())), + zero.add(Constants.EIGEN5C_EARTH_MU)); + T mass = zero.add(5600.0); + FieldAbsoluteDate t0 = leo.getDate().shiftedBy(1000.0); + FieldVector3D dV = new FieldVector3D<>(zero.add(-0.01), zero.add(0.02), zero.add(0.03)); + T f = zero.add(20.0); + T isp = zero.add(315.0); + FieldBoundedPropagator withoutManeuver = getEphemeris(leo, mass, t0, FieldVector3D.getZero(field), f, isp); + FieldBoundedPropagator withManeuver = getEphemeris(leo, mass, t0, dV, f, isp); + FieldSmallManeuverAnalyticalModel model = new FieldSmallManeuverAnalyticalModel<>(field, + withoutManeuver.propagate(t0), dV, isp); + Assert.assertEquals(t0, model.getDate()); + + for (FieldAbsoluteDate t = withoutManeuver.getMinDate(); t.compareTo(withoutManeuver.getMaxDate()) < 0; t = t + .shiftedBy(60.0)) { + FieldPVCoordinates pvWithout = withoutManeuver.getPVCoordinates(t, leo.getFrame()); + FieldPVCoordinates pvWith = withManeuver.getPVCoordinates(t, leo.getFrame()); + FieldPVCoordinates pvModel = model.apply(withoutManeuver.propagate(t)).getPVCoordinates(leo.getFrame()); + T nominalDeltaP = new FieldPVCoordinates<>(pvWith, pvWithout).getPosition().getNorm(); + T modelError = new FieldPVCoordinates<>(pvWith, pvModel).getPosition().getNorm(); + if (t.compareTo(t0) < 0) { +// before maneuver, all positions should be equal + Assert.assertEquals(0, nominalDeltaP.getReal(), 1.0e-10); + Assert.assertEquals(0, modelError.getReal(), 1.0e-10); + } else { +// after maneuver, model error should be less than 0.8m, +// despite nominal deltaP exceeds 1 kilometer after less than 3 orbits + if (t.durationFrom(t0).getReal() > 0.1 * leo.getKeplerianPeriod().getReal()) { + Assert.assertTrue(modelError.getReal() < 0.009 * nominalDeltaP.getReal()); + } + Assert.assertTrue(modelError.getReal() < 0.8); + } + } + } + + public > void doTestEccentricOrbit(Field field) { + final T zero = field.getZero(); + FieldOrbit heo = new FieldKeplerianOrbit<>(zero.add(90000000.0), zero.add(0.92), + zero.add(FastMath.toRadians(98.0)), zero.add(FastMath.toRadians(12.3456)), + zero.add(FastMath.toRadians(123.456)), zero.add(FastMath.toRadians(1.23456)), PositionAngle.MEAN, + FramesFactory.getEME2000(), + new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 01), + new TimeComponents(23, 30, 00.000), TimeScalesFactory.getUTC())), + zero.add(Constants.EIGEN5C_EARTH_MU)); + T mass = zero.add(5600.0); + FieldAbsoluteDate t0 = heo.getDate().shiftedBy(1000.0); + FieldVector3D dV = new FieldVector3D<>(zero.add(-0.01), zero.add(0.02), zero.add(0.03)); + T f = zero.add(20.0); + T isp = zero.add(315.0); + FieldBoundedPropagator withoutManeuver = getEphemeris(heo, mass, t0, FieldVector3D.getZero(field), f, isp); + FieldBoundedPropagator withManeuver = getEphemeris(heo, mass, t0, dV, f, isp); + FieldSpacecraftState s0 = withoutManeuver.propagate(t0); + FieldSmallManeuverAnalyticalModel model = new FieldSmallManeuverAnalyticalModel<>(field, s0, dV, isp); + Assert.assertEquals(t0, model.getDate()); + Assert.assertEquals(0.0, + FieldVector3D.distance(dV, s0.getAttitude().getRotation().applyTo(model.getInertialDV())).getReal(), + 1.0e-15); + Assert.assertSame(FramesFactory.getEME2000(), model.getInertialFrame()); + + for (FieldAbsoluteDate t = withoutManeuver.getMinDate(); t.compareTo(withoutManeuver.getMaxDate()) < 0; t = t + .shiftedBy(600.0)) { + FieldPVCoordinates pvWithout = withoutManeuver.getPVCoordinates(t, heo.getFrame()); + FieldPVCoordinates pvWith = withManeuver.getPVCoordinates(t, heo.getFrame()); + FieldPVCoordinates pvModel = model.apply(withoutManeuver.propagate(t)).getPVCoordinates(heo.getFrame()); + T nominalDeltaP = new FieldPVCoordinates<>(pvWith, pvWithout).getPosition().getNorm(); + T modelError = new FieldPVCoordinates<>(pvWith, pvModel).getPosition().getNorm(); + if (t.compareTo(t0) < 0) { +// before maneuver, all positions should be equal + Assert.assertEquals(0, nominalDeltaP.getReal(), 1.0e-10); + Assert.assertEquals(0, modelError.getReal(), 1.0e-10); + } else { +// after maneuver, model error should be less than 1700m, +// despite nominal deltaP exceeds 300 kilometers at perigee, after 3 orbits + if (t.durationFrom(t0).getReal() > 0.01 * heo.getKeplerianPeriod().getReal()) { + Assert.assertTrue(modelError.getReal() < 0.005 * nominalDeltaP.getReal()); + } + Assert.assertTrue(modelError.getReal() < 1700); + } + } + + } + + public > void doTestJacobian(Field field) { + Frame eme2000 = FramesFactory.getEME2000(); + final T zero = field.getZero(); + final T one = field.getOne(); + FieldOrbit leo = new FieldCircularOrbit<>(zero.add(7200000.0), zero.add(-1.0e-5), zero.add(2.0e-4), + zero.add(FastMath.toRadians(98.0)), zero.add(FastMath.toRadians(123.456)), zero.add(0.0), + PositionAngle.MEAN, FramesFactory.getEME2000(), + new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 01), + new TimeComponents(23, 30, 00.000), TimeScalesFactory.getUTC())), + zero.add(Constants.EIGEN5C_EARTH_MU)); + T mass = zero.add(5600.0); + FieldAbsoluteDate t0 = leo.getDate().shiftedBy(1000.0); + FieldVector3D dV0 = new FieldVector3D<>(zero.add(-0.1), zero.add(0.2), zero.add(0.3)); + T f = zero.add(400.0); + T isp = zero.add(315.0); + + for (OrbitType orbitType : OrbitType.values()) { + for (PositionAngle positionAngle : PositionAngle.values()) { + FieldBoundedPropagator withoutManeuver = getEphemeris(orbitType.convertType(leo), mass, t0, + FieldVector3D.getZero(field), f, isp); + + FieldSpacecraftState state0 = withoutManeuver.propagate(t0); + FieldSmallManeuverAnalyticalModel model = new FieldSmallManeuverAnalyticalModel<>(field, state0, + eme2000, dV0, isp); + Assert.assertEquals(t0, model.getDate()); + + FieldVector3D[] velDirs;// = new FieldVector3D[]; { Vector3D.PLUS_I, Vector3D.PLUS_J, + // Vector3D.PLUS_K, Vector3D.ZERO }; + velDirs = (FieldVector3D[]) Array.newInstance(FieldVector3D.class, 4); + velDirs[0] = FieldVector3D.getPlusI(field); + velDirs[1] = FieldVector3D.getPlusJ(field); + velDirs[2] = FieldVector3D.getPlusK(field); + velDirs[3] = FieldVector3D.getZero(field); + + T[] timeDirs = MathArrays.buildArray(field, 4); + timeDirs[0] = zero; + timeDirs[1] = zero; + timeDirs[2] = zero; + timeDirs[3] = one; + T h = one; + FieldAbsoluteDate t1 = t0.shiftedBy(20.0); + for (int i = 0; i < 4; ++i) { + + FieldSmallManeuverAnalyticalModel[] models; + models = (FieldSmallManeuverAnalyticalModel[]) Array + .newInstance(FieldSmallManeuverAnalyticalModel.class, 8); + models[0] = new FieldSmallManeuverAnalyticalModel(field, + withoutManeuver.propagate(t0.shiftedBy(zero.add(-4).multiply(h).multiply(timeDirs[i]))), + eme2000, new FieldVector3D(zero.add(1), dV0, zero.add(-4).multiply(h), velDirs[i]), isp); + models[1] = new FieldSmallManeuverAnalyticalModel(field, + withoutManeuver.propagate(t0.shiftedBy(zero.add(-3).multiply(h).multiply(timeDirs[i]))), + eme2000, new FieldVector3D(zero.add(1), dV0, zero.add(-3).multiply(h), velDirs[i]), isp); + models[2] = new FieldSmallManeuverAnalyticalModel(field, + withoutManeuver.propagate(t0.shiftedBy(zero.add(-2).multiply(h).multiply(timeDirs[i]))), + eme2000, new FieldVector3D(zero.add(1), dV0, zero.add(-2).multiply(h), velDirs[i]), isp); + models[3] = new FieldSmallManeuverAnalyticalModel(field, + withoutManeuver.propagate(t0.shiftedBy(zero.add(-1).multiply(h).multiply(timeDirs[i]))), + eme2000, new FieldVector3D(zero.add(1), dV0, zero.add(-1).multiply(h), velDirs[i]), isp); + models[4] = new FieldSmallManeuverAnalyticalModel(field, + withoutManeuver.propagate(t0.shiftedBy(zero.add(+1).multiply(h).multiply(timeDirs[i]))), + eme2000, new FieldVector3D(zero.add(1), dV0, zero.add(+1).multiply(h), velDirs[i]), isp); + models[5] = new FieldSmallManeuverAnalyticalModel(field, + withoutManeuver.propagate(t0.shiftedBy(zero.add(+2).multiply(h).multiply(timeDirs[i]))), + eme2000, new FieldVector3D(zero.add(1), dV0, zero.add(+2).multiply(h), velDirs[i]), isp); + models[6] = new FieldSmallManeuverAnalyticalModel(field, + withoutManeuver.propagate(t0.shiftedBy(zero.add(+3).multiply(h).multiply(timeDirs[i]))), + eme2000, new FieldVector3D(zero.add(1), dV0, zero.add(+3).multiply(h), velDirs[i]), isp); + models[7] = new FieldSmallManeuverAnalyticalModel(field, + withoutManeuver.propagate(t0.shiftedBy(zero.add(+4).multiply(h).multiply(timeDirs[i]))), + eme2000, new FieldVector3D(zero.add(1), dV0, zero.add(+4).multiply(h), velDirs[i]), isp); + + T[][] array = MathArrays.buildArray(field, models.length, 6); + + FieldOrbit orbitWithout = withoutManeuver.propagate(t1).getOrbit(); + + // compute reference orbit gradient by finite differences + T c = one.divide(h.multiply(840)); + for (int j = 0; j < models.length; ++j) { + orbitType.mapOrbitToArray(models[j].apply(orbitWithout), positionAngle, array[j], null); + } + T[] orbitGradient = MathArrays.buildArray(field, 6); + for (int k = 0; k < orbitGradient.length; ++k) { + T d4 = array[7][k].subtract(array[0][k]); + T d3 = array[6][k].subtract(array[1][k]); + T d2 = array[5][k].subtract(array[2][k]); + T d1 = array[4][k].subtract(array[3][k]); + orbitGradient[k] = (d4.multiply(-3).add(d3.multiply(32)).subtract(d2.multiply(168)) + .add(d1.multiply(672))).multiply(c); + } + + // analytical Jacobian to check + T[][] jacobian = MathArrays.buildArray(field, 6, 4); + model.getJacobian(orbitWithout, positionAngle, jacobian); + + for (int j = 0; j < orbitGradient.length; ++j) { + //System.out.println(orbitGradient[j].getReal() + "\t" + jacobian[j][i].getReal() + "\t" + FastMath.abs(orbitGradient[j]).multiply(1.6e-4).getReal()); + Assert.assertEquals(orbitGradient[j].getReal(), jacobian[j][i].getReal(), + FastMath.abs(orbitGradient[j]).multiply(1.6e-4).getReal()); + } + + } + + } + + } + } + + private > FieldBoundedPropagator getEphemeris(final FieldOrbit orbit, final T mass, final FieldAbsoluteDate t0, + final FieldVector3D dV, final T f, final T isp) { + + AttitudeProvider law = new LofOffset(orbit.getFrame(), LOFType.LVLH); + final FieldSpacecraftState initialState = new FieldSpacecraftState<>(orbit, + law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass); + +// set up numerical propagator + final T zero = mass.getField().getZero(); + final T dP = zero.add(1); + double[][] tolerances = FieldNumericalPropagator.tolerances(dP, orbit, orbit.getType()); + AdaptiveStepsizeFieldIntegrator integrator = new DormandPrince853FieldIntegrator<>(mass.getField(), 0.001, 1000.0, tolerances[0], + tolerances[1]); + integrator.setInitialStepSize(orbit.getKeplerianPeriod().divide(100.0)); + final FieldNumericalPropagator propagator = new FieldNumericalPropagator<>(mass.getField(), integrator); + propagator.setOrbitType(orbit.getType()); + propagator.setInitialState(initialState); + propagator.setAttitudeProvider(law); + + if (dV.getNorm().getReal() > 1.0e-6) { +// set up maneuver + final T vExhaust = isp.multiply(Constants.G0_STANDARD_GRAVITY); + final T dt = zero.subtract(mass.multiply(vExhaust).divide(f)).multiply(FastMath.expm1( zero.subtract(dV.getNorm()).divide(vExhaust))); + final ConstantThrustManeuver maneuver = new ConstantThrustManeuver(t0.toAbsoluteDate(), dt.getReal(), f.getReal(), isp.getReal(), dV.normalize().toVector3D()); + propagator.addForceModel(maneuver); + } + + propagator.setEphemerisMode(); + propagator.propagate(t0.shiftedBy(orbit.getKeplerianPeriod().multiply(5))); + return propagator.getGeneratedEphemeris(); + + } + + @Before + public void setUp() { + Utils.setDataRoot("regular-data"); + } + +} diff --git a/src/test/java/org/orekit/propagation/analytical/FieldAdapterPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/FieldAdapterPropagatorTest.java new file mode 100644 index 000000000..40a1adc7a --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/FieldAdapterPropagatorTest.java @@ -0,0 +1,338 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical; + +import java.io.IOException; +import java.text.ParseException; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator; +import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.hipparchus.util.MathArrays; +import org.junit.Assert; +import org.junit.Before; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.attitudes.LofOffset; +import org.orekit.bodies.CelestialBodyFactory; +import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel; +import org.orekit.forces.gravity.ThirdBodyAttraction; +import org.orekit.forces.gravity.potential.GravityFieldFactory; +import org.orekit.forces.gravity.potential.ICGEMFormatReader; +import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider; +import org.orekit.forces.maneuvers.ConstantThrustManeuver; +import org.orekit.frames.FramesFactory; +import org.orekit.frames.LOFType; +import org.orekit.orbits.FieldCircularOrbit; +import org.orekit.orbits.FieldKeplerianOrbit; +import org.orekit.orbits.FieldOrbit; +import org.orekit.orbits.PositionAngle; +import org.orekit.propagation.FieldAdditionalStateProvider; +import org.orekit.propagation.FieldBoundedPropagator; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.numerical.FieldNumericalPropagator; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.DateComponents; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeComponents; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldPVCoordinates; + +public class FieldAdapterPropagatorTest { + + @Test + public void testLowEarthOrbit() throws ParseException, IOException{ + doTestLowEarthOrbit(Decimal64Field.getInstance()); + } + + @Test + public void testEccentricOrbit() throws ParseException, IOException { + doTestEccentricOrbit(Decimal64Field.getInstance()); + } + + @Test + public void testNonKeplerian() throws ParseException, IOException { + doTestNonKeplerian(Decimal64Field.getInstance()); + } + + public > void doTestLowEarthOrbit(Field field) throws ParseException, IOException { + final T zero = field.getZero(); + FieldOrbit leo = new FieldCircularOrbit(zero.add(7200000.0), zero.add(-1.0e-5), zero.add(2.0e-4), + zero.add(FastMath.toRadians(98.0)), zero.add(FastMath.toRadians(123.456)), zero.add(0.0), + PositionAngle.MEAN, FramesFactory.getEME2000(), + new FieldAbsoluteDate(field, new AbsoluteDate(new DateComponents(2004, 01, 01), + new TimeComponents(23, 30, 00.000), TimeScalesFactory.getUTC())), + zero.add(Constants.EIGEN5C_EARTH_MU)); + T mass = zero.add(5600.0); + FieldAbsoluteDate t0 = leo.getDate().shiftedBy(1000.0); + FieldVector3D dV = new FieldVector3D<>(zero.add(-0.1), zero.add(0.2), zero.add(0.3)); + T f = zero.add(20.0); + T isp = zero.add(315.0); + T vExhaust = zero.add(Constants.G0_STANDARD_GRAVITY).multiply(isp); + T dt = zero.subtract(mass.multiply(vExhaust).divide(f)) + .multiply(FastMath.expm1(zero.subtract(dV.getNorm()).divide(vExhaust))); + FieldBoundedPropagator withoutManeuver = getEphemeris(leo, mass, 5, + new LofOffset(leo.getFrame(), LOFType.LVLH), t0, FieldVector3D.getZero(field), f, isp, false, false, null); + FieldBoundedPropagator withManeuver = getEphemeris(leo, mass, 5, new LofOffset(leo.getFrame(), LOFType.LVLH), + t0, dV, f, isp, false, false, null); + +// we set up a model that reverts the maneuvers + FieldAdapterPropagator adapterPropagator = new FieldAdapterPropagator(field, withManeuver); + FieldAdapterPropagator.FieldDifferentialEffect effect = new FieldSmallManeuverAnalyticalModel<>(field, + adapterPropagator.propagate(t0), dV.negate(), isp); + adapterPropagator.addEffect(effect); + adapterPropagator.addAdditionalStateProvider(new FieldAdditionalStateProvider() { + public String getName() { + return "dummy 3"; + } + + public T[] getAdditionalState(FieldSpacecraftState state) { + return MathArrays.buildArray(state.getDate().getField(), 3); //new double[3]; + } + }); + +// the adapted propagators do not manage the additional states from the reference, +// they simply forward them + Assert.assertFalse(adapterPropagator.isAdditionalStateManaged("dummy 1")); + Assert.assertFalse(adapterPropagator.isAdditionalStateManaged("dummy 2")); + Assert.assertTrue(adapterPropagator.isAdditionalStateManaged("dummy 3")); + + for (FieldAbsoluteDate t = t0.shiftedBy(zero.add(0.5).multiply(dt)); t + .compareTo(withoutManeuver.getMaxDate()) < 0; t = t.shiftedBy(60.0)) { + FieldPVCoordinates pvWithout = withoutManeuver.getPVCoordinates(t, leo.getFrame()); + FieldPVCoordinates pvReverted = adapterPropagator.getPVCoordinates(t, leo.getFrame()); + T revertError = new FieldPVCoordinates(pvWithout, pvReverted).getPosition().getNorm(); + Assert.assertEquals(0, revertError.getReal(), 0.45); + Assert.assertEquals(2, adapterPropagator.propagate(t).getAdditionalState("dummy 1").length); + Assert.assertEquals(1, adapterPropagator.propagate(t).getAdditionalState("dummy 2").length); + Assert.assertEquals(3, adapterPropagator.propagate(t).getAdditionalState("dummy 3").length); + } + } + + public > void doTestEccentricOrbit(Field field) throws ParseException, IOException { + final T zero = field.getZero(); + FieldOrbit heo = new FieldKeplerianOrbit<>(zero.add(90000000.0), zero.add(0.92), + zero.add(FastMath.toRadians(98.0)), zero.add(FastMath.toRadians(12.3456)), + zero.add(FastMath.toRadians(123.456)), zero.add(FastMath.toRadians(1.23456)), PositionAngle.MEAN, + FramesFactory.getEME2000(), + new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 01), + new TimeComponents(23, 30, 00.000), TimeScalesFactory.getUTC())), + zero.add(Constants.EIGEN5C_EARTH_MU)); + T mass = zero.add(5600.0); + FieldAbsoluteDate t0 = heo.getDate().shiftedBy(1000.0); + FieldVector3D dV = new FieldVector3D<>(zero.add(-0.01), zero.add(0.02), zero.add(0.03)); + T f = zero.add(20.0); + T isp = zero.add(315.0); + T vExhaust = zero.add(Constants.G0_STANDARD_GRAVITY).multiply(isp); + T dt = zero.subtract(mass.multiply(vExhaust).divide(f)) + .multiply(FastMath.expm1(zero.subtract(dV.getNorm()).divide(vExhaust))); + FieldBoundedPropagator withoutManeuver = getEphemeris(heo, mass, 5, + new LofOffset(heo.getFrame(), LOFType.LVLH), t0, FieldVector3D.getZero(field), f, isp, false, false, null); + FieldBoundedPropagator withManeuver = getEphemeris(heo, mass, 5, new LofOffset(heo.getFrame(), LOFType.LVLH), + t0, dV, f, isp, false, false, null); + +// we set up a model that reverts the maneuvers + FieldAdapterPropagator adapterPropagator = new FieldAdapterPropagator(field, withManeuver); + FieldAdapterPropagator.FieldDifferentialEffect effect = new FieldSmallManeuverAnalyticalModel<>(field, + adapterPropagator.propagate(t0), dV.negate(), isp); + adapterPropagator.addEffect(effect); + adapterPropagator.addAdditionalStateProvider(new FieldAdditionalStateProvider() { + public String getName() { + return "dummy 3"; + } + + public T[] getAdditionalState(FieldSpacecraftState state) { + return MathArrays.buildArray(state.getDate().getField(), 3); //new double[3]; + } + + }); + +// the adapted propagators do not manage the additional states from the reference, +// they simply forward them + Assert.assertFalse(adapterPropagator.isAdditionalStateManaged("dummy 1")); + Assert.assertFalse(adapterPropagator.isAdditionalStateManaged("dummy 2")); + Assert.assertTrue(adapterPropagator.isAdditionalStateManaged("dummy 3")); + + for (FieldAbsoluteDate t = t0.shiftedBy(dt.multiply(0.5)); t + .compareTo(withoutManeuver.getMaxDate()) < 0; t = t.shiftedBy(300.0)) { + FieldPVCoordinates pvWithout = withoutManeuver.getPVCoordinates(t, heo.getFrame()); + FieldPVCoordinates pvReverted = adapterPropagator.getPVCoordinates(t, heo.getFrame()); + T revertError = FieldVector3D.distance(pvWithout.getPosition(), pvReverted.getPosition()); + Assert.assertEquals(0, revertError.getReal(), 2.5e-5 * heo.getA().getReal()); + Assert.assertEquals(2, adapterPropagator.propagate(t).getAdditionalState("dummy 1").length); + Assert.assertEquals(1, adapterPropagator.propagate(t).getAdditionalState("dummy 2").length); + Assert.assertEquals(3, adapterPropagator.propagate(t).getAdditionalState("dummy 3").length); + } + + } + + public > void doTestNonKeplerian(Field field) throws ParseException, IOException { + final T zero = field.getZero(); + FieldOrbit leo = new FieldCircularOrbit<>(zero.add(7204319.233600575), zero.add(4.434564637450575E-4), + zero.add(0.0011736728299091088), zero.add(1.7211611441767323), zero.add(5.5552084166959474), + zero.add(24950.321259193086), PositionAngle.TRUE, FramesFactory.getEME2000(), + new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2003, 9, 16), + new TimeComponents(23, 11, 20.264), TimeScalesFactory.getUTC())), + zero.add(Constants.EIGEN5C_EARTH_MU)); + T mass = zero.add(4093.0); + FieldAbsoluteDate t0 = new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2003, 9, 16), + new TimeComponents(23, 14, 40.264), TimeScalesFactory.getUTC())); + FieldVector3D dV = new FieldVector3D<>(zero.add(0.0), zero.add(3.0), zero.add(0.0)); + T f = zero.add(40.0); + T isp = zero.add(300.0); + T vExhaust = zero.add(Constants.G0_STANDARD_GRAVITY).multiply(isp); + T dt = zero.subtract(mass.multiply(vExhaust).divide(f)) + .multiply(FastMath.expm1(zero.subtract(dV.getNorm()).divide(vExhaust))); +// setup a specific coefficient file for gravity potential as it will also +// try to read a corrupted one otherwise + GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("g007_eigen_05c_coef", false)); + NormalizedSphericalHarmonicsProvider gravityField = GravityFieldFactory.getNormalizedProvider(8, 8); + FieldBoundedPropagator withoutManeuver = getEphemeris(leo, mass, 10, + new LofOffset(leo.getFrame(), LOFType.VNC), t0, FieldVector3D.getZero(field), f, isp, true, true, gravityField); + FieldBoundedPropagator withManeuver = getEphemeris(leo, mass, 10, new LofOffset(leo.getFrame(), LOFType.VNC), + t0, dV, f, isp, true, true, gravityField); + +// we set up a model that reverts the maneuvers + FieldAdapterPropagator adapterPropagator = new FieldAdapterPropagator<>(field, withManeuver); + FieldSpacecraftState state0 = adapterPropagator.propagate(t0); + FieldAdapterPropagator.FieldDifferentialEffect directEffect = new FieldSmallManeuverAnalyticalModel<>(field, + state0, dV.negate(), isp); + FieldAdapterPropagator.FieldDifferentialEffect derivedEffect = new FieldJ2DifferentialEffect<>(field, state0, + directEffect, false, GravityFieldFactory.getUnnormalizedProvider(gravityField)); + adapterPropagator.addEffect(directEffect); + adapterPropagator.addEffect(derivedEffect); + adapterPropagator.addAdditionalStateProvider(new FieldAdditionalStateProvider() { + public String getName() { + return "dummy 3"; + } + + public T[] getAdditionalState(FieldSpacecraftState state) { + return MathArrays.buildArray(field, 3); + } + }); + +// the adapted propagators do not manage the additional states from the reference, +// they simply forward them + Assert.assertFalse(adapterPropagator.isAdditionalStateManaged("dummy 1")); + Assert.assertFalse(adapterPropagator.isAdditionalStateManaged("dummy 2")); + Assert.assertTrue(adapterPropagator.isAdditionalStateManaged("dummy 3")); + + T maxDelta = zero.add(0); + T maxNominal = zero.add(0); + for (FieldAbsoluteDate t = t0.shiftedBy(dt.multiply(0.5)); t + .compareTo(withoutManeuver.getMaxDate()) < 0; t = t.shiftedBy(600.0)) { + FieldPVCoordinates pvWithout = withoutManeuver.getPVCoordinates(t, leo.getFrame()); + FieldPVCoordinates pvWith = withManeuver.getPVCoordinates(t, leo.getFrame()); + FieldPVCoordinates pvReverted = adapterPropagator.getPVCoordinates(t, leo.getFrame()); + T nominal = new FieldPVCoordinates<>(pvWithout, pvWith).getPosition().getNorm(); + T revertError = new FieldPVCoordinates<>(pvWithout, pvReverted).getPosition().getNorm(); + maxDelta = FastMath.max(maxDelta, revertError); + maxNominal = FastMath.max(maxNominal, nominal); + Assert.assertEquals(2, adapterPropagator.propagate(t).getAdditionalState("dummy 1").length); + Assert.assertEquals(1, adapterPropagator.propagate(t).getAdditionalState("dummy 2").length); + Assert.assertEquals(3, adapterPropagator.propagate(t).getAdditionalState("dummy 3").length); + } + Assert.assertTrue(maxDelta.getReal() < 120); + Assert.assertTrue(maxNominal.getReal() > 2800); + } + + private > FieldBoundedPropagator getEphemeris(final FieldOrbit leo, final T mass, final int nbOrbits, + final AttitudeProvider law, final FieldAbsoluteDate t0, final FieldVector3D zero2, final T f, final T isp, + final boolean sunAttraction, final boolean moonAttraction, + final NormalizedSphericalHarmonicsProvider gravityField) throws ParseException, IOException { + + FieldSpacecraftState initialState = new FieldSpacecraftState<>(leo, + law.getAttitude(leo, leo.getDate(), leo.getFrame()), mass); + +// add some dummy additional states + final T zero = mass.getField().getZero(); + initialState = initialState.addAdditionalState("dummy 1", zero.add(1.25), zero.add(2.5)); + initialState = initialState.addAdditionalState("dummy 2", zero.add(5.0)); + +// set up numerical propagator + final T dP = zero.add(1); + double[][] tolerances = FieldNumericalPropagator.tolerances(dP, leo, leo.getType()); + AdaptiveStepsizeFieldIntegrator integrator = new DormandPrince853FieldIntegrator<>(mass.getField(), 0.001, 1000.0, tolerances[0], + tolerances[1]); + integrator.setInitialStepSize(leo.getKeplerianPeriod().divide(100.0)); + final FieldNumericalPropagator propagator = new FieldNumericalPropagator<>(mass.getField(), integrator); + propagator.addAdditionalStateProvider(new FieldAdditionalStateProvider() { + public String getName() { + return "dummy 2"; + } + + public T[] getAdditionalState(FieldSpacecraftState state) { + T[] tab = MathArrays.buildArray(state.getDate().getField(), 1); + tab[0] = zero.add(5.0); + return tab; + } + + + }); + propagator.setInitialState(initialState); + propagator.setAttitudeProvider(law); + + if (zero2.getNorm().getReal() > 1.0e-6) { +// set up maneuver + final T vExhaust = isp.multiply(Constants.G0_STANDARD_GRAVITY); + final T dt = zero.subtract(mass.multiply(vExhaust).divide(f)).multiply(FastMath.expm1( zero.subtract(zero2.getNorm()).divide(vExhaust))); + final ConstantThrustManeuver maneuver = new ConstantThrustManeuver(t0.shiftedBy(dt.multiply(-0.5)).toAbsoluteDate(), dt.getReal(), f.getReal(), isp.getReal(), + zero2.normalize().toVector3D()); + propagator.addForceModel(maneuver); + } + + if (sunAttraction) { + propagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getSun())); + } + + if (moonAttraction) { + propagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getMoon())); + } + + if (gravityField != null) { + propagator.addForceModel(new HolmesFeatherstoneAttractionModel(FramesFactory.getGTOD(false), gravityField)); + } + + propagator.setEphemerisMode(); + propagator.propagate(t0.shiftedBy(leo.getKeplerianPeriod().multiply(nbOrbits))); + + final FieldBoundedPropagator ephemeris = propagator.getGeneratedEphemeris(); + +// both the initial propagator and generated ephemeris manage one of the two +// additional states, but they also contain unmanaged copies of the other one + Assert.assertFalse(propagator.isAdditionalStateManaged("dummy 1")); + Assert.assertTrue(propagator.isAdditionalStateManaged("dummy 2")); + Assert.assertFalse(ephemeris.isAdditionalStateManaged("dummy 1")); + Assert.assertTrue(ephemeris.isAdditionalStateManaged("dummy 2")); + Assert.assertEquals(2, ephemeris.getInitialState().getAdditionalState("dummy 1").length); + Assert.assertEquals(1, ephemeris.getInitialState().getAdditionalState("dummy 2").length); + + return ephemeris; + + } + + @Before + public void setUp() { + Utils.setDataRoot("regular-data:potential/icgem-format"); + } +} diff --git a/src/test/java/org/orekit/propagation/analytical/FieldEphemerisTest.java b/src/test/java/org/orekit/propagation/analytical/FieldEphemerisTest.java new file mode 100644 index 000000000..f5dfd2894 --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/FieldEphemerisTest.java @@ -0,0 +1,578 @@ +package org.orekit.propagation.analytical; + +import java.lang.reflect.InvocationTargetException; +import java.lang.reflect.Method; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldRotation; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.hipparchus.util.MathArrays; +import org.junit.Assert; +import org.junit.Before; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.attitudes.AttitudeProvider; +import org.orekit.attitudes.AttitudesSequence; +import org.orekit.attitudes.CelestialBodyPointed; +import org.orekit.attitudes.LofOffset; +import org.orekit.bodies.CelestialBodyFactory; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.errors.TimeStampedCacheException; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.frames.LOFType; +import org.orekit.orbits.FieldKeplerianOrbit; +import org.orekit.orbits.FieldOrbit; +import org.orekit.orbits.PositionAngle; +import org.orekit.propagation.FieldAdditionalStateProvider; +import org.orekit.propagation.FieldPropagator; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.SpacecraftState; +import org.orekit.propagation.events.DateDetector; +import org.orekit.propagation.events.handlers.ContinueOnEvent; +import org.orekit.propagation.numerical.FieldNumericalPropagator; +import org.orekit.propagation.sampling.FieldOrekitFixedStepHandler; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.DateComponents; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeComponents; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.AbsolutePVCoordinates; +import org.orekit.utils.AngularDerivativesFilter; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldAbsolutePVCoordinates; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.PVCoordinates; +import org.orekit.utils.TimeStampedFieldPVCoordinates; +import org.orekit.utils.TimeStampedPVCoordinates; + +public class FieldEphemerisTest { + + @Before + public void setUp() throws IllegalArgumentException, OrekitException { + Utils.setDataRoot("regular-data"); + } + + private > T calculatePositionDelta(FieldSpacecraftState state1, + FieldSpacecraftState state2) { + return FieldVector3D.distance(state1.getPVCoordinates().getPosition(), state2.getPVCoordinates().getPosition()); + } + + private > T calculateVelocityDelta(FieldSpacecraftState state1, + FieldSpacecraftState state2) { + return FieldVector3D.distance(state1.getPVCoordinates().getVelocity(), state2.getPVCoordinates().getVelocity()); + } + + private > T calculateAttitudeDelta(FieldSpacecraftState state1, + FieldSpacecraftState state2) { + return FieldRotation.distance(state1.getAttitude().getRotation(), state2.getAttitude().getRotation()); + } + + // TESTS + + @Test + public void testAttitudeOverride() { + doTestAttitudeOverride(Decimal64Field.getInstance()); + } + + @Test + public void testAttitudeSequenceTransition() { + doTestAttitudeSequenceTransition(Decimal64Field.getInstance()); + } + + @Test + public void testNonResettableState() { + doTestNonResettableState(Decimal64Field.getInstance()); + } + + @Test + public void testAdditionalStates() { + doTestAdditionalStates(Decimal64Field.getInstance()); + } + + @Test + public void testProtectedMethods() throws NoSuchMethodException, SecurityException, IllegalAccessException, IllegalArgumentException, InvocationTargetException { + doTestProtectedMethods(Decimal64Field.getInstance()); + } + + @Test + public void testExtrapolation() { + doTestExtrapolation(Decimal64Field.getInstance()); + } + + @Test + public void testIssue662() { + doTestIssue662(Decimal64Field.getInstance()); + } + + @Test + public void testIssue680() { + doTestIssue680(Decimal64Field.getInstance()); + } + + public > void doTestAttitudeOverride(Field field) { + final T zero = field.getZero(); + // Conversion from double to Field + FieldAbsoluteDate initDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC())); + FieldAbsoluteDate finalDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC())); + T a = zero.add(7187990.1979844316); + T e = zero.add(0.5e-4); + T i = zero.add(1.7105407051081795); + T omega = zero.add(1.9674147913622104); + T OMEGA = zero.add(FastMath.toRadians(261)); + T lv = zero; + T mu = zero.add(3.9860047e14); + Frame inertialFrame = FramesFactory.getEME2000(); + + FieldOrbit initialState = new FieldKeplerianOrbit<>(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE, + inertialFrame, initDate, mu); + FieldPropagator propagator = new FieldKeplerianPropagator<>(initialState); + + // + + final T positionTolerance = zero.add(1e-6); + final T velocityTolerance = zero.add(1e-5); + final T attitudeTolerance = zero.add(1e-6); + + int numberOfInterals = 1440; + T deltaT = finalDate.durationFrom(initDate).divide(numberOfInterals); + + propagator.setAttitudeProvider(new LofOffset(inertialFrame, LOFType.VVLH)); + + List states = new ArrayList(numberOfInterals + 1); + for (int j = 0; j <= numberOfInterals; j++) { + states.add(propagator.propagate(initDate.shiftedBy((deltaT.multiply(j)))).toSpacecraftState()); + } + + int numInterpolationPoints = 2; + FieldEphemeris ephemPropagator = new FieldEphemeris<>(field, states, numInterpolationPoints); + Assert.assertEquals(0, ephemPropagator.getManagedAdditionalStates().length); + + // First test that we got position, velocity and attitude nailed + int numberEphemTestIntervals = 2880; + deltaT = finalDate.durationFrom(initDate).divide(numberEphemTestIntervals); + for (int j = 0; j <= numberEphemTestIntervals; j++) { + FieldAbsoluteDate currentDate = initDate.shiftedBy(deltaT.multiply(j)); + FieldSpacecraftState ephemState = ephemPropagator.propagate(currentDate); + FieldSpacecraftState keplerState = propagator.propagate(currentDate); + T positionDelta = calculatePositionDelta(ephemState, keplerState); + T velocityDelta = calculateVelocityDelta(ephemState, keplerState); + T attitudeDelta = calculateAttitudeDelta(ephemState, keplerState); + Assert.assertEquals("VVLH Unmatched Position at: " + currentDate, 0.0, positionDelta.getReal(), + positionTolerance.getReal()); + Assert.assertEquals("VVLH Unmatched Velocity at: " + currentDate, 0.0, velocityDelta.getReal(), + velocityTolerance.getReal()); + Assert.assertEquals("VVLH Unmatched Attitude at: " + currentDate, 0.0, attitudeDelta.getReal(), + attitudeTolerance.getReal()); + } + + // Now force an override on the attitude and check it against a Keplerian + // propagator + // setup identically to the first but with a different attitude + // If override isn't working this will fail. + propagator = new FieldKeplerianPropagator<>(propagator.getInitialState().getOrbit()); + propagator.setAttitudeProvider(new LofOffset(inertialFrame, LOFType.QSW)); + + ephemPropagator.setAttitudeProvider(new LofOffset(inertialFrame, LOFType.QSW)); + for (int j = 0; j <= numberEphemTestIntervals; j++) { + FieldAbsoluteDate currentDate = initDate.shiftedBy(deltaT.multiply(j)); + FieldSpacecraftState ephemState = ephemPropagator.propagate(currentDate); + FieldSpacecraftState keplerState = propagator.propagate(currentDate); + T positionDelta = calculatePositionDelta(ephemState, keplerState); + T velocityDelta = calculateVelocityDelta(ephemState, keplerState); + T attitudeDelta = calculateAttitudeDelta(ephemState, keplerState); + Assert.assertEquals("QSW Unmatched Position at: " + currentDate, 0.0, positionDelta.getReal(), + positionTolerance.getReal()); + Assert.assertEquals("QSW Unmatched Velocity at: " + currentDate, 0.0, velocityDelta.getReal(), + velocityTolerance.getReal()); + Assert.assertEquals("QSW Unmatched Attitude at: " + currentDate, 0.0, attitudeDelta.getReal(), + attitudeTolerance.getReal()); + } + } + + public > void doTestAttitudeSequenceTransition(Field field) { + final T zero = field.getZero(); + // Initialize the orbit + final FieldAbsoluteDate initialDate = new FieldAbsoluteDate<>(field, 2003, 01, 01, 0, 0, 00.000, + TimeScalesFactory.getUTC()); + final FieldVector3D position = new FieldVector3D<>(zero.add(-39098981.4866597), zero.add(-15784239.3610601), + zero.add(78908.2289853595)); + final FieldVector3D velocity = new FieldVector3D<>(zero.add(1151.00321021175), zero.add(-2851.14864755189), + zero.add(-2.02133248357321)); + final FieldOrbit initialOrbit = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(position, velocity), + FramesFactory.getGCRF(), initialDate, zero.add(Constants.WGS84_EARTH_MU)); + final FieldSpacecraftState initialState = new FieldSpacecraftState<>(initialOrbit); + + // Define attitude laws + AttitudeProvider before = new CelestialBodyPointed(FramesFactory.getICRF(), CelestialBodyFactory.getSun(), + Vector3D.PLUS_K, Vector3D.PLUS_I, Vector3D.PLUS_K); + AttitudeProvider after = new CelestialBodyPointed(FramesFactory.getICRF(), CelestialBodyFactory.getEarth(), + Vector3D.PLUS_K, Vector3D.PLUS_I, Vector3D.PLUS_K); + + // Define attitude sequence + FieldAbsoluteDate switchDate = initialDate.shiftedBy(86400.0); + double transitionTime = 600; + DateDetector switchDetector = new DateDetector(switchDate.toAbsoluteDate()).withHandler(new ContinueOnEvent()); + + AttitudesSequence attitudeSequence = new AttitudesSequence(); + attitudeSequence.resetActiveProvider(before); + attitudeSequence.addSwitchingCondition(before, after, switchDetector, true, false, transitionTime, AngularDerivativesFilter.USE_RR, null); + + FieldNumericalPropagator propagator = new FieldNumericalPropagator<>(field, + new DormandPrince853FieldIntegrator<>(field, 0.1, 500, 1e-9, 1e-9)); + propagator.setInitialState(initialState); + + // Propagate and build ephemeris + final List propagatedStates = new ArrayList<>(); + + propagator.setMasterMode(zero.add(60), new FieldOrekitFixedStepHandler() { + + @Override + public void handleStep(FieldSpacecraftState currentState, boolean isLast) throws OrekitException { + propagatedStates.add(currentState.toSpacecraftState()); + } + }); + propagator.propagate(initialDate.shiftedBy(zero.add(2 * 86400.0))); + final FieldEphemeris ephemeris = new FieldEphemeris<>(field, propagatedStates, 8); + + // Add attitude switch event to ephemeris + ephemeris.setAttitudeProvider(attitudeSequence); + attitudeSequence.registerSwitchEvents(field, ephemeris); + + // Propagate with a step during the transition + FieldAbsoluteDate endDate = initialDate.shiftedBy(zero.add(2 * 86400.0)); + FieldSpacecraftState stateBefore = ephemeris.getInitialState(); + ephemeris.propagate(switchDate.shiftedBy(transitionTime/2)); + FieldSpacecraftState stateAfter = ephemeris.propagate(endDate); + + // Check that the attitudes are correct + Assert.assertEquals(before.getAttitude(stateBefore.getOrbit(), stateBefore.getDate(), stateBefore.getFrame()) + .getRotation().getQ0().getReal(), stateBefore.getAttitude().getRotation().getQ0().getReal(), 1.0E-16); + Assert.assertEquals(before.getAttitude(stateBefore.getOrbit(), stateBefore.getDate(), stateBefore.getFrame()) + .getRotation().getQ1().getReal(), stateBefore.getAttitude().getRotation().getQ1().getReal(), 1.0E-16); + Assert.assertEquals(before.getAttitude(stateBefore.getOrbit(), stateBefore.getDate(), stateBefore.getFrame()) + .getRotation().getQ2().getReal(), stateBefore.getAttitude().getRotation().getQ2().getReal(), 1.0E-16); + Assert.assertEquals(before.getAttitude(stateBefore.getOrbit(), stateBefore.getDate(), stateBefore.getFrame()) + .getRotation().getQ3().getReal(), stateBefore.getAttitude().getRotation().getQ3().getReal(), 1.0E-16); + + Assert.assertEquals(after.getAttitude(stateAfter.getOrbit(), stateAfter.getDate(), stateAfter.getFrame()) + .getRotation().getQ0().getReal(), stateAfter.getAttitude().getRotation().getQ0().getReal(), 1.0E-16); + Assert.assertEquals(after.getAttitude(stateAfter.getOrbit(), stateAfter.getDate(), stateAfter.getFrame()) + .getRotation().getQ1().getReal(), stateAfter.getAttitude().getRotation().getQ1().getReal(), 1.0E-16); + Assert.assertEquals(after.getAttitude(stateAfter.getOrbit(), stateAfter.getDate(), stateAfter.getFrame()) + .getRotation().getQ2().getReal(), stateAfter.getAttitude().getRotation().getQ2().getReal(), 1.0E-16); + Assert.assertEquals(after.getAttitude(stateAfter.getOrbit(), stateAfter.getDate(), stateAfter.getFrame()) + .getRotation().getQ3().getReal(), stateAfter.getAttitude().getRotation().getQ3().getReal(), 1.0E-16); + } + + public > void doTestNonResettableState(Field field) { + try { + final T zero = field.getZero(); + // Conversion from double to Field + FieldAbsoluteDate initDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC())); + FieldAbsoluteDate finalDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC())); + T a = zero.add(7187990.1979844316); + T e = zero.add(0.5e-4); + T i = zero.add(1.7105407051081795); + T omega = zero.add(1.9674147913622104); + T OMEGA = zero.add(FastMath.toRadians(261)); + T lv = zero; + T mu = zero.add(3.9860047e14); + Frame inertialFrame = FramesFactory.getEME2000(); + + FieldOrbit initialState = new FieldKeplerianOrbit<>(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE, + inertialFrame, initDate, mu); + FieldPropagator propagator = new FieldKeplerianPropagator<>(initialState); + + // + propagator.setAttitudeProvider(new LofOffset(inertialFrame, LOFType.VVLH)); + + List states = new ArrayList(); + for (double dt = 0; dt >= -1200; dt -= 60.0) { + states.add(propagator.propagate(initDate.shiftedBy(dt)).toSpacecraftState()); + } + + new FieldEphemeris<>(field, states, 2).resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + public > void doTestAdditionalStates(Field field) { + final T zero = field.getZero(); + // Conversion from double to Field + FieldAbsoluteDate initDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC())); + FieldAbsoluteDate finalDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC())); + T a = zero.add(7187990.1979844316); + T e = zero.add(0.5e-4); + T i = zero.add(1.7105407051081795); + T omega = zero.add(1.9674147913622104); + T OMEGA = zero.add(FastMath.toRadians(261)); + T lv = zero; + T mu = zero.add(3.9860047e14); + Frame inertialFrame = FramesFactory.getEME2000(); + + FieldOrbit initialState = new FieldKeplerianOrbit<>(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE, + inertialFrame, initDate, mu); + FieldPropagator propagator = new FieldKeplerianPropagator<>(initialState); + + // + final String name1 = "dt0"; + final String name2 = "dt1"; + propagator.setAttitudeProvider(new LofOffset(inertialFrame, LOFType.VVLH)); + + List states = new ArrayList(); + for (double dt = 0; dt >= -1200; dt -= 60.0) { + final FieldSpacecraftState original = propagator.propagate(initDate.shiftedBy(dt)); + final FieldSpacecraftState expanded = original.addAdditionalState(name2, + original.getDate().durationFrom(finalDate)); + states.add(expanded.toSpacecraftState()); + } + + final FieldPropagator ephem = new FieldEphemeris<>(field, states, 2); + ephem.addAdditionalStateProvider(new FieldAdditionalStateProvider() { + public String getName() { + return name1; + } + + @Override + public T[] getAdditionalState(FieldSpacecraftState state) { + T[] tab = MathArrays.buildArray(field, 1); + tab[0] = state.getDate().durationFrom(initDate); + return tab; + } + }); + + final String[] additional = ephem.getManagedAdditionalStates(); + Arrays.sort(additional); + Assert.assertEquals(2, additional.length); + Assert.assertEquals(name1, ephem.getManagedAdditionalStates()[0]); + Assert.assertEquals(name2, ephem.getManagedAdditionalStates()[1]); + Assert.assertTrue(ephem.isAdditionalStateManaged(name1)); + Assert.assertTrue(ephem.isAdditionalStateManaged(name2)); + Assert.assertFalse(ephem.isAdditionalStateManaged("not managed")); + + FieldSpacecraftState s = ephem.propagate(initDate.shiftedBy(-270.0)); + Assert.assertEquals(-270.0, s.getAdditionalState(name1)[0].getReal(), 1.0e-15); + Assert.assertEquals(-86670.0, s.getAdditionalState(name2)[0].getReal(), 1.0e-15); + } + + public > void doTestProtectedMethods(Field field) + throws NoSuchMethodException, SecurityException, IllegalAccessException, IllegalArgumentException, InvocationTargetException { + final T zero = field.getZero(); + // Conversion from double to Field + FieldAbsoluteDate initDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC())); + FieldAbsoluteDate finalDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC())); + T a = zero.add(7187990.1979844316); + T e = zero.add(0.5e-4); + T i = zero.add(1.7105407051081795); + T omega = zero.add(1.9674147913622104); + T OMEGA = zero.add(FastMath.toRadians(261)); + T lv = zero; + T mu = zero.add(3.9860047e14); + Frame inertialFrame = FramesFactory.getEME2000(); + + FieldOrbit initialState = new FieldKeplerianOrbit<>(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE, + inertialFrame, initDate, mu); + FieldPropagator propagator = new FieldKeplerianPropagator<>(initialState); + + // + propagator.setAttitudeProvider(new LofOffset(inertialFrame, LOFType.VVLH)); + + List states = new ArrayList(); + for (double dt = 0; dt >= -1200; dt -= 60.0) { + final FieldSpacecraftState original = propagator.propagate(initDate.shiftedBy(dt)); + final FieldSpacecraftState modified = new FieldSpacecraftState(original.getOrbit(), + original.getAttitude(), original.getMass().subtract(0.0625 * dt)); + states.add(modified.toSpacecraftState()); + } + + final FieldPropagator ephem = new FieldEphemeris<>(field, states, 2); + Method propagateOrbit = FieldEphemeris.class.getDeclaredMethod("propagateOrbit", FieldAbsoluteDate.class); + propagateOrbit.setAccessible(true); + Method getMass = FieldEphemeris.class.getDeclaredMethod("getMass", FieldAbsoluteDate.class); + getMass.setAccessible(true); + + FieldSpacecraftState s = ephem.propagate(initDate.shiftedBy(-270.0)); + FieldOrbit o = (FieldOrbit) propagateOrbit.invoke(ephem, s.getDate()); + //double m = ((Double) getMass.invoke(ephem, s.getDate())).doubleValue(); + T m = ((T) getMass.invoke(ephem, s.getDate())); + Assert.assertEquals(0.0, FieldVector3D + .distance(s.getPVCoordinates().getPosition(), o.getPVCoordinates().getPosition()).getReal(), 1.0e-15); + Assert.assertEquals(s.getMass().getReal(), m.getReal(), 1.0e-15); + } + + public > void doTestExtrapolation(Field field) { + final T zero = field.getZero(); + // Conversion from double to Field + FieldAbsoluteDate initDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC())); + FieldAbsoluteDate finalDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC())); + T a = zero.add(7187990.1979844316); + T e = zero.add(0.5e-4); + T i = zero.add(1.7105407051081795); + T omega = zero.add(1.9674147913622104); + T OMEGA = zero.add(FastMath.toRadians(261)); + T lv = zero; + T mu = zero.add(3.9860047e14); + Frame inertialFrame = FramesFactory.getEME2000(); + + FieldOrbit initialState = new FieldKeplerianOrbit<>(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE, + inertialFrame, initDate, mu); + FieldPropagator propagator = new FieldKeplerianPropagator<>(initialState); + + // + double dt = finalDate.durationFrom(initDate).getReal(); + double timeStep = dt / 20.0; + List states = new ArrayList(); + + for (double t = 0; t <= dt; t += timeStep) { + states.add(propagator.propagate(initDate.shiftedBy(t)).toSpacecraftState()); + } + + final int interpolationPoints = 5; + FieldEphemeris ephemeris = new FieldEphemeris<>(field, states, interpolationPoints); + Assert.assertEquals(finalDate, ephemeris.getMaxDate()); + + T tolerance = ephemeris.getExtrapolationThreshold(); + + ephemeris.propagate(ephemeris.getMinDate()); + ephemeris.propagate(ephemeris.getMaxDate()); + ephemeris.propagate(ephemeris.getMinDate().shiftedBy(zero.subtract(tolerance).divide(2.0))); + ephemeris.propagate(ephemeris.getMaxDate().shiftedBy(tolerance.divide(2.0))); + + try { + ephemeris.propagate(ephemeris.getMinDate().shiftedBy(tolerance.multiply(-2.0))); + Assert.fail("an exception should have been thrown"); + } catch (TimeStampedCacheException ex) { + // supposed to fail since out of bounds + } + + try { + ephemeris.propagate(ephemeris.getMaxDate().shiftedBy(tolerance.multiply(+2.0))); + Assert.fail("an exception should have been thrown"); + } catch (TimeStampedCacheException ex) { + // supposed to fail since out of bounds + } + } + + public > void doTestIssue662(Field field) { + final T zero = field.getZero(); + // Conversion from double to Field + FieldAbsoluteDate initDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC())); + FieldAbsoluteDate finalDate = new FieldAbsoluteDate<>(field, + new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC())); + T a = zero.add(7187990.1979844316); + T e = zero.add(0.5e-4); + T i = zero.add(1.7105407051081795); + T omega = zero.add(1.9674147913622104); + T OMEGA = zero.add(FastMath.toRadians(261)); + T lv = zero; + T mu = zero.add(3.9860047e14); + Frame inertialFrame = FramesFactory.getEME2000(); + + FieldOrbit initialState = new FieldKeplerianOrbit<>(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE, + inertialFrame, initDate, mu); + FieldPropagator propagator = new FieldKeplerianPropagator<>(initialState); + + // + + // Input parameters + int numberOfInterals = 1440; + T deltaT = finalDate.durationFrom(initDate).divide(numberOfInterals); + + // Build the list of spacecraft states + String additionalName = "testValue"; + T additionalValue = zero.add(1.0); + List states = new ArrayList(numberOfInterals + 1); + for (int j = 0; j <= numberOfInterals; j++) { + states.add(propagator.propagate(initDate.shiftedBy((deltaT.multiply(j)))).addAdditionalState(additionalName, + additionalValue).toSpacecraftState()); + } + + // Build the epemeris propagator + FieldEphemeris ephemPropagator = new FieldEphemeris<>(field, states, 2); + + // State before adding an attitude provider + FieldSpacecraftState stateBefore = ephemPropagator.propagate(ephemPropagator.getMaxDate().shiftedBy(-60.0)); + Assert.assertEquals(1, stateBefore.getAdditionalState(additionalName).length); + Assert.assertEquals(additionalValue.getReal(), stateBefore.getAdditionalState(additionalName)[0].getReal(), + Double.MIN_VALUE); + + // Set an attitude provider + ephemPropagator.setAttitudeProvider(new LofOffset(inertialFrame, LOFType.VVLH)); + + // State after adding an attitude provider + FieldSpacecraftState stateAfter = ephemPropagator.propagate(ephemPropagator.getMaxDate().shiftedBy(-60.0)); + Assert.assertEquals(1, stateAfter.getAdditionalState(additionalName).length); + Assert.assertEquals(additionalValue.getReal(), stateAfter.getAdditionalState(additionalName)[0].getReal(), + Double.MIN_VALUE); + + } + + public > void doTestIssue680(Field field) { + + // Conversion from double to Field + AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC()); + AbsoluteDate finalDate = new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC()); + Frame inertialFrame = FramesFactory.getEME2000(); + + // Initial PV coordinates + AbsolutePVCoordinates initPV = new AbsolutePVCoordinates(inertialFrame, + new TimeStampedPVCoordinates(initDate, + new PVCoordinates(new Vector3D(-29536113.0, 30329259.0, -100125.0), + new Vector3D(-2194.0, -2141.0, -8.0)))); + + // Input parameters + int numberOfInterals = 1440; + double deltaT = finalDate.durationFrom(initDate)/((double)numberOfInterals); + + // Build the list of spacecraft states + List states = new ArrayList(numberOfInterals + 1); + for (int j = 0; j<= numberOfInterals; j++) { + states.add(new SpacecraftState(initPV).shiftedBy(j * deltaT)); + } + + // Build the epemeris propagator + FieldEphemeris ephemPropagator = new FieldEphemeris<>(field, states, 2); + + // Get initial state without attitude provider + FieldSpacecraftState withoutAttitudeProvider = ephemPropagator.getInitialState(); + Assert.assertEquals(0.0, + FieldVector3D.distance(withoutAttitudeProvider.getAbsPVA().getPosition(), initPV.getPosition()).getReal(), 1.0e-10); + Assert.assertEquals(0.0, + FieldVector3D.distance(withoutAttitudeProvider.getAbsPVA().getVelocity(), initPV.getVelocity()).getReal(), 1.0e-10); + + // Set an attitude provider + ephemPropagator.setAttitudeProvider(new LofOffset(inertialFrame, LOFType.VVLH)); + + // Get initial state with attitude provider + FieldSpacecraftState withAttitudeProvider = ephemPropagator.getInitialState(); + Assert.assertEquals(0.0, + FieldVector3D.distance(withAttitudeProvider.getAbsPVA().getPosition(), initPV.getPosition()).getReal(), 1.0e-10); + Assert.assertEquals(0.0, + FieldVector3D.distance(withAttitudeProvider.getAbsPVA().getVelocity(), initPV.getVelocity()).getReal(), 1.0e-10); + + } + +} diff --git a/src/test/java/org/orekit/propagation/analytical/FieldJ2DifferentialEffect.java b/src/test/java/org/orekit/propagation/analytical/FieldJ2DifferentialEffect.java new file mode 100644 index 000000000..e2b486207 --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/FieldJ2DifferentialEffect.java @@ -0,0 +1,239 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.util.FastMath; +import org.hipparchus.util.FieldSinCos; +import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider; +import org.orekit.orbits.FieldEquinoctialOrbit; +import org.orekit.orbits.FieldOrbit; +import org.orekit.orbits.OrbitType; +import org.orekit.orbits.PositionAngle; +import org.orekit.propagation.FieldSpacecraftState; +import org.orekit.propagation.SpacecraftState; +import org.orekit.time.FieldAbsoluteDate; + +public class FieldJ2DifferentialEffect> + implements FieldAdapterPropagator.FieldDifferentialEffect { + /** Reference date. */ + private final FieldAbsoluteDate referenceDate; + + /** Differential drift on perigee argument. */ + private final T dPaDot; + + /** Differential drift on ascending node. */ + private final T dRaanDot; + + /** Indicator for applying effect before reference date. */ + private final boolean applyBefore; + + /** + * Simple constructor. + *

+ * The {@code applyBefore} parameter is mainly used when the differential effect + * is associated with a maneuver. In this case, the parameter must be set to + * {@code false}. + *

+ * + * @param original original state at reference date + * @param directEffect direct effect changing the orbit + * @param applyBefore if true, effect is applied both before and after + * reference date, if false it is only applied after + * reference date + * @param gravityField gravity field to use + */ + public FieldJ2DifferentialEffect(Field field, final FieldSpacecraftState original, + final FieldAdapterPropagator.FieldDifferentialEffect directEffect, final boolean applyBefore, + final UnnormalizedSphericalHarmonicsProvider gravityField) { + this(field, original, directEffect, applyBefore, gravityField.getAe(), gravityField.getMu(), + -gravityField.onDate(original.getDate().toAbsoluteDate()).getUnnormalizedCnm(2, 0)); + } + + /** + * Simple constructor. + *

+ * The {@code applyBefore} parameter is mainly used when the differential effect + * is associated with a maneuver. In this case, the parameter must be set to + * {@code false}. + *

+ * + * @param orbit0 original orbit at reference date + * @param orbit1 shifted orbit at reference date + * @param applyBefore if true, effect is applied both before and after + * reference date, if false it is only applied after + * reference date + * @param gravityField gravity field to use + */ + public FieldJ2DifferentialEffect(Field field, final FieldOrbit orbit0, final FieldOrbit orbit1, + final boolean applyBefore, final UnnormalizedSphericalHarmonicsProvider gravityField) { + this(field, orbit0, orbit1, applyBefore, gravityField.getAe(), gravityField.getMu(), + -gravityField.onDate(orbit0.getDate().toAbsoluteDate()).getUnnormalizedCnm(2, 0)); + } + + /** + * Simple constructor. + *

+ * The {@code applyBefore} parameter is mainly used when the differential effect + * is associated with a maneuver. In this case, the parameter must be set to + * {@code false}. + *

+ * + * @param original original state at reference date + * @param directEffect direct effect changing the orbit + * @param applyBefore if true, effect is applied both before and after + * reference date, if false it is only applied after + * reference date + * @param referenceRadius reference radius of the Earth for the potential model + * (m) + * @param mu central attraction coefficient (m³/s²) + * @param j2 un-normalized zonal coefficient (about +1.08e-3 for + * Earth) + */ + public FieldJ2DifferentialEffect(Field field, final FieldSpacecraftState original, + final FieldAdapterPropagator.FieldDifferentialEffect directEffect, final boolean applyBefore, + final double referenceRadius, final double mu, final double j2) { + this(field, original.getOrbit(), directEffect.apply(original.shiftedBy(0.001)).getOrbit().shiftedBy(-0.001), + applyBefore, referenceRadius, mu, j2); + } + + /** + * Simple constructor. + *

+ * The {@code applyBefore} parameter is mainly used when the differential effect + * is associated with a maneuver. In this case, the parameter must be set to + * {@code false}. + *

+ * + * @param orbit0 original orbit at reference date + * @param orbit1 shifted orbit at reference date + * @param applyBefore if true, effect is applied both before and after + * reference date, if false it is only applied after + * reference date + * @param referenceRadius reference radius of the Earth for the potential model + * (m) + * @param mu central attraction coefficient (m³/s²) + * @param j2 un-normalized zonal coefficient (about +1.08e-3 for + * Earth) + */ + public FieldJ2DifferentialEffect(Field field, final FieldOrbit orbit0, final FieldOrbit orbit1, + final boolean applyBefore, final double referenceRadius, double mu, double j2) { + + this.referenceDate = orbit0.getDate(); + this.applyBefore = applyBefore; + + // extract useful parameters + final T a0 = orbit0.getA(); + final T e0 = orbit0.getE(); + final T i0 = orbit0.getI(); + final T a1 = orbit1.getA(); + final T e1 = orbit1.getE(); + final T i1 = orbit1.getI(); + + // compute reference drifts + final T zero = field.getZero(); + final T oMe2 = zero.add(1).subtract(e0.multiply(e0)); + final T ratio = zero.add(referenceRadius).divide(a0.multiply(oMe2)); + final FieldSinCos scI = FastMath.sinCos(i0); + final T n = FastMath.sqrt(zero.add(mu).divide(a0)).divide(a0); + final T c = ratio.multiply(ratio).multiply(n).multiply(j2); + final T refPaDot = zero.add(0.75).multiply(c) + .multiply(zero.add(4).subtract(zero.add(5).multiply(scI.sin()).multiply(scI.sin()))); + final T refRaanDot = zero.add(-1.5).multiply(c).multiply(scI.cos()); + + // differential model on perigee argument drift + final T dPaDotDa = zero.add(-3.5).multiply(refPaDot).divide(a0); + final T dPaDotDe = zero.add(4).multiply(refPaDot).multiply(e0).divide(oMe2); + final T dPaDotDi = zero.add(-7.5).multiply(c).multiply(scI.sin()).multiply(scI.cos()); + dPaDot = dPaDotDa.multiply(a1.subtract(a0)).add(dPaDotDe.multiply(e1.subtract(e0))) + .add(dPaDotDi.multiply(i1.subtract(i0))); + + // differential model on ascending node drift + final T dRaanDotDa = zero.add(-3.5).multiply(refRaanDot).divide(a0); + final T dRaanDotDe = zero.add(4).multiply(refRaanDot).multiply(e0).divide(oMe2); + final T dRaanDotDi = zero.subtract(refRaanDot).multiply(FastMath.tan(i0)); + dRaanDot = dRaanDotDa.multiply(a1.subtract(a0)).add(dRaanDotDe.multiply(e1.subtract(e0))).add(dRaanDotDi.multiply(i1.subtract(i0))); + } + + /** + * Compute the effect of the maneuver on an orbit. + * + * @param orbit1 original orbit at t₁, without maneuver + * @return orbit at t₁, taking the maneuver into account if t₁ > t₀ + * @see #apply(SpacecraftState) + */ + public FieldOrbit apply(final FieldOrbit orbit1) { + + if (orbit1.getDate().compareTo(referenceDate) <= 0 && !applyBefore) { + // the orbit change has not occurred yet, don't change anything + return orbit1; + } + + return updateOrbit(orbit1); + + } + + /** {@inheritDoc} */ + public FieldSpacecraftState apply(final FieldSpacecraftState state1) { + + if (state1.getDate().compareTo(referenceDate) <= 0 && !applyBefore) { + // the orbit change has not occurred yet, don't change anything + return state1; + } + + return new FieldSpacecraftState<>(updateOrbit(state1.getOrbit()), state1.getAttitude(), state1.getMass()); + + } + + /** + * Compute the differential effect of J2 on an orbit. + * + * @param orbit1 original orbit at t₁, without differential J2 + * @return orbit at t₁, always taking the effect into account + */ + private FieldOrbit updateOrbit(final FieldOrbit orbit1) { + + // convert current orbital state to equinoctial elements + final FieldEquinoctialOrbit original = (FieldEquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(orbit1); + + // compute differential effect + final FieldAbsoluteDate date = original.getDate(); + final T dt = date.durationFrom(referenceDate); + final T dPaRaan = (dPaDot.add(dRaanDot)).multiply(dt); + final FieldSinCos scPaRaan = FastMath.sinCos(dPaRaan); + final T dRaan = dRaanDot.multiply(dt); + final FieldSinCos scRaan = FastMath.sinCos(dRaan); + + final T ex = original.getEquinoctialEx().multiply(scPaRaan.cos()) + .subtract(original.getEquinoctialEy().multiply(scPaRaan.sin())); + final T ey = original.getEquinoctialEx().multiply(scPaRaan.sin()) + .add(original.getEquinoctialEy().multiply(scPaRaan.cos())); + final T hx = original.getHx().multiply(scRaan.cos()).subtract(original.getHy().multiply(scRaan.sin())); + final T hy = original.getHx().multiply(scRaan.sin()).add(original.getHy().multiply(scRaan.cos())); + final T lambda = original.getLv().add(dPaRaan); + + // build updated orbit + final FieldEquinoctialOrbit updated = new FieldEquinoctialOrbit<>(original.getA(), ex, ey, hx, hy, lambda, + PositionAngle.TRUE, original.getFrame(), date, original.getMu()); + + // convert to required type + return orbit1.getType().convertType(updated); + + } + +} diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/BeidouPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/BeidouPropagatorTest.java index dcbea167c..529b47872 100644 --- a/src/test/java/org/orekit/propagation/analytical/gnss/BeidouPropagatorTest.java +++ b/src/test/java/org/orekit/propagation/analytical/gnss/BeidouPropagatorTest.java @@ -42,285 +42,284 @@ import org.orekit.utils.TimeStampedPVCoordinates; public class BeidouPropagatorTest { - private static BeidouAlmanac almanac; - - @BeforeClass - public static void setUpBeforeClass() { - Utils.setDataRoot("gnss"); - - // Almanac for satellite 18 for May 28th 2019 - almanac = new BeidouAlmanac(18, 694, 4096, 6493.3076, 0.00482368, - 0.0, -0.01365602, 1.40069711, -2.11437379e-9, - 3.11461541, -2.53029382, 0.0001096725, 7.27596e-12, 0); - } - - @Test - public void testBeidouCycle() { - // Builds the BeiDou propagator from the almanac - final BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); - // Intermediate verification - Assert.assertEquals(18, almanac.getPRN()); - Assert.assertEquals(0, almanac.getHealth()); - Assert.assertEquals(0.0001096725, almanac.getAf0(), 1.0e-15); - Assert.assertEquals(7.27596e-12, almanac.getAf1(), 1.0e-15); - // Propagate at the BeiDou date and one BeiDou cycle later - final AbsoluteDate date0 = almanac.getDate(); - final Vector3D p0 = propagator.propagateInEcef(date0).getPosition(); - final double bdtCycleDuration = BeidouOrbitalElements.BEIDOU_WEEK_IN_SECONDS * BeidouOrbitalElements.BEIDOU_WEEK_NB; - final AbsoluteDate date1 = date0.shiftedBy(bdtCycleDuration); - final Vector3D p1 = propagator.propagateInEcef(date1).getPosition(); - - // Checks - Assert.assertEquals(0., p0.distance(p1), 0.); - } - - @Test - public void testFrames() { - // Builds the BeiDou propagator from the almanac - final BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); - Assert.assertEquals("EME2000", propagator.getFrame().getName()); - Assert.assertEquals(3.986004418e+14, BeidouOrbitalElements.BEIDOU_MU, 1.0e6); - // Defines some date - final AbsoluteDate date = new AbsoluteDate(2016, 3, 3, 12, 0, 0., TimeScalesFactory.getUTC()); - // Get PVCoordinates at the date in the ECEF - final PVCoordinates pv0 = propagator.propagateInEcef(date); - // Get PVCoordinates at the date in the ECEF - final PVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); - - // Checks - Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()), 3.3e-8); - Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()), 3.9e-12); - } - - @Test - public void testNoReset() { - try { - BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); - propagator.resetInitialState(propagator.getInitialState()); - Assert.fail("an exception should have been thrown"); - } catch (OrekitException oe) { - Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); - } - try { - BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); - propagator.resetIntermediateState(propagator.getInitialState(), true); - Assert.fail("an exception should have been thrown"); - } catch (OrekitException oe) { - Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); - } - } - - @Test - public void testDerivativesConsistency() { - - final Frame eme2000 = FramesFactory.getEME2000(); - double errorP = 0; - double errorV = 0; - double errorA = 0; - BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); - BeidouOrbitalElements elements = propagator.getBeidouOrbitalElements(); - AbsoluteDate t0 = new GNSSDate(elements.getWeek(), 0.001 * elements.getTime(), SatelliteSystem.BEIDOU).getDate(); - for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { - final AbsoluteDate central = t0.shiftedBy(dt); - final PVCoordinates pv = propagator.getPVCoordinates(central, eme2000); - final double h = 10.0; - List sample = new ArrayList(); - for (int i = -3; i <= 3; ++i) { - sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); - } - final PVCoordinates interpolated = - TimeStampedPVCoordinates.interpolate(central, - CartesianDerivativesFilter.USE_P, - sample); - errorP = FastMath.max(errorP, Vector3D.distance(pv.getPosition(), interpolated.getPosition())); - errorV = FastMath.max(errorV, Vector3D.distance(pv.getVelocity(), interpolated.getVelocity())); - errorA = FastMath.max(errorA, Vector3D.distance(pv.getAcceleration(), interpolated.getAcceleration())); - } - - Assert.assertEquals(0.0, errorP, 3.8e-9); - Assert.assertEquals(0.0, errorV, 8.0e-8); - Assert.assertEquals(0.0, errorA, 2.0e-8); - - } - - @Test - public void testPosition() { - // Initial BeiDou orbital elements (Ref: IGS) - final BeidouOrbitalElements boe = new BeidouEphemeris(7, 713, 284400.0, 6492.84515953064, 0.00728036486543715, - 2.1815194404696853E-9, 0.9065628903946735, 0.0, -0.6647664535282437, - -3.136916379444212E-9, -2.6584351442773304, 0.9614806010234702, - 7.306225597858429E-6, -6.314832717180252E-6, 406.96875, - 225.9375, -7.450580596923828E-9, -1.4062970876693726E-7); - // Date of the BeiDou orbital elements (GPStime - BDTtime = 14s) - final AbsoluteDate target = boe.getDate().shiftedBy(-14.0); - // Build the BeiDou propagator - final BeidouPropagator propagator = new BeidouPropagator.Builder(boe).build(); - // Compute the PV coordinates at the date of the BeiDou orbital elements - final PVCoordinates pv = propagator.getPVCoordinates(target, FramesFactory.getITRF(IERSConventions.IERS_2010, true)); - // Computed position - final Vector3D computedPos = pv.getPosition(); - // Expected position (reference from sp3 file WUM0MGXULA_20192470700_01D_05M_ORB.SP33) - final Vector3D expectedPos = new Vector3D(-10260690.520, 24061180.795, -32837341.074); - Assert.assertEquals(0., Vector3D.distance(expectedPos, computedPos), 3.1); - } - - @Test - public void testIssue544() { - // Builds the BeidouPropagator from the almanac - final BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); - // In order to test the issue, we volontary set a Double.NaN value in the date. - final AbsoluteDate date0 = new AbsoluteDate(2010, 5, 7, 7, 50, Double.NaN, TimeScalesFactory.getUTC()); - final PVCoordinates pv0 = propagator.propagateInEcef(date0); - // Verify that an infinite loop did not occur - Assert.assertEquals(Vector3D.NaN, pv0.getPosition()); - Assert.assertEquals(Vector3D.NaN, pv0.getVelocity()); - - } - - private class BeidouEphemeris implements BeidouOrbitalElements { - - private int prn; - private int week; - private double toe; - private double sma; - private double deltaN; - private double ecc; - private double inc; - private double iDot; - private double om0; - private double dom; - private double aop; - private double anom; - private double cuc; - private double cus; - private double crc; - private double crs; - private double cic; - private double cis; - - /** - * Build a new instance. - */ - BeidouEphemeris(int prn, int week, double toe, double sqa, double ecc, - double deltaN, double inc, double iDot, double om0, - double dom, double aop, double anom, double cuc, - double cus, double crc, double crs, double cic, double cis) { - this.prn = prn; - this.week = week; - this.toe = toe; - this.sma = sqa * sqa; - this.ecc = ecc; - this.deltaN = deltaN; - this.inc = inc; - this.iDot = iDot; - this.om0 = om0; - this.dom = dom; - this.aop = aop; - this.anom = anom; - this.cuc = cuc; - this.cus = cus; - this.crc = crc; - this.crs = crs; - this.cic = cic; - this.cis = cis; - } - - @Override - public int getPRN() { - return prn; - } - - @Override - public int getWeek() { - return week; - } - - @Override - public double getTime() { - return toe; - } - - @Override - public double getSma() { - return sma; - } - - @Override - public double getMeanMotion() { - final double absA = FastMath.abs(sma); - return FastMath.sqrt(BEIDOU_MU / absA) / absA + deltaN; - } - - @Override - public double getE() { - return ecc; - } - - @Override - public double getI0() { - return inc; - } - - @Override - public double getIDot() { - return iDot; - } - - @Override - public double getOmega0() { - return om0; - } - - @Override - public double getOmegaDot() { - return dom; - } - - @Override - public double getPa() { - return aop; - } - - @Override - public double getM0() { - return anom; - } - - @Override - public double getCuc() { - return cuc; - } - - @Override - public double getCus() { - return cus; - } - - @Override - public double getCrc() { - return crc; - } - - @Override - public double getCrs() { - return crs; - } - - @Override - public double getCic() { - return cic; - } - - @Override - public double getCis() { - return cis; - } - - @Override - public AbsoluteDate getDate() { - return new GNSSDate(week, toe * 1000., SatelliteSystem.BEIDOU).getDate(); - } - - } + private static BeidouAlmanac almanac; + + @BeforeClass + public static void setUpBeforeClass() { + Utils.setDataRoot("gnss"); + + // Almanac for satellite 18 for May 28th 2019 + almanac = new BeidouAlmanac(18, 694, 4096, 6493.3076, 0.00482368, 0.0, -0.01365602, 1.40069711, -2.11437379e-9, + 3.11461541, -2.53029382, 0.0001096725, 7.27596e-12, 0); + } + + @Test + public void testBeidouCycle() { + // Builds the BeiDou propagator from the almanac + final BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); + // Intermediate verification + Assert.assertEquals(18, almanac.getPRN()); + Assert.assertEquals(0, almanac.getHealth()); + Assert.assertEquals(0.0001096725, almanac.getAf0(), 1.0e-15); + Assert.assertEquals(7.27596e-12, almanac.getAf1(), 1.0e-15); + // Propagate at the BeiDou date and one BeiDou cycle later + final AbsoluteDate date0 = almanac.getDate(); + final Vector3D p0 = propagator.propagateInEcef(date0).getPosition(); + final double bdtCycleDuration = BeidouOrbitalElements.BEIDOU_WEEK_IN_SECONDS + * BeidouOrbitalElements.BEIDOU_WEEK_NB; + final AbsoluteDate date1 = date0.shiftedBy(bdtCycleDuration); + final Vector3D p1 = propagator.propagateInEcef(date1).getPosition(); + + // Checks + Assert.assertEquals(0., p0.distance(p1), 0.); + } + + @Test + public void testFrames() { + // Builds the BeiDou propagator from the almanac + final BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); + Assert.assertEquals("EME2000", propagator.getFrame().getName()); + Assert.assertEquals(3.986004418e+14, BeidouOrbitalElements.BEIDOU_MU, 1.0e6); + // Defines some date + final AbsoluteDate date = new AbsoluteDate(2016, 3, 3, 12, 0, 0., TimeScalesFactory.getUTC()); + // Get PVCoordinates at the date in the ECEF + final PVCoordinates pv0 = propagator.propagateInEcef(date); + // Get PVCoordinates at the date in the ECEF + final PVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); + + // Checks + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()), 3.3e-8); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()), 3.9e-12); + } + + @Test + public void testNoReset() { + try { + BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); + propagator.resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + try { + BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); + propagator.resetIntermediateState(propagator.getInitialState(), true); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + @Test + public void testDerivativesConsistency() { + + final Frame eme2000 = FramesFactory.getEME2000(); + double errorP = 0; + double errorV = 0; + double errorA = 0; + BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); + BeidouOrbitalElements elements = propagator.getBeidouOrbitalElements(); + AbsoluteDate t0 = new GNSSDate(elements.getWeek(), 0.001 * elements.getTime(), SatelliteSystem.BEIDOU) + .getDate(); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final AbsoluteDate central = t0.shiftedBy(dt); + final PVCoordinates pv = propagator.getPVCoordinates(central, eme2000); + final double h = 10.0; + List sample = new ArrayList(); + for (int i = -3; i <= 3; ++i) { + sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); + } + final PVCoordinates interpolated = TimeStampedPVCoordinates.interpolate(central, + CartesianDerivativesFilter.USE_P, sample); + errorP = FastMath.max(errorP, Vector3D.distance(pv.getPosition(), interpolated.getPosition())); + errorV = FastMath.max(errorV, Vector3D.distance(pv.getVelocity(), interpolated.getVelocity())); + errorA = FastMath.max(errorA, Vector3D.distance(pv.getAcceleration(), interpolated.getAcceleration())); + } + + Assert.assertEquals(0.0, errorP, 3.8e-9); + Assert.assertEquals(0.0, errorV, 8.0e-8); + Assert.assertEquals(0.0, errorA, 2.0e-8); + + } + + @Test + public void testPosition() { + // Initial BeiDou orbital elements (Ref: IGS) + final BeidouOrbitalElements boe = new BeidouEphemeris(7, 713, 284400.0, 6492.84515953064, 0.00728036486543715, + 2.1815194404696853E-9, 0.9065628903946735, 0.0, -0.6647664535282437, -3.136916379444212E-9, + -2.6584351442773304, 0.9614806010234702, 7.306225597858429E-6, -6.314832717180252E-6, 406.96875, + 225.9375, -7.450580596923828E-9, -1.4062970876693726E-7); + // Date of the BeiDou orbital elements (GPStime - BDTtime = 14s) + final AbsoluteDate target = boe.getDate().shiftedBy(-14.0); + // Build the BeiDou propagator + final BeidouPropagator propagator = new BeidouPropagator.Builder(boe).build(); + // Compute the PV coordinates at the date of the BeiDou orbital elements + final PVCoordinates pv = propagator.getPVCoordinates(target, + FramesFactory.getITRF(IERSConventions.IERS_2010, true)); + // Computed position + final Vector3D computedPos = pv.getPosition(); + // Expected position (reference from sp3 file + // WUM0MGXULA_20192470700_01D_05M_ORB.SP33) + final Vector3D expectedPos = new Vector3D(-10260690.520, 24061180.795, -32837341.074); + Assert.assertEquals(0., Vector3D.distance(expectedPos, computedPos), 3.1); + } + + @Test + public void testIssue544() { + // Builds the BeidouPropagator from the almanac + final BeidouPropagator propagator = new BeidouPropagator.Builder(almanac).build(); + // In order to test the issue, we volontary set a Double.NaN value in the date. + final AbsoluteDate date0 = new AbsoluteDate(2010, 5, 7, 7, 50, Double.NaN, TimeScalesFactory.getUTC()); + final PVCoordinates pv0 = propagator.propagateInEcef(date0); + // Verify that an infinite loop did not occur + Assert.assertEquals(Vector3D.NaN, pv0.getPosition()); + Assert.assertEquals(Vector3D.NaN, pv0.getVelocity()); + + } + + private class BeidouEphemeris implements BeidouOrbitalElements { + + private int prn; + private int week; + private double toe; + private double sma; + private double deltaN; + private double ecc; + private double inc; + private double iDot; + private double om0; + private double dom; + private double aop; + private double anom; + private double cuc; + private double cus; + private double crc; + private double crs; + private double cic; + private double cis; + + /** + * Build a new instance. + */ + BeidouEphemeris(int prn, int week, double toe, double sqa, double ecc, double deltaN, double inc, double iDot, + double om0, double dom, double aop, double anom, double cuc, double cus, double crc, double crs, + double cic, double cis) { + this.prn = prn; + this.week = week; + this.toe = toe; + this.sma = sqa * sqa; + this.ecc = ecc; + this.deltaN = deltaN; + this.inc = inc; + this.iDot = iDot; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.cuc = cuc; + this.cus = cus; + this.crc = crc; + this.crs = crs; + this.cic = cic; + this.cis = cis; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public double getTime() { + return toe; + } + + @Override + public double getSma() { + return sma; + } + + @Override + public double getMeanMotion() { + final double absA = FastMath.abs(sma); + return FastMath.sqrt(BEIDOU_MU / absA) / absA + deltaN; + } + + @Override + public double getE() { + return ecc; + } + + @Override + public double getI0() { + return inc; + } + + @Override + public double getIDot() { + return iDot; + } + + @Override + public double getOmega0() { + return om0; + } + + @Override + public double getOmegaDot() { + return dom; + } + + @Override + public double getPa() { + return aop; + } + + @Override + public double getM0() { + return anom; + } + + @Override + public double getCuc() { + return cuc; + } + + @Override + public double getCus() { + return cus; + } + + @Override + public double getCrc() { + return crc; + } + + @Override + public double getCrs() { + return crs; + } + + @Override + public double getCic() { + return cic; + } + + @Override + public double getCis() { + return cis; + } + + @Override + public AbsoluteDate getDate() { + return new GNSSDate(week, toe * 1000., SatelliteSystem.BEIDOU).getDate(); + } + + } } diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagatorTest.java new file mode 100644 index 000000000..e8bc3b4a8 --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagatorTest.java @@ -0,0 +1,436 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import java.util.ArrayList; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.junit.Assert; +import org.junit.BeforeClass; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.CartesianDerivativesFilter; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.IERSConventions; +import org.orekit.utils.TimeStampedFieldPVCoordinates; + +public class FieldBeidouPropagatorTest { + + @Test + public void testBeidouCycle() { + doTestBeidouCycle(Decimal64Field.getInstance()); + } + + @Test + public void testFrames() { + doTestFrames(Decimal64Field.getInstance()); + } + + @Test + public void testNoReset() { + doTestNoReset(Decimal64Field.getInstance()); + } + + @Test + public void testDerivativesConsistency() { + doTestDerivativesConsistency(Decimal64Field.getInstance()); + } + + @Test + public void testPosition() { + doTestPosition(Decimal64Field.getInstance()); + } + + @Test + public void testIssue544() { + doTestIssue544(Decimal64Field.getInstance()); + } + + @BeforeClass + public static void setUpBeforeClass() { + Utils.setDataRoot("gnss"); + } + + public > void doTestBeidouCycle(Field field) { + final T zero = field.getZero(); + final FieldBeidouAlmanac almanac = new FieldBeidouAlmanac(18, 694, zero.add(4096), zero.add(6493.3076), + zero.add(0.00482368), zero.add(0.0), zero.add(-0.01365602), zero.add(1.40069711), + zero.add(-2.11437379e-9), zero.add(3.11461541), zero.add(-2.53029382), zero.add(0.0001096725), + zero.add(7.27596e-12), 0); + // Builds the BeiDou propagator from the almanac + final FieldBeidouPropagator propagator = new FieldBeidouPropagator<>(field, almanac); + // Intermediate verification + Assert.assertEquals(18, almanac.getPRN()); + Assert.assertEquals(0, almanac.getHealth()); + Assert.assertEquals(0.0001096725, almanac.getAf0().getReal(), 1.0e-15); + Assert.assertEquals(7.27596e-12, almanac.getAf1().getReal(), 1.0e-15); + // Propagate at the BeiDou date and one BeiDou cycle later + final FieldAbsoluteDate date0 = almanac.getDate(); + final FieldVector3D p0 = propagator.propagateInEcef(date0).getPosition(); + final double bdtCycleDuration = BeidouOrbitalElements.BEIDOU_WEEK_IN_SECONDS + * BeidouOrbitalElements.BEIDOU_WEEK_NB; + final FieldAbsoluteDate date1 = date0.shiftedBy(bdtCycleDuration); + final FieldVector3D p1 = propagator.propagateInEcef(date1).getPosition(); + + // Checks + Assert.assertEquals(0., p0.distance(p1).getReal(), 0.); + } + + public > void doTestFrames(Field field) { + final T zero = field.getZero(); + final FieldBeidouAlmanac almanac = new FieldBeidouAlmanac(18, 694, zero.add(4096), zero.add(6493.3076), + zero.add(0.00482368), zero.add(0.0), zero.add(-0.01365602), zero.add(1.40069711), + zero.add(-2.11437379e-9), zero.add(3.11461541), zero.add(-2.53029382), zero.add(0.0001096725), + zero.add(7.27596e-12), 0); + // Builds the BeiDou propagator from the almanac + final FieldBeidouPropagator propagator = new FieldBeidouPropagator<>(field, almanac); + Assert.assertEquals("EME2000", propagator.getFrame().getName()); + Assert.assertEquals(3.986004418e+14, BeidouOrbitalElements.BEIDOU_MU, 1.0e6); + // Defines some date + final FieldAbsoluteDate date = new FieldAbsoluteDate(field, 2016, 3, 3, 12, 0, 0., + TimeScalesFactory.getUTC()); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); + + // Checks + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()).getReal(), 3.3e-8); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()).getReal(), 3.9e-12); + } + + public > void doTestNoReset(Field field) { + try { + final T zero = field.getZero(); + final FieldBeidouAlmanac almanac = new FieldBeidouAlmanac(18, 694, zero.add(4096), + zero.add(6493.3076), zero.add(0.00482368), zero.add(0.0), zero.add(-0.01365602), + zero.add(1.40069711), zero.add(-2.11437379e-9), zero.add(3.11461541), zero.add(-2.53029382), + zero.add(0.0001096725), zero.add(7.27596e-12), 0); + // Builds the BeiDou propagator from the almanac + final FieldBeidouPropagator propagator = new FieldBeidouPropagator<>(field, almanac); + propagator.resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + try { + final T zero = field.getZero(); + final FieldBeidouAlmanac almanac = new FieldBeidouAlmanac(18, 694, zero.add(4096), + zero.add(6493.3076), zero.add(0.00482368), zero.add(0.0), zero.add(-0.01365602), + zero.add(1.40069711), zero.add(-2.11437379e-9), zero.add(3.11461541), zero.add(-2.53029382), + zero.add(0.0001096725), zero.add(7.27596e-12), 0); + // Builds the BeiDou propagator from the almanac + final FieldBeidouPropagator propagator = new FieldBeidouPropagator<>(field, almanac); + propagator.resetIntermediateState(propagator.getInitialState(), true); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + public > void doTestDerivativesConsistency(Field field) { + final Frame eme2000 = FramesFactory.getEME2000(); + double errorP = 0; + double errorV = 0; + double errorA = 0; + final T zero = field.getZero(); + final FieldBeidouAlmanac almanac = new FieldBeidouAlmanac(18, 694, zero.add(4096), zero.add(6493.3076), + zero.add(0.00482368), zero.add(0.0), zero.add(-0.01365602), zero.add(1.40069711), + zero.add(-2.11437379e-9), zero.add(3.11461541), zero.add(-2.53029382), zero.add(0.0001096725), + zero.add(7.27596e-12), 0); + // Builds the BeiDou propagator from the almanac + final FieldBeidouPropagator propagator = new FieldBeidouPropagator<>(field, almanac); + FieldBeidouOrbitalElements elements = propagator.getFieldBeidouOrbitalElements(); + FieldAbsoluteDate t0 = new FieldGNSSDate<>(elements.getWeek(), elements.getTime().multiply(0.001), + SatelliteSystem.BEIDOU).getDate(); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final FieldAbsoluteDate central = t0.shiftedBy(dt); + final FieldPVCoordinates pv = propagator.getPVCoordinates(central, eme2000); + final double h = 10.0; + List> sample = new ArrayList>(); + for (int i = -3; i <= 3; ++i) { + sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); + } + final FieldPVCoordinates interpolated = TimeStampedFieldPVCoordinates.interpolate(central, + CartesianDerivativesFilter.USE_P, sample); + errorP = FastMath.max(errorP, + FieldVector3D.distance(pv.getPosition(), interpolated.getPosition()).getReal()); + errorV = FastMath.max(errorV, + FieldVector3D.distance(pv.getVelocity(), interpolated.getVelocity()).getReal()); + errorA = FastMath.max(errorA, + FieldVector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()).getReal()); + } + + Assert.assertEquals(0.0, errorP, 3.8e-9); + Assert.assertEquals(0.0, errorV, 8.0e-8); + Assert.assertEquals(0.0, errorA, 2.0e-8); + } + + public > void doTestPosition(Field field) { + final T zero = field.getZero(); + // Initial BeiDou orbital elements (Ref: IGS) + final FieldBeidouOrbitalElements boe = new FieldBeidouEphemeris<>(7, 713, zero.add(284400.0),zero.add(6492.84515953064), zero.add(0.00728036486543715), + zero.add(2.1815194404696853E-9), zero.add(0.9065628903946735), zero.add(0.0), zero.add(-0.6647664535282437), + zero.add(-3.136916379444212E-9), zero.add(-2.6584351442773304), zero.add(0.9614806010234702), + zero.add(7.306225597858429E-6), zero.add(-6.314832717180252E-6), zero.add(406.96875), + zero.add(225.9375), zero.add(-7.450580596923828E-9), zero.add(-1.4062970876693726E-7)); + // Date of the BeiDou orbital elements (GPStime - BDTtime = 14s) + final FieldAbsoluteDate target = boe.getDate().shiftedBy(-14.0); + // Build the BeiDou propagator + final FieldBeidouPropagator propagator = new FieldBeidouPropagator<>(field, boe); + // Compute the PV coordinates at the date of the BeiDou orbital elements + final FieldPVCoordinates pv = propagator.getPVCoordinates(target, FramesFactory.getITRF(IERSConventions.IERS_2010, true)); + // Computed position + final FieldVector3D computedPos = pv.getPosition(); + // Expected position (reference from sp3 file WUM0MGXULA_20192470700_01D_05M_ORB.SP33) + final FieldVector3D expectedPos = new FieldVector3D<>(zero.add(-10260690.520),zero.add(24061180.795) ,zero.add(-32837341.074)); + Assert.assertEquals(0., FieldVector3D.distance(expectedPos, computedPos).getReal(), 3.1); + } + + public > void doTestIssue544(Field field) { + final T zero = field.getZero(); + final FieldBeidouAlmanac almanac = new FieldBeidouAlmanac(18, 694, zero.add(4096), zero.add(6493.3076), + zero.add(0.00482368), zero.add(0.0), zero.add(-0.01365602), zero.add(1.40069711), + zero.add(-2.11437379e-9), zero.add(3.11461541), zero.add(-2.53029382), zero.add(0.0001096725), + zero.add(7.27596e-12), 0); + // Builds the BeiDou propagator from the almanac + final FieldBeidouPropagator propagator = new FieldBeidouPropagator<>(field, almanac); + // In order to test the issue, we volontary set a Double.NaN value in the date. + final FieldAbsoluteDate date0 = new FieldAbsoluteDate<>(field, 2010, 5, 7, 7, 50, Double.NaN, + TimeScalesFactory.getUTC()); + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date0); + // Verify that an infinite loop did not occur + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getPosition()); + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getVelocity()); + } + + + + + private class FieldBeidouEphemeris> implements FieldBeidouOrbitalElements { + + private int prn; + private int week; + private T toe; + private T sma; + private T deltaN; + private T ecc; + private T inc; + private T iDot; + private T om0; + private T dom; + private T aop; + private T anom; + private T cuc; + private T cus; + private T crc; + private T crs; + private T cic; + private T cis; + + /** + * Build a new instance. + */ + FieldBeidouEphemeris(int prn, int week, T toe, T sqa, T ecc, + T deltaN, T inc, T iDot, T om0, + T dom, T aop, T anom, T cuc, + T cus, T crc, T crs, T cic, T cis) { + this.prn = prn; + this.week = week; + this.toe = toe; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.deltaN = deltaN; + this.inc = inc; + this.iDot = iDot; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.cuc = cuc; + this.cus = cus; + this.crc = crc; + this.crs = crs; + this.cic = cic; + this.cis = cis; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toe; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(absA.getField().getZero().add(BEIDOU_MU).divide(absA)).divide(absA).add(deltaN); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return iDot; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return cuc; + } + + @Override + public T getCus() { + return cus; + } + + @Override + public T getCrc() { + return crc; + } + + @Override + public T getCrs() { + return crs; + } + + @Override + public T getCic() { + return cic; + } + + @Override + public T getCis() { + return cis; + } + + @Override + public FieldAbsoluteDate getDate() { + return new FieldGNSSDate<>(week, toe.multiply(1000), SatelliteSystem.BEIDOU).getDate(); + } + + @Override + public T getAf0() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAf1() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAf2() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public int getAODC() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public int getAODE() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public int getIOD() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getTGD1() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getTGD2() { + // TODO Auto-generated method stub + return null; + } + + } +} diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagatorTest.java new file mode 100644 index 000000000..97e1378af --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagatorTest.java @@ -0,0 +1,390 @@ +package org.orekit.propagation.analytical.gnss; + +import java.util.ArrayList; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.junit.Assert; +import org.junit.BeforeClass; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.gnss.GPSAlmanac; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.CartesianDerivativesFilter; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.IERSConventions; +import org.orekit.utils.TimeStampedFieldPVCoordinates; + +public class FieldGPSPropagatorTest { + + private static GPSAlmanac almanac; + + @BeforeClass + public static void setUpBeforeClass() { + Utils.setDataRoot("gnss"); + + almanac = new GPSAlmanac("SEM", 1, 63, 862, 319488, 5.15360253906250E+03, 5.10072708129883E-03, + 6.84547424316406E-03, -2.08778738975525E-01, -2.48837750405073E-09, 1.46086812019348E-01, + 4.55284833908081E-01, 1.33514404296875E-05, 0.00000000000000E+00, 0, 0, 11); + } + + @Test + public void testGPSCycle() { + doTestGPSCycle(Decimal64Field.getInstance()); + } + + @Test + public void testFrames() { + doTestFrames(Decimal64Field.getInstance()); + } + + @Test + public void testNoReset() { + doTestNoReset(Decimal64Field.getInstance()); + } + + @Test + public void testDerivativesConsistency() { + doTestDerivativesConsistency(Decimal64Field.getInstance()); + } + + @Test + public void testPosition() { + doTestPosition(Decimal64Field.getInstance()); + } + + @Test + public void testIssue544() { + doTestIssue544(Decimal64Field.getInstance()); + } + + public > void doTestGPSCycle(Field field) { + // Builds the GPSPropagator from the almanac + FieldGPSAlmanac almanac = new FieldGPSAlmanac(field, FieldGPSPropagatorTest.almanac); + final FieldGPSPropagator propagator = new FieldGPSPropagator<>(field, almanac); + // Propagate at the GPS date and one GPS cycle later + final FieldAbsoluteDate date0 = almanac.getDate(); + final FieldVector3D p0 = propagator.propagateInEcef(date0).getPosition(); + final double gpsCycleDuration = GPSOrbitalElements.GPS_WEEK_IN_SECONDS * GPSOrbitalElements.GPS_WEEK_NB; + final FieldAbsoluteDate date1 = date0.shiftedBy(gpsCycleDuration); + final FieldVector3D p1 = propagator.propagateInEcef(date1).getPosition(); + + // Checks + Assert.assertEquals(0., p0.distance(p1).getReal(), 0.); + } + + public > void doTestFrames(Field field) { + // Builds the GPSPropagator from the almanac + FieldGPSAlmanac almanac = new FieldGPSAlmanac(field, FieldGPSPropagatorTest.almanac); + final FieldGPSPropagator propagator = new FieldGPSPropagator<>(field, almanac); + Assert.assertEquals("EME2000", propagator.getFrame().getName()); + Assert.assertEquals(3.986005e14, GPSOrbitalElements.GPS_MU, 1.0e6); + // Defines some date + final FieldAbsoluteDate date = new FieldAbsoluteDate<>(field, 2016, 3, 3, 12, 0, 0., + TimeScalesFactory.getUTC()); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); + + // Checks + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()).getReal(), 9.4e-8); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()).getReal(), 2.8e-10); + } + + public > void doTestNoReset(Field field) { + try { + // Builds the GPSPropagator from the almanac + FieldGPSAlmanac almanac = new FieldGPSAlmanac(field, FieldGPSPropagatorTest.almanac); + final FieldGPSPropagator propagator = new FieldGPSPropagator<>(field, almanac); + propagator.resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + try { + // Builds the GPSPropagator from the almanac + FieldGPSAlmanac almanac = new FieldGPSAlmanac(field, FieldGPSPropagatorTest.almanac); + final FieldGPSPropagator propagator = new FieldGPSPropagator<>(field, almanac); + propagator.resetIntermediateState(propagator.getInitialState(), true); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + public > void doTestDerivativesConsistency(Field field) { + final Frame eme2000 = FramesFactory.getEME2000(); + double errorP = 0; + double errorV = 0; + double errorA = 0; + + // Builds the GPSPropagator from the almanac + FieldGPSAlmanac almanac = new FieldGPSAlmanac(field, FieldGPSPropagatorTest.almanac); + final FieldGPSPropagator propagator = new FieldGPSPropagator<>(field, almanac); + FieldGPSOrbitalElements elements = propagator.getFieldGPSOrbitalElements(); + FieldAbsoluteDate t0 = new FieldGNSSDate<>(elements.getWeek(), elements.getTime().multiply(0.001), + SatelliteSystem.GPS).getDate(); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final FieldAbsoluteDate central = t0.shiftedBy(dt); + final FieldPVCoordinates pv = propagator.getPVCoordinates(central, eme2000); + final double h = 10.0; + List> sample = new ArrayList>(); + for (int i = -3; i <= 3; ++i) { + sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); + } + final FieldPVCoordinates interpolated = TimeStampedFieldPVCoordinates.interpolate(central, + CartesianDerivativesFilter.USE_P, sample); + errorP = FastMath.max(errorP, + FieldVector3D.distance(pv.getPosition(), interpolated.getPosition()).getReal()); + errorV = FastMath.max(errorV, + FieldVector3D.distance(pv.getVelocity(), interpolated.getVelocity()).getReal()); + errorA = FastMath.max(errorA, + FieldVector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()).getReal()); + } + + Assert.assertEquals(0.0, errorP, 3.8e-9); + Assert.assertEquals(0.0, errorV, 3.8e-6); + Assert.assertEquals(0.0, errorA, 1.7e-8); + } + + public > void doTestPosition(Field field) { + final T zero = field.getZero(); + // Initial GPS orbital elements (Ref: IGS) + FieldGPSOrbitalElements goe = new FieldGPSEphemeris(7, 0, zero.add(288000), zero.add(5153.599830627441), + zero.add(0.012442796607501805), zero.add(4.419469802942352E-9), zero.add(0.9558937988021613), + zero.add(-2.4608167886110235E-10), zero.add(1.0479401362158658), zero.add(-7.967117576712062E-9), + zero.add(-2.4719019944000538), zero.add(-1.0899023379614294), zero.add(4.3995678424835205E-6), + zero.add(1.002475619316101E-5), zero.add(183.40625), zero.add(87.03125), zero.add(3.203749656677246E-7), + zero.add(4.0978193283081055E-8)); + + // Date of the GPS orbital elements + final FieldAbsoluteDate target = goe.getDate(); + // Build the GPS propagator + final FieldGPSPropagator propagator = new FieldGPSPropagator<>(field, goe); + // Compute the PV coordinates at the date of the GPS orbital elements + final FieldPVCoordinates pv = propagator.getPVCoordinates(target, + FramesFactory.getITRF(IERSConventions.IERS_2010, true)); + // Computed position + final FieldVector3D computedPos = pv.getPosition(); + // Expected position (reference from IGS file igu20484_00.sp3) + final FieldVector3D expectedPos = new FieldVector3D(zero.add(-4920705.292), zero.add(24248099.200), + zero.add(9236130.101)); + + Assert.assertEquals(0., FieldVector3D.distance(expectedPos, computedPos).getReal(), 3.2); + } + + public > void doTestIssue544(Field field) { + // Builds the GPSPropagator from the almanac + FieldGPSAlmanac almanac = new FieldGPSAlmanac(field, FieldGPSPropagatorTest.almanac); + final FieldGPSPropagator propagator = new FieldGPSPropagator<>(field, almanac); + // In order to test the issue, we volontary set a Double.NaN value in the date. + final FieldAbsoluteDate date0 = new FieldAbsoluteDate<>(field, 2010, 5, 7, 7, 50, Double.NaN, + TimeScalesFactory.getUTC()); + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date0); + // Verify that an infinite loop did not occur + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getPosition()); + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getVelocity()); + } + + private class FieldGPSEphemeris> implements FieldGPSOrbitalElements { + + private int prn; + private int week; + private T toe; + private T sma; + private T deltaN; + private T ecc; + private T inc; + private T iDot; + private T om0; + private T dom; + private T aop; + private T anom; + private T cuc; + private T cus; + private T crc; + private T crs; + private T cic; + private T cis; + + /** + * Build a new instance. + */ + FieldGPSEphemeris(int prn, int week, T toe, T sqa, T ecc, + T deltaN, T inc, T iDot, T om0, + T dom, T aop, T anom, T cuc, + T cus, T crc, T crs, T cic, T cis) { + + this.prn = prn; + this.week = week; + this.toe = toe; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.deltaN = deltaN; + this.inc = inc; + this.iDot = iDot; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.cuc = cuc; + this.cus = cus; + this.crc = crc; + this.crs = crs; + this.cic = cic; + this.cis = cis; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toe; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(absA.getField().getZero().add(GPS_MU).divide(absA)).divide(absA).add(deltaN); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return iDot; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return cuc; + } + + @Override + public T getCus() { + return cus; + } + + @Override + public T getCrc() { + return crc; + } + + @Override + public T getCrs() { + return crs; + } + + @Override + public T getCic() { + return cic; + } + + @Override + public T getCis() { + return cis; + } + + @Override + public FieldAbsoluteDate getDate() { + return new FieldGNSSDate<>(week, toe.multiply(1000), SatelliteSystem.GPS).getDate(); + } + + @Override + public T getAf0() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAf1() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAf2() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public int getIODC() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public int getIODE() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getTGD() { + // TODO Auto-generated method stub + return null; + } + + } + +} diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagatorTest.java new file mode 100644 index 000000000..a6e17decc --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagatorTest.java @@ -0,0 +1,442 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import java.util.ArrayList; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.junit.Assert; +import org.junit.BeforeClass; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.CartesianDerivativesFilter; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.IERSConventions; +import org.orekit.utils.TimeStampedFieldPVCoordinates; + +public class FieldGalileoPropagatorTest { + + @Test + public void testGalileoCycle() { + doTestGalileoCycle(Decimal64Field.getInstance()); + } + + @Test + public void testFrames() { + doTestFrames(Decimal64Field.getInstance()); + } + + @Test + public void testNoReset() { + doTestNoReset(Decimal64Field.getInstance()); + } + + @Test + public void testDerivativesConsistency() { + doTestDerivativesConsistency(Decimal64Field.getInstance()); + } + + @Test + public void testPosition() { + doTestPosition(Decimal64Field.getInstance()); + } + + @Test + public void testIssue544() { + doTestIssue544(Decimal64Field.getInstance()); + } + + @BeforeClass + public static void setUpBeforeClass() { + Utils.setDataRoot("gnss"); + } + + public > void doTestGalileoCycle(Field field) { + final T zero = field.getZero(); + // Reference for the almanac: 2019-05-28T09:40:01.0Z + final FieldGalileoAlmanac almanac = new FieldGalileoAlmanac<>(1, 1024, zero.add(293400.0), + zero.add(0.013671875), zero.add(0.000152587890625), zero.add(0.003356933593), 4, + zero.add(0.2739257812499857891), zero.add(-1.74622982740407E-9), zero.add(0.7363586425), + zero.add(0.27276611328124), zero.add(-0.0006141662597), zero.add(-7.275957614183E-12), 0, 0, 0); + // Intermediate verification + Assert.assertEquals(1, almanac.getPRN()); + Assert.assertEquals(1024, almanac.getWeek()); + Assert.assertEquals(4, almanac.getIOD()); + Assert.assertEquals(0, almanac.getHealthE1()); + Assert.assertEquals(0, almanac.getHealthE5a()); + Assert.assertEquals(0, almanac.getHealthE5b()); + Assert.assertEquals(-0.0006141662597, almanac.getAf0().getReal(), 1.0e-15); + Assert.assertEquals(-7.275957614183E-12, almanac.getAf1().getReal(), 1.0e-15); + + // Builds the GalileoPropagator from the almanac + FieldGalileoPropagator propagator = new FieldGalileoPropagator<>(field, almanac); + // Propagate at the Galileo date and one Galileo cycle later + final FieldAbsoluteDate date0 = almanac.getDate(); + final FieldVector3D p0 = propagator.propagateInEcef(date0).getPosition(); + final double galCycleDuration = FieldGalileoOrbitalElements.GALILEO_WEEK_IN_SECONDS + * FieldGalileoOrbitalElements.GALILEO_WEEK_NB; + final FieldAbsoluteDate date1 = date0.shiftedBy(galCycleDuration); + final FieldVector3D p1 = propagator.propagateInEcef(date1).getPosition(); + + // Checks + Assert.assertEquals(0., p0.distance(p1).getReal(), 0.); + } + + public > void doTestFrames(Field field) { + final T zero = field.getZero(); + FieldGalileoOrbitalElements goe = new FieldGalileoEphemeris(4, 1024, zero.add(293400.0), + zero.add(5440.602949142456), zero.add(3.7394414770330066E-9), zero.add(2.4088891223073006E-4), + zero.add(0.9531656087278083), zero.add(-2.36081262303612E-10), zero.add(-0.36639513583951266), + zero.add(-5.7695260382035525E-9), zero.add(-1.6870064194345724), zero.add(-0.38716557650888), + zero.add(-8.903443813323975E-7), zero.add(6.61797821521759E-6), zero.add(194.0625), zero.add(-18.78125), + zero.add(3.166496753692627E-8), zero.add(-1.862645149230957E-8)); + // Builds the GalileoPropagator from the ephemeris + FieldGalileoPropagator propagator = new FieldGalileoPropagator<>(field, goe); + Assert.assertEquals("EME2000", propagator.getFrame().getName()); + Assert.assertEquals(3.986004418e+14, FieldGalileoOrbitalElements.GALILEO_MU, 1.0e6); + // Defines some date + final FieldAbsoluteDate date = new FieldAbsoluteDate<>(field, 2016, 3, 3, 12, 0, 0., + TimeScalesFactory.getUTC()); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); + + // Checks + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()).getReal(), 2.4e-8); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()).getReal(), 2.8e-12); + } + + public > void doTestNoReset(Field field) { + try { + final T zero = field.getZero(); + FieldGalileoOrbitalElements goe = new FieldGalileoEphemeris(4, 1024, zero.add(293400.0), + zero.add(5440.602949142456), zero.add(3.7394414770330066E-9), zero.add(2.4088891223073006E-4), + zero.add(0.9531656087278083), zero.add(-2.36081262303612E-10), zero.add(-0.36639513583951266), + zero.add(-5.7695260382035525E-9), zero.add(-1.6870064194345724), zero.add(-0.38716557650888), + zero.add(-8.903443813323975E-7), zero.add(6.61797821521759E-6), zero.add(194.0625), zero.add(-18.78125), + zero.add(3.166496753692627E-8), zero.add(-1.862645149230957E-8)); + FieldGalileoPropagator propagator = new FieldGalileoPropagator<>(field, goe); + propagator.resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + try { + final T zero = field.getZero(); + FieldGalileoOrbitalElements goe = new FieldGalileoEphemeris(4, 1024, zero.add(293400.0), + zero.add(5440.602949142456), zero.add(3.7394414770330066E-9), zero.add(2.4088891223073006E-4), + zero.add(0.9531656087278083), zero.add(-2.36081262303612E-10), zero.add(-0.36639513583951266), + zero.add(-5.7695260382035525E-9), zero.add(-1.6870064194345724), zero.add(-0.38716557650888), + zero.add(-8.903443813323975E-7), zero.add(6.61797821521759E-6), zero.add(194.0625), zero.add(-18.78125), + zero.add(3.166496753692627E-8), zero.add(-1.862645149230957E-8)); + FieldGalileoPropagator propagator = new FieldGalileoPropagator<>(field, goe); + propagator.resetIntermediateState(propagator.getInitialState(), true); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + public > void doTestDerivativesConsistency(Field field) { + final Frame eme2000 = FramesFactory.getEME2000(); + double errorP = 0; + double errorV = 0; + double errorA = 0; + final T zero = field.getZero(); + FieldGalileoOrbitalElements goe = new FieldGalileoEphemeris(4, 1024, zero.add(293400.0), + zero.add(5440.602949142456), zero.add(3.7394414770330066E-9), zero.add(2.4088891223073006E-4), + zero.add(0.9531656087278083), zero.add(-2.36081262303612E-10), zero.add(-0.36639513583951266), + zero.add(-5.7695260382035525E-9), zero.add(-1.6870064194345724), zero.add(-0.38716557650888), + zero.add(-8.903443813323975E-7), zero.add(6.61797821521759E-6), zero.add(194.0625), zero.add(-18.78125), + zero.add(3.166496753692627E-8), zero.add(-1.862645149230957E-8)); + FieldGalileoPropagator propagator = new FieldGalileoPropagator<>(field, goe); + FieldGalileoOrbitalElements elements = propagator.getFieldGalileoOrbitalElements(); + FieldAbsoluteDate t0 = new FieldGNSSDate<>(elements.getWeek(), elements.getTime().multiply(0.001), + SatelliteSystem.GALILEO).getDate(); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final FieldAbsoluteDate central = t0.shiftedBy(dt); + final FieldPVCoordinates pv = propagator.getPVCoordinates(central, eme2000); + final double h = 10.0; + List> sample = new ArrayList>(); + for (int i = -3; i <= 3; ++i) { + sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); + } + final FieldPVCoordinates interpolated = TimeStampedFieldPVCoordinates.interpolate(central, + CartesianDerivativesFilter.USE_P, sample); + errorP = FastMath.max(errorP, + FieldVector3D.distance(pv.getPosition(), interpolated.getPosition()).getReal()); + errorV = FastMath.max(errorV, + FieldVector3D.distance(pv.getVelocity(), interpolated.getVelocity()).getReal()); + errorA = FastMath.max(errorA, + FieldVector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()).getReal()); + } + Assert.assertEquals(0.0, errorP, 1.5e-11); + Assert.assertEquals(0.0, errorV, 2.2e-7); + Assert.assertEquals(0.0, errorA, 4.9e-8); + + } + + public > void doTestPosition(Field field) { + final T zero = field.getZero(); + FieldGalileoOrbitalElements goe = new FieldGalileoEphemeris(4, 1024, zero.add(293400.0), + zero.add(5440.602949142456), zero.add(3.7394414770330066E-9), zero.add(2.4088891223073006E-4), + zero.add(0.9531656087278083), zero.add(-2.36081262303612E-10), zero.add(-0.36639513583951266), + zero.add(-5.7695260382035525E-9), zero.add(-1.6870064194345724), zero.add(-0.38716557650888), + zero.add(-8.903443813323975E-7), zero.add(6.61797821521759E-6), zero.add(194.0625), zero.add(-18.78125), + zero.add(3.166496753692627E-8), zero.add(-1.862645149230957E-8)); + // Date of the Galileo orbital elements, 10 April 2019 at 09:30:00 UTC + final FieldAbsoluteDate target = goe.getDate(); + // Build the Galileo propagator + FieldGalileoPropagator propagator = new FieldGalileoPropagator<>(field, goe); + // Compute the PV coordinates at the date of the Galileo orbital elements + final FieldPVCoordinates pv = propagator.getPVCoordinates(target, + FramesFactory.getITRF(IERSConventions.IERS_2010, true)); + // Computed position + final FieldVector3D computedPos = pv.getPosition(); + // Expected position (reference from IGS file + // WUM0MGXULA_20191010500_01D_15M_ORB.sp3) + final FieldVector3D expectedPos = new FieldVector3D<>(zero.add(10487480.721), zero.add(17867448.753), + zero.add(-21131462.002)); + Assert.assertEquals(0., FieldVector3D.distance(expectedPos, computedPos).getReal(), 2.1); + } + + public > void doTestIssue544(Field field) { + final T zero = field.getZero(); + FieldGalileoOrbitalElements goe = new FieldGalileoEphemeris(4, 1024, zero.add(293400.0), + zero.add(5440.602949142456), zero.add(3.7394414770330066E-9), zero.add(2.4088891223073006E-4), + zero.add(0.9531656087278083), zero.add(-2.36081262303612E-10), zero.add(-0.36639513583951266), + zero.add(-5.7695260382035525E-9), zero.add(-1.6870064194345724), zero.add(-0.38716557650888), + zero.add(-8.903443813323975E-7), zero.add(6.61797821521759E-6), zero.add(194.0625), zero.add(-18.78125), + zero.add(3.166496753692627E-8), zero.add(-1.862645149230957E-8)); + // Builds the GalileoPropagator from the almanac + FieldGalileoPropagator propagator = new FieldGalileoPropagator<>(field, goe); + // In order to test the issue, we volontary set a Double.NaN value in the date. + final FieldAbsoluteDate date0 = new FieldAbsoluteDate<>(field, 2010, 5, 7, 7, 50, Double.NaN, + TimeScalesFactory.getUTC()); + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date0); + // Verify that an infinite loop did not occur + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getPosition()); + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getVelocity()); + } + + private class FieldGalileoEphemeris> implements FieldGalileoOrbitalElements { + + private int satID; + private int week; + private T toe; + private T sma; + private T deltaN; + private T ecc; + private T inc; + private T iDot; + private T om0; + private T dom; + private T aop; + private T anom; + private T cuc; + private T cus; + private T crc; + private T crs; + private T cic; + private T cis; + + /** + * Build a new instance. + */ + public FieldGalileoEphemeris(int satID, int week, T toe, T sqa, T deltaN, T ecc, T inc, T iDot, T om0, T dom, + T aop, T anom, T cuc, T cus, T crc, T crs, T cic, T cis) { + this.satID = satID; + this.week = week; + this.toe = toe; + this.sma = sqa.multiply(sqa); + this.deltaN = deltaN; + this.ecc = ecc; + this.inc = inc; + this.iDot = iDot; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.cuc = cuc; + this.cus = cus; + this.crc = crc; + this.crs = crs; + this.cic = cic; + this.cis = cis; + } + + @Override + public int getPRN() { + return satID; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toe; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(absA.getField().getZero().add(GALILEO_MU).divide(absA)).divide(absA).add(deltaN); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return iDot; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return cuc; + } + + @Override + public T getCus() { + return cus; + } + + @Override + public T getCrc() { + return crc; + } + + @Override + public T getCrs() { + return crs; + } + + @Override + public T getCic() { + return cic; + } + + @Override + public T getCis() { + return cis; + } + + @Override + public FieldAbsoluteDate getDate() { + return new FieldGNSSDate<>(week, toe.multiply(1000), SatelliteSystem.GALILEO).getDate(); + } + + @Override + public T getAf0() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAf1() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAf2() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public int getIODNav() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getBGD() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getBGDE1E5a() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getBGDE5bE1() { + // TODO Auto-generated method stub + return null; + } + + } + +} diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagatorTest.java new file mode 100644 index 000000000..aa277087c --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagatorTest.java @@ -0,0 +1,181 @@ +package org.orekit.propagation.analytical.gnss; + +import java.util.ArrayList; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.junit.Assert; +import org.junit.BeforeClass; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.data.DataContext; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.frames.Frames; +import org.orekit.frames.FramesFactory; +import org.orekit.gnss.IRNSSAlmanac; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.GNSSDate; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.CartesianDerivativesFilter; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.PVCoordinates; +import org.orekit.utils.TimeStampedFieldPVCoordinates; +import org.orekit.utils.TimeStampedPVCoordinates; + +public class FieldIRNSSPropagatorTest { + + private static IRNSSOrbitalElements almanac; + private static Frames frames; + + @BeforeClass + public static void setUpBeforeClass() { + Utils.setDataRoot("gnss"); + + // Almanac for satellite 1 for April 1st 2014 (Source: Rinex 3.04 format - Table + // A19) + final int week = 1786; + final double toa = 172800.0; + almanac = new IRNSSAlmanac(1, week, toa, 6.493487739563E03, 2.257102518342E-03, 4.758105460020e-01, + -8.912102146884E-01, -4.414469594664e-09, -2.999907424014, -1.396094758025, -9.473115205765e-04, + 1.250555214938e-12, new GNSSDate(week, toa * 1000, SatelliteSystem.IRNSS).getDate()); + frames = DataContext.getDefault().getFrames(); + } + + @Test + public void testBeidouCycle() { + doTestIRNSSCycle(Decimal64Field.getInstance()); + } + + @Test + public void testFrames() { + doTestFrames(Decimal64Field.getInstance()); + } + + @Test + public void testNoReset() { + doTestNoReset(Decimal64Field.getInstance()); + } + + @Test + public void testDerivativesConsistency() { + doTestDerivativesConsistency(Decimal64Field.getInstance()); + } + + @Test + public void testIssue544() { + doTestIssue544(Decimal64Field.getInstance()); + } + + public > void doTestIRNSSCycle(Field field) { + // Builds the IRNSS propagator from the almanac + FieldIRNSSAlmanac almanac = new FieldIRNSSAlmanac(field, FieldIRNSSPropagatorTest.almanac); + final FieldIRNSSPropagator propagator = new FieldIRNSSPropagator(field, almanac, frames); + // Propagate at the IRNSS date and one IRNSS cycle later + final FieldAbsoluteDate date0 = almanac.getDate(); + final FieldVector3D p0 = propagator.propagateInEcef(date0).getPosition(); + final double bdtCycleDuration = IRNSSOrbitalElements.IRNSS_WEEK_IN_SECONDS * IRNSSOrbitalElements.IRNSS_WEEK_NB; + final FieldAbsoluteDate date1 = date0.shiftedBy(bdtCycleDuration); + final FieldVector3D p1 = propagator.propagateInEcef(date1).getPosition(); + + // Checks + Assert.assertEquals(0., p0.distance(p1).getReal(), 0.); + } + + public > void doTestFrames(Field field) { + // Builds the IRNSS propagator from the almanac + FieldIRNSSAlmanac almanac = new FieldIRNSSAlmanac(field, FieldIRNSSPropagatorTest.almanac); + final FieldIRNSSPropagator propagator = new FieldIRNSSPropagator(field, almanac, frames); + Assert.assertEquals("EME2000", propagator.getFrame().getName()); + Assert.assertEquals(3.986005e+14, IRNSSOrbitalElements.IRNSS_MU, 1.0e6); + // Defines some date + final FieldAbsoluteDate date = new FieldAbsoluteDate<>(field, 2016, 3, 3, 12, 0, 0., + TimeScalesFactory.getUTC()); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); + + // Checks + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()).getReal(), 3.3e-8); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()).getReal(), 3.9e-12); + } + + public > void doTestNoReset(Field field) { + try { + // Builds the IRNSS propagator from the almanac + FieldIRNSSAlmanac almanac = new FieldIRNSSAlmanac(field, FieldIRNSSPropagatorTest.almanac); + final FieldIRNSSPropagator propagator = new FieldIRNSSPropagator(field, almanac, frames); + propagator.resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + try { + // Builds the IRNSS propagator from the almanac + FieldIRNSSAlmanac almanac = new FieldIRNSSAlmanac(field, FieldIRNSSPropagatorTest.almanac); + final FieldIRNSSPropagator propagator = new FieldIRNSSPropagator(field, almanac, frames); + propagator.resetIntermediateState(propagator.getInitialState(), true); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + public > void doTestDerivativesConsistency(Field field) { + final Frame eme2000 = FramesFactory.getEME2000(); + double errorP = 0; + double errorV = 0; + double errorA = 0; + // Builds the IRNSS propagator from the almanac + FieldIRNSSAlmanac almanac = new FieldIRNSSAlmanac(field, FieldIRNSSPropagatorTest.almanac); + final FieldIRNSSPropagator propagator = new FieldIRNSSPropagator(field, almanac, frames); + FieldIRNSSOrbitalElements elements = propagator.getFieldIRNSSOrbitalElements(); + FieldAbsoluteDate t0 = new FieldGNSSDate<>(elements.getWeek(), elements.getTime().multiply(0.001), + SatelliteSystem.IRNSS).getDate(); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final FieldAbsoluteDate central = t0.shiftedBy(dt); + final FieldPVCoordinates pv = propagator.getPVCoordinates(central, eme2000); + final double h = 10.0; + List> sample = new ArrayList>(); + for (int i = -3; i <= 3; ++i) { + sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); + } + final FieldPVCoordinates interpolated = TimeStampedFieldPVCoordinates.interpolate(central, + CartesianDerivativesFilter.USE_P, sample); + errorP = FastMath.max(errorP, + FieldVector3D.distance(pv.getPosition(), interpolated.getPosition()).getReal()); + errorV = FastMath.max(errorV, + FieldVector3D.distance(pv.getVelocity(), interpolated.getVelocity()).getReal()); + errorA = FastMath.max(errorA, + FieldVector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()).getReal()); + } + + Assert.assertEquals(0.0, errorP, 3.8e-9); + Assert.assertEquals(0.0, errorV, 2.6e-7); + Assert.assertEquals(0.0, errorA, 6.5e-8); + + } + + public > void doTestIssue544(Field field) { + // Builds the IRNSS propagator from the almanac + FieldIRNSSAlmanac almanac = new FieldIRNSSAlmanac(field, FieldIRNSSPropagatorTest.almanac); + final FieldIRNSSPropagator propagator = new FieldIRNSSPropagator(field, almanac, frames); + // In order to test the issue, we volontary set a Double.NaN value in the date. + final FieldAbsoluteDate date0 = new FieldAbsoluteDate<>(field, 2010, 5, 7, 7, 50, Double.NaN, + TimeScalesFactory.getUTC()); + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date0); + // Verify that an infinite loop did not occur + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getPosition()); + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getVelocity()); + } +} diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagatorTest.java new file mode 100644 index 000000000..47efaddd1 --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagatorTest.java @@ -0,0 +1,389 @@ +package org.orekit.propagation.analytical.gnss; + +import java.util.ArrayList; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.junit.Assert; +import org.junit.BeforeClass; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.gnss.QZSSAlmanac; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.CartesianDerivativesFilter; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.IERSConventions; +import org.orekit.utils.TimeStampedFieldPVCoordinates; + +public class FieldQZSSPropagatorTest { + + private static QZSSAlmanac almanac; + + @BeforeClass + public static void setUpBeforeClass() { + Utils.setDataRoot("gnss"); + + // Almanac for satellite 193 for May 27th 2019 (q201914.alm) + almanac = new QZSSAlmanac(null, 193, 7, 348160.0, 6493.145996, 7.579761505E-02, 0.7201680272, -1.643310999, + -3.005839491E-09, -1.561775201, -4.050903957E-01, -2.965927124E-04, 7.275957614E-12, 0); + } + + @Test + public void testQZSSCycle() { + doTestQZSSCycle(Decimal64Field.getInstance()); + } + + @Test + public void testFrames() { + doTestFrames(Decimal64Field.getInstance()); + } + + @Test + public void testNoReset() { + doTestNoReset(Decimal64Field.getInstance()); + } + + @Test + public void testDerivativesConsistency() { + doTestDerivativesConsistency(Decimal64Field.getInstance()); + } + + @Test + public void testPosition() { + doTestPosition(Decimal64Field.getInstance()); + } + + @Test + public void testIssue544() { + doTestIssue544(Decimal64Field.getInstance()); + } + + public > void doTestQZSSCycle(Field field) { + // Builds the QZSSPropagator from the almanac + FieldQZSSAlmanac almanac = new FieldQZSSAlmanac(field, FieldQZSSPropagatorTest.almanac); + final FieldQZSSPropagator propagator = new FieldQZSSPropagator<>(field, almanac); + + // Propagate at the QZSS date and one QZSS cycle later + final FieldAbsoluteDate date0 = almanac.getDate(); + final FieldVector3D p0 = propagator.propagateInEcef(date0).getPosition(); + final double gpsCycleDuration = QZSSOrbitalElements.QZSS_WEEK_IN_SECONDS * QZSSOrbitalElements.QZSS_WEEK_NB; + final FieldAbsoluteDate date1 = date0.shiftedBy(gpsCycleDuration); + final FieldVector3D p1 = propagator.propagateInEcef(date1).getPosition(); + + // Checks + Assert.assertEquals(0., p0.distance(p1).getReal(), 0.); + } + + public > void doTestFrames(Field field) { + // Builds the QZSSPropagator from the almanac + FieldQZSSAlmanac almanac = new FieldQZSSAlmanac(field, FieldQZSSPropagatorTest.almanac); + final FieldQZSSPropagator propagator = new FieldQZSSPropagator<>(field, almanac); + Assert.assertEquals("EME2000", propagator.getFrame().getName()); + Assert.assertEquals(3.986005e+14, FieldQZSSOrbitalElements.QZSS_MU, 1.0e6); + // Defines some date + final FieldAbsoluteDate date = new FieldAbsoluteDate<>(field, 2016, 3, 3, 12, 0, 0., + TimeScalesFactory.getUTC()); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); + + // Checks + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()).getReal(), 3.3e-8); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()).getReal(), 3.9e-12); + } + + public > void doTestNoReset(Field field) { + try { + // Builds the QZSSPropagator from the almanac + FieldQZSSAlmanac almanac = new FieldQZSSAlmanac(field, FieldQZSSPropagatorTest.almanac); + final FieldQZSSPropagator propagator = new FieldQZSSPropagator<>(field, almanac); + propagator.resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + try { + // Builds the QZSSPropagator from the almanac + FieldQZSSAlmanac almanac = new FieldQZSSAlmanac(field, FieldQZSSPropagatorTest.almanac); + final FieldQZSSPropagator propagator = new FieldQZSSPropagator<>(field, almanac); + propagator.resetIntermediateState(propagator.getInitialState(), true); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + public > void doTestDerivativesConsistency(Field field) { + final Frame eme2000 = FramesFactory.getEME2000(); + double errorP = 0; + double errorV = 0; + double errorA = 0; + + // Builds the QZSSPropagator from the almanac + FieldQZSSAlmanac almanac = new FieldQZSSAlmanac(field, FieldQZSSPropagatorTest.almanac); + final FieldQZSSPropagator propagator = new FieldQZSSPropagator<>(field, almanac); + FieldQZSSOrbitalElements elements = propagator.getFieldQZSSOrbitalElements(); + FieldAbsoluteDate t0 = new FieldGNSSDate<>(elements.getWeek(), elements.getTime().multiply(0.001), + SatelliteSystem.QZSS).getDate(); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final FieldAbsoluteDate central = t0.shiftedBy(dt); + final FieldPVCoordinates pv = propagator.getPVCoordinates(central, eme2000); + final double h = 10.0; + List> sample = new ArrayList>(); + for (int i = -3; i <= 3; ++i) { + sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); + } + final FieldPVCoordinates interpolated = TimeStampedFieldPVCoordinates.interpolate(central, + CartesianDerivativesFilter.USE_P, sample); + errorP = FastMath.max(errorP, + FieldVector3D.distance(pv.getPosition(), interpolated.getPosition()).getReal()); + errorV = FastMath.max(errorV, + FieldVector3D.distance(pv.getVelocity(), interpolated.getVelocity()).getReal()); + errorA = FastMath.max(errorA, + FieldVector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()).getReal()); + } + + Assert.assertEquals(0.0, errorP, 3.8e-9); + Assert.assertEquals(0.0, errorV, 8.4e-8); + Assert.assertEquals(0.0, errorA, 2.1e-8); + } + + public > void doTestPosition(Field field) { + final T zero = field.getZero(); + // Initial QZSS orbital elements (Ref: IGS) + final FieldQZSSOrbitalElements qoe = new FieldQZSSEphemeris<>(195, 21, zero.add(226800.0), + zero.add(6493.226968765259), zero.add(0.07426900835707784), zero.add(4.796628370253418E-10), + zero.add(0.7116940567084221), zero.add(4.835915721014987E-10), zero.add(0.6210371871830609), + zero.add(-8.38963517626603E-10), zero.add(-1.5781555771543598), zero.add(1.077008903618136), + zero.add(-8.8568776845932E-6), zero.add(1.794286072254181E-5), zero.add(-344.03125), + zero.add(-305.6875), zero.add(1.2032687664031982E-6), zero.add(-2.6728957891464233E-6)); + // Date of the QZSS orbital elements + final FieldAbsoluteDate target = qoe.getDate(); + // Builds the QZSSPropagator from the almanac + FieldQZSSAlmanac almanac = new FieldQZSSAlmanac(field, FieldQZSSPropagatorTest.almanac); + final FieldQZSSPropagator propagator = new FieldQZSSPropagator<>(field, almanac); + // Compute the PV coordinates at the date of the QZSS orbital elements + final FieldPVCoordinates pv = propagator.getPVCoordinates(target, + FramesFactory.getITRF(IERSConventions.IERS_2010, true)); + // Computed position + final FieldVector3D computedPos = pv.getPosition(); + // Expected position (reference from QZSS sp3 file qzu20693_00.sp3) + final FieldVector3D expectedPos = new FieldVector3D(zero.add(-35047225.493), zero.add(18739632.916), + zero.add(-9522204.569)); + Assert.assertEquals(0., FieldVector3D.distance(expectedPos, computedPos).getReal(), 0.7); + } + + public > void doTestIssue544(Field field) { + // Builds the QZSSPropagator from the almanac + FieldQZSSAlmanac almanac = new FieldQZSSAlmanac(field, FieldQZSSPropagatorTest.almanac); + final FieldQZSSPropagator propagator = new FieldQZSSPropagator<>(field, almanac); + // In order to test the issue, we volontary set a Double.NaN value in the date. + final FieldAbsoluteDate date0 = new FieldAbsoluteDate<>(field, 2010, 5, 7, 7, 50, Double.NaN, + TimeScalesFactory.getUTC()); + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date0); + // Verify that an infinite loop did not occur + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getPosition()); + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getVelocity()); + } + + // ENF of Tests + + private class FieldQZSSEphemeris> implements FieldQZSSOrbitalElements { + + private int prn; + private int week; + private T toe; + private T sma; + private T deltaN; + private T ecc; + private T inc; + private T iDot; + private T om0; + private T dom; + private T aop; + private T anom; + private T cuc; + private T cus; + private T crc; + private T crs; + private T cic; + private T cis; + + /** + * Build a new instance. + */ + FieldQZSSEphemeris(int prn, int week, T toe, T sqa, T ecc, T deltaN, T inc, T iDot, T om0, T dom, T aop, T anom, + T cuc, T cus, T crc, T crs, T cic, T cis) { + this.prn = prn; + this.week = week; + this.toe = toe; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.deltaN = deltaN; + this.inc = inc; + this.iDot = iDot; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.cuc = cuc; + this.cus = cus; + this.crc = crc; + this.crs = crs; + this.cic = cic; + this.cis = cis; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toe; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(absA.getField().getZero().add(QZSS_MU).divide(absA)).divide(absA).add(deltaN); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return iDot; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return cuc; + } + + @Override + public T getCus() { + return cus; + } + + @Override + public T getCrc() { + return crc; + } + + @Override + public T getCrs() { + return crs; + } + + @Override + public T getCic() { + return cic; + } + + @Override + public T getCis() { + return cis; + } + + @Override + public FieldAbsoluteDate getDate() { + return new FieldGNSSDate<>(week, toe.multiply(1000), SatelliteSystem.QZSS).getDate(); + } + + @Override + public T getAf0() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAf1() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAf2() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + @Override + public int getIODC() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public int getIODE() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getTGD() { + // TODO Auto-generated method stub + return null; + } + + } + +} diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagatorTest.java new file mode 100644 index 000000000..c6e654c1e --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagatorTest.java @@ -0,0 +1,481 @@ +package org.orekit.propagation.analytical.gnss; + +import java.util.ArrayList; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.junit.Assert; +import org.junit.Before; +import org.junit.BeforeClass; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.data.DataContext; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.frames.Frames; +import org.orekit.frames.FramesFactory; +import org.orekit.gnss.SatelliteSystem; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.GNSSDate; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.CartesianDerivativesFilter; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.TimeStampedFieldPVCoordinates; + +public class FieldSBASPropagatorTest { + + /** Threshold for test validation. */ + private static double eps = 1.0e-15; + + /** SBAS orbital elements. */ + private SBASNavigationData soe; + private Frames frames; + + @Before + public > void setUp() { + // Reference data are taken from IGS file brdm0370.17p + soe = new SBASNavigationData(127, 1935, 1.23303e+05, 2.406022248000e+07, -2.712500000000e-01, + 3.250000000000e-04, 3.460922568000e+07, 3.063125000000e-00, -1.500000000000e-04, 1.964040000000e+04, + 1.012000000000e-00, -1.250000000000e-04); + frames = DataContext.getDefault().getFrames(); + } + + @BeforeClass + public static void setUpBeforeClass() { + Utils.setDataRoot("gnss"); + } + + @Test + public void testPropagationAtReferenceTime() { + doTestPropagation(Decimal64Field.getInstance()); + } + + @Test + public void testPropagation() { + doTestPropagationAtReferenceTime(Decimal64Field.getInstance()); + } + + @Test + public void testFrames() { + doTestFrames(Decimal64Field.getInstance()); + } + + @Test + public void testDerivativesConsistency() { + doTestDerivativesConsistency(Decimal64Field.getInstance()); + } + + @Test + public void testNoReset() { + doTestNoReset(Decimal64Field.getInstance()); + } + + public > void doTestPropagationAtReferenceTime(Field field) { + // SBAS propagator + final FieldSBASNavigationData soe = new FieldSBASNavigationData<>(field, this.soe); + final FieldSBASPropagator propagator = new FieldSBASPropagator(field, soe); + // Propagation + final FieldPVCoordinates pv = propagator.propagateInEcef(soe.getDate()); + // Position/Velocity/Acceleration + final FieldVector3D position = pv.getPosition(); + final FieldVector3D velocity = pv.getVelocity(); + final FieldVector3D acceleration = pv.getAcceleration(); + // Verify + System.out.println(soe.getXDotDot().getReal() + "\t" + acceleration.getX().getReal() + "\t" + eps); + System.out.println(soe.getYDotDot().getReal() + "\t" + acceleration.getY().getReal() + "\t" + eps); + System.out.println(soe.getZDotDot().getReal() + "\t" + acceleration.getZ().getReal() + "\t" + eps); + Assert.assertEquals(soe.getX().getReal(), position.getX().getReal(), eps); + Assert.assertEquals(soe.getY().getReal(), position.getY().getReal(), eps); + Assert.assertEquals(soe.getZ().getReal(), position.getZ().getReal(), eps); + Assert.assertEquals(soe.getXDot().getReal(), velocity.getX().getReal(), eps); + Assert.assertEquals(soe.getYDot().getReal(), velocity.getY().getReal(), eps); + Assert.assertEquals(soe.getZDot().getReal(), velocity.getZ().getReal(), eps); + Assert.assertEquals(soe.getXDotDot().getReal(), acceleration.getX().getReal(), eps); + Assert.assertEquals(soe.getYDotDot().getReal(), acceleration.getY().getReal(), eps); + Assert.assertEquals(soe.getZDotDot().getReal(), acceleration.getZ().getReal(), eps); + } + + public > void doTestPropagation(Field field) { + // SBAS propagator + final FieldSBASNavigationData soe = new FieldSBASNavigationData<>(field, this.soe); + final FieldSBASPropagator propagator = new FieldSBASPropagator(field, soe); + // Propagation + final FieldPVCoordinates pv = propagator.propagateInEcef(soe.getDate().shiftedBy(1.0)); + // Position/Velocity/Acceleration + final FieldVector3D position = pv.getPosition(); + final FieldVector3D velocity = pv.getVelocity(); + final FieldVector3D acceleration = pv.getAcceleration(); + // Verify + System.out.println(soe.getXDotDot().getReal() + "\t" + acceleration.getX().getReal() + "\t" + eps); + System.out.println(soe.getYDotDot().getReal() + "\t" + acceleration.getY().getReal() + "\t" + eps); + System.out.println(soe.getZDotDot().getReal() + "\t" + acceleration.getZ().getReal() + "\t" + eps); + Assert.assertEquals(24060222.2089125, position.getX().getReal(), eps); + Assert.assertEquals(34609228.7430500, position.getY().getReal(), eps); + Assert.assertEquals(19641.4119375, position.getZ().getReal(), eps); + Assert.assertEquals(-0.270925, velocity.getX().getReal(), eps); + Assert.assertEquals(3.062975, velocity.getY().getReal(), eps); + Assert.assertEquals(1.011875, velocity.getZ().getReal(), eps); + Assert.assertEquals(soe.getXDotDot().getReal(), acceleration.getX().getReal(), eps); + Assert.assertEquals(soe.getYDotDot().getReal(), acceleration.getY().getReal(), eps); + Assert.assertEquals(soe.getZDotDot().getReal(), acceleration.getZ().getReal(), eps); + } + + public > void doTestFrames(Field field) { + // SBAS propagator + final FieldSBASNavigationData soe = new FieldSBASNavigationData<>(field, this.soe); + final FieldSBASPropagator propagator = new FieldSBASPropagator(field, soe); + Assert.assertEquals("EME2000", propagator.getFrame().getName()); + Assert.assertEquals(3.986005e+14, propagator.getMU().getReal(), 1.0e6); + Assert.assertEquals(propagator.getECI().getName(), propagator.getFrame().getName()); + // Defines some date + final FieldAbsoluteDate date = new FieldAbsoluteDate(field, 2017, 2, 3, 12, 0, 0., + TimeScalesFactory.getUTC()); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); + + // Checks + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()).getReal(), 7.7e-9); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()).getReal(), 3.8e-12); + } + + public > void doTestDerivativesConsistency(Field field) { + final Frame eme2000 = FramesFactory.getEME2000(); + double errorP = 0; + double errorV = 0; + double errorA = 0; + + // SBAS propagator + final FieldSBASNavigationData soe = new FieldSBASNavigationData<>(field, this.soe); + final FieldSBASPropagator propagator = new FieldSBASPropagator(field, soe); + + FieldSBASOrbitalElements elements = propagator.getSBASOrbitalElements(); + FieldAbsoluteDate t0 = new FieldGNSSDate<>(elements.getWeek(), elements.getTime().multiply(0.001), + SatelliteSystem.SBAS).getDate(); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final FieldAbsoluteDate central = t0.shiftedBy(dt); + final FieldPVCoordinates pv = propagator.getPVCoordinates(central, eme2000); + final double h = 10.0; + List> sample = new ArrayList>(); + for (int i = -3; i <= 3; ++i) { + sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); + } + final FieldPVCoordinates interpolated = TimeStampedFieldPVCoordinates.interpolate(central, + CartesianDerivativesFilter.USE_P, sample); + errorP = FastMath.max(errorP, + FieldVector3D.distance(pv.getPosition(), interpolated.getPosition()).getReal()); + errorV = FastMath.max(errorV, + FieldVector3D.distance(pv.getVelocity(), interpolated.getVelocity()).getReal()); + errorA = FastMath.max(errorA, + FieldVector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()).getReal()); + } + Assert.assertEquals(0.0, errorP, 1.5e-11); + Assert.assertEquals(0.0, errorV, 6.7e-8); + Assert.assertEquals(0.0, errorA, 1.8e-8); + + } + + public > void doTestNoReset(Field field) { + try { + // SBAS propagator + final FieldSBASNavigationData soe = new FieldSBASNavigationData<>(field, this.soe); + final FieldSBASPropagator propagator = new FieldSBASPropagator(field, soe); + + propagator.resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + try { + // SBAS propagator + final FieldSBASNavigationData soe = new FieldSBASNavigationData<>(field, this.soe); + final FieldSBASPropagator propagator = new FieldSBASPropagator(field, soe); + + propagator.resetIntermediateState(propagator.getInitialState(), true); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + // END of Tests + + /** SBAS orbital elements as read from navigation data files. */ + private class SBASNavigationData implements SBASOrbitalElements { + + private int prn; + private int week; + private double time; + private double x; + private double xDot; + private double xDotDot; + private double y; + private double yDot; + private double yDotDot; + private double z; + private double zDot; + private double zDotDot; + + /** + * Constructor. + * + * @param prn prn code of the satellote + * @param week week number + * @param time reference time (s) + * @param x ECEF-X component of satellite coordinates (m) + * @param xDot ECEF-X component of satellite velocity (m/s) + * @param xDotDot ECEF-X component of satellite acceleration (m/s²) + * @param y ECEF-Y component of satellite coordinates (m) + * @param yDot ECEF-Y component of satellite velocity (m/s) + * @param yDotDot ECEF-Y component of satellite acceleration (m/s²) + * @param z ECEF-Z component of satellite coordinates (m) + * @param zDot ECEF-Z component of satellite velocity (m/s) + * @param zDotDot ECEF-Z component of satellite acceleration (m/s²) + */ + public SBASNavigationData(final int prn, final int week, final double time, final double x, final double xDot, + final double xDotDot, final double y, final double yDot, final double yDotDot, final double z, + final double zDot, final double zDotDot) { + this.prn = prn; + this.week = week; + this.time = time; + this.x = x; + this.xDot = xDot; + this.xDotDot = xDotDot; + this.y = y; + this.yDot = yDot; + this.yDotDot = yDotDot; + this.z = z; + this.zDot = zDot; + this.zDotDot = zDotDot; + } + + @Override + public AbsoluteDate getDate() { + return new GNSSDate(week, time * 1000.0, SatelliteSystem.SBAS).getDate(); + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public double getTime() { + return time; + } + + @Override + public double getX() { + return x; + } + + @Override + public double getXDot() { + return xDot; + } + + @Override + public double getXDotDot() { + return xDotDot; + } + + @Override + public double getY() { + return y; + } + + @Override + public double getYDot() { + return yDot; + } + + @Override + public double getYDotDot() { + return yDotDot; + } + + @Override + public double getZ() { + return z; + } + + @Override + public double getZDot() { + return zDot; + } + + @Override + public double getZDotDot() { + return zDotDot; + } + + } + + /** SBAS orbital elements as read from navigation data files. */ + private class FieldSBASNavigationData> implements FieldSBASOrbitalElements { + + private int prn; + private int week; + private T time; + private T x; + private T xDot; + private T xDotDot; + private T y; + private T yDot; + private T yDotDot; + private T z; + private T zDot; + private T zDotDot; + + /** + * Constructor + * + * @param prn + * @param week + * @param time + * @param x + * @param xDot + * @param xDotDot + * @param y + * @param yDot + * @param yDotDot + * @param z + * @param zDot + * @param zDotDot + */ + public FieldSBASNavigationData(final int prn, final int week, final T time, final T x, final T xDot, + final T xDotDot, final T y, final T yDot, final T yDotDot, final T z, final T zDot, final T zDotDot) { + this.prn = prn; + this.week = week; + this.time = time; + this.x = x; + this.xDot = xDot; + this.xDotDot = xDotDot; + this.y = y; + this.yDot = yDot; + this.yDotDot = yDotDot; + this.z = z; + this.zDot = zDot; + this.zDotDot = zDotDot; + } + + public FieldSBASNavigationData(Field field, SBASNavigationData sbasNavigationData) { + final T zero = field.getZero(); + this.prn = sbasNavigationData.getPRN(); + this.week = sbasNavigationData.getWeek(); + this.time = zero.add(sbasNavigationData.getTime()); + this.x = zero.add(sbasNavigationData.getX()); + this.xDot = zero.add(sbasNavigationData.getXDot()); + this.xDotDot = zero.add(sbasNavigationData.getXDotDot()); + this.y = zero.add(sbasNavigationData.getY()); + this.yDot = zero.add(sbasNavigationData.getYDot()); + this.yDotDot = zero.add(sbasNavigationData.getYDotDot()); + this.z = zero.add(sbasNavigationData.getZ()); + this.zDot = zero.add(sbasNavigationData.getZDot()); + this.zDotDot = zero.add(sbasNavigationData.getZDotDot()); + } + + @Override + public FieldAbsoluteDate getDate() { + return new FieldGNSSDate<>(week, time.multiply(1000.0), SatelliteSystem.SBAS).getDate(); + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return time; + } + + @Override + public T getX() { + return x; + } + + @Override + public T getXDot() { + return xDot; + } + + @Override + public T getXDotDot() { + return xDotDot; + } + + @Override + public T getY() { + return y; + } + + @Override + public T getYDot() { + return yDot; + } + + @Override + public T getYDotDot() { + return yDotDot; + } + + @Override + public T getZ() { + return z; + } + + @Override + public T getZDot() { + return zDot; + } + + @Override + public T getZDotDot() { + return zDotDot; + } + + @Override + public int getIODN() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public T getAGf0() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getAGf1() { + // TODO Auto-generated method stub + return null; + } + + @Override + public T getToc() { + // TODO Auto-generated method stub + return null; + } + + } +} diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/GPSPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/GPSPropagatorTest.java index 43052cf8a..7121695d6 100644 --- a/src/test/java/org/orekit/propagation/analytical/gnss/GPSPropagatorTest.java +++ b/src/test/java/org/orekit/propagation/analytical/gnss/GPSPropagatorTest.java @@ -48,432 +48,394 @@ import org.orekit.utils.IERSConventions; import org.orekit.utils.PVCoordinates; import org.orekit.utils.TimeStampedPVCoordinates; - public class GPSPropagatorTest { - private static List almanacs; - - @BeforeClass - public static void setUpBeforeClass() { - Utils.setDataRoot("gnss"); - GNSSDate.setRolloverReference(new DateComponents(DateComponents.GPS_EPOCH, 7 * 512)); - // Get the parser to read a SEM file - SEMParser reader = new SEMParser(null); - // Reads the SEM file - reader.loadData(); - // Gets the first SEM almanac - almanacs = reader.getAlmanacs(); - } - - @Test - public void testClockCorrections() { - final GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); - propagator.addAdditionalStateProvider(new ClockCorrectionsProvider(almanacs.get(0))); - // Propagate at the GPS date and one GPS cycle later - final AbsoluteDate date0 = almanacs.get(0).getDate(); - double dtRelMin = 0; - double dtRelMax = 0; - for (double dt = 0; dt < 0.5 * Constants.JULIAN_DAY; dt += 1.0) { - SpacecraftState state = propagator.propagate(date0.shiftedBy(dt)); - double[] corrections = state.getAdditionalState(ClockCorrectionsProvider.CLOCK_CORRECTIONS); - Assert.assertEquals(3, corrections.length); - Assert.assertEquals(1.33514404296875E-05, corrections[0], 1.0e-19); - dtRelMin = FastMath.min(dtRelMin, corrections[1]); - dtRelMax = FastMath.max(dtRelMax, corrections[1]); - Assert.assertEquals(0.0, corrections[2], Precision.SAFE_MIN); - } - Assert.assertEquals(-1.1679e-8, dtRelMin, 1.0e-12); - Assert.assertEquals(+1.1679e-8, dtRelMax, 1.0e-12); - } - - @Test - public void testGPSCycle() { - // Builds the GPSPropagator from the almanac - final GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); - // Propagate at the GPS date and one GPS cycle later - final AbsoluteDate date0 = almanacs.get(0).getDate(); - final Vector3D p0 = propagator.propagateInEcef(date0).getPosition(); - final double gpsCycleDuration = GPSOrbitalElements.GPS_WEEK_IN_SECONDS * GPSOrbitalElements.GPS_WEEK_NB; - final AbsoluteDate date1 = date0.shiftedBy(gpsCycleDuration); - final Vector3D p1 = propagator.propagateInEcef(date1).getPosition(); - - // Checks - Assert.assertEquals(0., p0.distance(p1), 0.); - } - - @Test - public void testFrames() { - // Builds the GPSPropagator from the almanac - final GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); - Assert.assertEquals("EME2000", propagator.getFrame().getName()); - Assert.assertEquals(3.986005e14, GPSOrbitalElements.GPS_MU, 1.0e6); - // Defines some date - final AbsoluteDate date = new AbsoluteDate(2016, 3, 3, 12, 0, 0., TimeScalesFactory.getUTC()); - // Get PVCoordinates at the date in the ECEF - final PVCoordinates pv0 = propagator.propagateInEcef(date); - // Get PVCoordinates at the date in the ECEF - final PVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); - - // Checks - Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()), 3.3e-8); - Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()), 3.9e-12); - } - - @Test - public void testNoReset() { - try { - GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); - propagator.resetInitialState(propagator.getInitialState()); - Assert.fail("an exception should have been thrown"); - } catch (OrekitException oe) { - Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); - } - try { - GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); - propagator.resetIntermediateState(propagator.getInitialState(), true); - Assert.fail("an exception should have been thrown"); - } catch (OrekitException oe) { - Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); - } - } - - @Test - public void testTLE() { - - List gpsPropagators = new ArrayList(); - for (final GPSAlmanac almanac : almanacs) { - gpsPropagators.add(new GPSPropagator.Builder(almanac).build()); - } - - // the following map corresponds to the GPS constellation status in early 2016 - final Map prnToTLE = new HashMap(); - prnToTLE.put( 1, - new TLE("1 37753U 11036A 16059.51505483 -.00000016 00000-0 00000+0 0 9995", - "2 37753 55.2230 119.7200 0049958 23.9363 306.3749 2.00566105 33828")); - prnToTLE.put( 2, - new TLE("1 28474U 04045A 16059.68518942 -.00000018 +00000-0 +00000-0 0 9992", - "2 28474 054.0048 117.2153 0156693 236.8152 092.4773 02.00556694082981")); - prnToTLE.put( 3, - new TLE("1 40294U 14068A 16059.64183862 +.00000012 +00000-0 +00000-0 0 9990", - "2 40294 054.9632 179.3690 0003634 223.4011 136.5596 02.00553679009768")); - prnToTLE.put( 4, - new TLE("1 34661U 09014A 16059.51658765 -.00000072 00000-0 00000+0 0 9997", - "2 34661 56.3471 0.6400 0080177 48.0430 129.2467 2.00572451 50844")); - prnToTLE.put( 5, - new TLE("1 35752U 09043A 16059.39819238 .00000011 00000-0 00000+0 0 9993", - "2 35752 54.2243 178.7652 0044753 24.8598 29.4422 2.00555538 47893")); - prnToTLE.put( 6, - new TLE("1 39741U 14026A 16059.18044747 -.00000016 00000-0 00000+0 0 9990", - "2 39741 55.2140 119.2493 0005660 259.2190 100.7882 2.00566982 13061")); - prnToTLE.put( 7, - new TLE("1 32711U 08012A 16059.36304856 -.00000033 +00000-0 +00000-0 0 9998", - "2 32711 055.4269 300.7399 0091867 207.6311 151.9340 02.00564257058321")); - prnToTLE.put( 8, - new TLE("1 40730U 15033A 16059.44106931 -.00000026 +00000-0 +00000-0 0 9994", - "2 40730 055.1388 059.0069 0020452 282.1769 077.6168 02.00566073004562")); - prnToTLE.put( 9, - new TLE("1 40105U 14045A 16059.27451329 .00000045 00000-0 00000+0 0 9996", - "2 40105 54.7529 238.9873 0004485 121.4766 238.5557 2.00568637 11512")); - prnToTLE.put(10, - new TLE("1 41019U 15062A 16059.49433942 .00000013 00000-0 00000+0 0 9991", - "2 41019 54.9785 179.1399 0012328 204.9013 155.0292 2.00561967 2382")); - prnToTLE.put(11, - new TLE("1 25933U 99055A 16059.51073770 -.00000024 00000-0 00000+0 0 9997", - "2 25933 51.3239 98.4815 0159812 86.1576 266.7718 2.00565163120122")); - prnToTLE.put(12, - new TLE("1 29601U 06052A 16059.62966898 -.00000070 +00000-0 +00000-0 0 9994", - "2 29601 056.7445 002.2755 0057667 037.0706 323.3313 02.00552237067968")); - prnToTLE.put(13, - new TLE("1 24876U 97035A 16059.41696335 .00000046 00000-0 00000+0 0 9998", - "2 24876 55.6966 245.8203 0044339 114.8899 245.5712 2.00562657136305")); - prnToTLE.put(14, - new TLE("1 26605U 00071A 16059.56211888 .00000047 00000-0 00000+0 0 9997", - "2 26605 55.2663 243.7251 0085518 248.7231 95.5323 2.00557009112094")); - prnToTLE.put(15, - new TLE("1 32260U 07047A 16059.45678257 +.00000044 +00000-0 +00000-0 0 9994", - "2 32260 053.3641 236.0940 0079746 026.4105 333.9774 02.00547771061402")); - prnToTLE.put(16, - new TLE("1 27663U 03005A 16059.14440417 -.00000071 00000-0 00000+0 0 9996", - "2 27663 56.7743 3.3691 0085346 17.6322 214.4333 2.00559487 95843")); - prnToTLE.put(17, - new TLE("1 28874U 05038A 16059.21070933 -.00000024 00000-0 00000+0 0 9997", - "2 28874 55.8916 61.9596 0112077 248.9647 205.7384 2.00567116 76379")); - prnToTLE.put(18, - new TLE("1 26690U 01004A 16059.51332910 .00000008 00000-0 00000+0 0 9990", - "2 26690 52.9999 177.6630 0169501 250.8579 153.6293 2.00563995110499")); - prnToTLE.put(19, - new TLE("1 28190U 04009A 16058.12363503 -.00000030 00000-0 00000+0 0 9999", - "2 28190 55.7230 64.8110 0105865 40.0254 321.4519 2.00572212 87500")); - prnToTLE.put(20, - new TLE("1 26360U 00025A 16059.44770263 .00000005 00000-0 00000+0 0 9992", - "2 26360 53.0712 174.6895 0046205 76.0615 334.4302 2.00559931115818")); - prnToTLE.put(21, - new TLE("1 27704U 03010A 16059.50719524 -.00000019 00000-0 00000+0 0 9998", - "2 27704 53.6134 117.9454 0234081 255.6874 199.2128 2.00564673 94659")); - prnToTLE.put(22, - new TLE("1 28129U 03058A 16059.06680941 .00000008 00000-0 00000+0 0 9990", - "2 28129 52.8771 177.7253 0079127 245.1376 114.0279 2.00398763 89357")); - prnToTLE.put(23, - new TLE("1 28361U 04023A 16059.54310021 .00000046 00000-0 00000+0 0 9995", - "2 28361 54.2347 239.3240 0106509 211.5355 11.7648 2.00557932 85613")); - prnToTLE.put(24, - new TLE("1 38833U 12053A 16059.04618549 -.00000032 00000-0 00000+0 0 9999", - "2 38833 54.4591 298.1383 0042253 18.7074 341.5041 2.00568407 24895")); - prnToTLE.put(25, - new TLE("1 36585U 10022A 16059.29300735 -.00000074 00000-0 00000+0 0 9993", - "2 36585 56.0738 359.4320 0050768 38.3425 49.1794 2.00578535 42134")); - prnToTLE.put(26, - new TLE("1 40534U 15013A 16059.28299301 -.00000076 00000-0 00000+0 0 9994", - "2 40534 55.0430 359.0082 0009349 342.4081 17.5685 2.00558853 6801")); - prnToTLE.put(27, - new TLE("1 39166U 13023A 16059.40401153 -.00000025 00000-0 00000+0 0 9990", - "2 39166 55.6020 59.1224 0032420 7.7969 352.2759 2.00568484 20414")); - prnToTLE.put(28, - new TLE("1 26407U 00040A 16059.80383354 -.00000069 +00000-0 +00000-0 0 9994", - "2 26407 056.6988 003.6328 0201499 267.0948 317.6209 02.00569902114508")); - prnToTLE.put(29, - new TLE("1 32384U 07062A 16059.44770263 -.00000021 00000-0 00000+0 0 9992", - "2 32384 55.9456 62.5022 0011922 319.9531 172.6730 2.00571577 60128")); - prnToTLE.put(30, - new TLE("1 39533U 14008A 16059.40267873 -.00000038 +00000-0 +00000-0 0 9996", - "2 39533 054.6126 303.3404 0017140 179.4267 180.6311 02.00568364014251")); - prnToTLE.put(31, - new TLE("1 29486U 06042A 16059.50651990 -.00000032 00000-0 00000+0 0 9992", - "2 29486 55.7041 301.2472 0084115 334.2804 254.9897 2.00560606 69098")); - prnToTLE.put(32, - new TLE("1 41328U 16007A 16059.56873502 .00000049 00000-0 00000+0 0 9991", - "2 41328 55.0137 239.0304 0002157 298.9074 61.0768 1.99172830 453")); - - for (final GPSPropagator gpsPropagator : gpsPropagators) { - final int prn = gpsPropagator.getGPSOrbitalElements().getPRN(); - TLE tle = prnToTLE.get(prn); - TLEPropagator tlePropagator = TLEPropagator.selectExtrapolator(tle); - for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { - final AbsoluteDate date = tlePropagator.getInitialState().getDate().shiftedBy(dt); - final PVCoordinates gpsPV = gpsPropagator.getPVCoordinates(date, gpsPropagator.getECI()); - final PVCoordinates tlePV = tlePropagator.getPVCoordinates(date, gpsPropagator.getECI()); - Assert.assertEquals(0.0, - Vector3D.distance(gpsPV.getPosition(), tlePV.getPosition()), - 8400.0); - } - } - } - - @Test - public void testDerivativesConsistency() { - - final Frame eme2000 = FramesFactory.getEME2000(); - double errorP = 0; - double errorV = 0; - double errorA = 0; - for (final GPSAlmanac almanac : almanacs) { - GPSPropagator propagator = new GPSPropagator.Builder(almanac).build(); - GPSOrbitalElements elements = propagator.getGPSOrbitalElements(); - AbsoluteDate t0 = new GNSSDate(elements.getWeek(), 0.001 * elements.getTime(), SatelliteSystem.GPS).getDate(); - for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { - final AbsoluteDate central = t0.shiftedBy(dt); - final PVCoordinates pv = propagator.getPVCoordinates(central, eme2000); - final double h = 10.0; - List sample = new ArrayList(); - for (int i = -3; i <= 3; ++i) { - sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); - } - final PVCoordinates interpolated = - TimeStampedPVCoordinates.interpolate(central, - CartesianDerivativesFilter.USE_P, - sample); - errorP = FastMath.max(errorP, Vector3D.distance(pv.getPosition(), interpolated.getPosition())); - errorV = FastMath.max(errorV, Vector3D.distance(pv.getVelocity(), interpolated.getVelocity())); - errorA = FastMath.max(errorA, Vector3D.distance(pv.getAcceleration(), interpolated.getAcceleration())); - } - } - Assert.assertEquals(0.0, errorP, 3.8e-9); - Assert.assertEquals(0.0, errorV, 3.5e-8); - Assert.assertEquals(0.0, errorA, 1.1e-8); - - } - - @Test - public void testPosition() { - // Initial GPS orbital elements (Ref: IGS) - GPSOrbitalElements goe = new GPSEphemeris(7, 0, 288000, 5153.599830627441, - 0.012442796607501805, 4.419469802942352E-9,0.9558937988021613, - -2.4608167886110235E-10, 1.0479401362158658, -7.967117576712062E-9, - -2.4719019944000538, -1.0899023379614294, 4.3995678424835205E-6, - 1.002475619316101E-5, 183.40625, 87.03125, 3.203749656677246E-7, - 4.0978193283081055E-8); - - // Date of the GPS orbital elements - final AbsoluteDate target = goe.getDate(); - // Build the GPS propagator - final GPSPropagator propagator = new GPSPropagator.Builder(goe).build(); - // Compute the PV coordinates at the date of the GPS orbital elements - final PVCoordinates pv = propagator.getPVCoordinates(target, FramesFactory.getITRF(IERSConventions.IERS_2010, true)); - // Computed position - final Vector3D computedPos = pv.getPosition(); - // Expected position (reference from IGS file igu20484_00.sp3) - final Vector3D expectedPos = new Vector3D(-4920705.292, 24248099.200, 9236130.101); - - Assert.assertEquals(0., Vector3D.distance(expectedPos, computedPos), 3.2); - } - - @Test - public void testIssue544() { - // Builds the GPSPropagator from the almanac - final GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); - // In order to test the issue, we volontary set a Double.NaN value in the date. - final AbsoluteDate date0 = new AbsoluteDate(2010, 5, 7, 7, 50, Double.NaN, TimeScalesFactory.getUTC()); - final PVCoordinates pv0 = propagator.propagateInEcef(date0); - // Verify that an infinite loop did not occur - Assert.assertEquals(Vector3D.NaN, pv0.getPosition()); - Assert.assertEquals(Vector3D.NaN, pv0.getVelocity()); - - } - - private class GPSEphemeris implements GPSOrbitalElements { - - private int prn; - private int week; - private double toe; - private double sma; - private double deltaN; - private double ecc; - private double inc; - private double iDot; - private double om0; - private double dom; - private double aop; - private double anom; - private double cuc; - private double cus; - private double crc; - private double crs; - private double cic; - private double cis; - - /** - * Build a new instance. - */ - GPSEphemeris(int prn, int week, double toe, double sqa, double ecc, - double deltaN, double inc, double iDot, double om0, - double dom, double aop, double anom, double cuc, - double cus, double crc, double crs, double cic, double cis) { - this.prn = prn; - this.week = week; - this.toe = toe; - this.sma = sqa * sqa; - this.ecc = ecc; - this.deltaN = deltaN; - this.inc = inc; - this.iDot = iDot; - this.om0 = om0; - this.dom = dom; - this.aop = aop; - this.anom = anom; - this.cuc = cuc; - this.cus = cus; - this.crc = crc; - this.crs = crs; - this.cic = cic; - this.cis = cis; - } - - @Override - public int getPRN() { - return prn; - } - - @Override - public int getWeek() { - return week; - } - - @Override - public double getTime() { - return toe; - } - - @Override - public double getSma() { - return sma; - } - - @Override - public double getMeanMotion() { - final double absA = FastMath.abs(sma); - return FastMath.sqrt(GPS_MU / absA) / absA + deltaN; - } - - @Override - public double getE() { - return ecc; - } - - @Override - public double getI0() { - return inc; - } - - @Override - public double getIDot() { - return iDot; - } - - @Override - public double getOmega0() { - return om0; - } - - @Override - public double getOmegaDot() { - return dom; - } - - @Override - public double getPa() { - return aop; - } - - @Override - public double getM0() { - return anom; - } - - @Override - public double getCuc() { - return cuc; - } - - @Override - public double getCus() { - return cus; - } - - @Override - public double getCrc() { - return crc; - } - - @Override - public double getCrs() { - return crs; - } - - @Override - public double getCic() { - return cic; - } - - @Override - public double getCis() { - return cis; - } - - @Override - public AbsoluteDate getDate() { - return new GNSSDate(week, toe * 1000., SatelliteSystem.GPS).getDate(); - } - - } + private static List almanacs; + + @BeforeClass + public static void setUpBeforeClass() { + Utils.setDataRoot("gnss"); + GNSSDate.setRolloverReference(new DateComponents(DateComponents.GPS_EPOCH, 7 * 512)); + // Get the parser to read a SEM file + SEMParser reader = new SEMParser(null); + // Reads the SEM file + reader.loadData(); + // Gets the first SEM almanac + almanacs = reader.getAlmanacs(); + } + + @Test + public void testClockCorrections() { + final GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); + propagator.addAdditionalStateProvider(new ClockCorrectionsProvider(almanacs.get(0))); + // Propagate at the GPS date and one GPS cycle later + final AbsoluteDate date0 = almanacs.get(0).getDate(); + double dtRelMin = 0; + double dtRelMax = 0; + for (double dt = 0; dt < 0.5 * Constants.JULIAN_DAY; dt += 1.0) { + SpacecraftState state = propagator.propagate(date0.shiftedBy(dt)); + double[] corrections = state.getAdditionalState(ClockCorrectionsProvider.CLOCK_CORRECTIONS); + Assert.assertEquals(3, corrections.length); + Assert.assertEquals(1.33514404296875E-05, corrections[0], 1.0e-19); + dtRelMin = FastMath.min(dtRelMin, corrections[1]); + dtRelMax = FastMath.max(dtRelMax, corrections[1]); + Assert.assertEquals(0.0, corrections[2], Precision.SAFE_MIN); + } + Assert.assertEquals(-1.1679e-8, dtRelMin, 1.0e-12); + Assert.assertEquals(+1.1679e-8, dtRelMax, 1.0e-12); + } + + @Test + public void testGPSCycle() { + // Builds the GPSPropagator from the almanac + final GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); + // Propagate at the GPS date and one GPS cycle later + final AbsoluteDate date0 = almanacs.get(0).getDate(); + final Vector3D p0 = propagator.propagateInEcef(date0).getPosition(); + final double gpsCycleDuration = GPSOrbitalElements.GPS_WEEK_IN_SECONDS * GPSOrbitalElements.GPS_WEEK_NB; + final AbsoluteDate date1 = date0.shiftedBy(gpsCycleDuration); + final Vector3D p1 = propagator.propagateInEcef(date1).getPosition(); + + // Checks + Assert.assertEquals(0., p0.distance(p1), 0.); + } + + @Test + public void testFrames() { + // Builds the GPSPropagator from the almanac + final GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); + Assert.assertEquals("EME2000", propagator.getFrame().getName()); + Assert.assertEquals(3.986005e14, GPSOrbitalElements.GPS_MU, 1.0e6); + // Defines some date + final AbsoluteDate date = new AbsoluteDate(2016, 3, 3, 12, 0, 0., TimeScalesFactory.getUTC()); + // Get PVCoordinates at the date in the ECEF + final PVCoordinates pv0 = propagator.propagateInEcef(date); + // Get PVCoordinates at the date in the ECEF + final PVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); + + // Checks + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()), 3.3e-8); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()), 3.9e-12); + } + + @Test + public void testNoReset() { + try { + GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); + propagator.resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + try { + GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); + propagator.resetIntermediateState(propagator.getInitialState(), true); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + @Test + public void testTLE() { + + List gpsPropagators = new ArrayList(); + for (final GPSAlmanac almanac : almanacs) { + gpsPropagators.add(new GPSPropagator.Builder(almanac).build()); + } + + // the following map corresponds to the GPS constellation status in early 2016 + final Map prnToTLE = new HashMap(); + prnToTLE.put(1, new TLE("1 37753U 11036A 16059.51505483 -.00000016 00000-0 00000+0 0 9995", + "2 37753 55.2230 119.7200 0049958 23.9363 306.3749 2.00566105 33828")); + prnToTLE.put(2, new TLE("1 28474U 04045A 16059.68518942 -.00000018 +00000-0 +00000-0 0 9992", + "2 28474 054.0048 117.2153 0156693 236.8152 092.4773 02.00556694082981")); + prnToTLE.put(3, new TLE("1 40294U 14068A 16059.64183862 +.00000012 +00000-0 +00000-0 0 9990", + "2 40294 054.9632 179.3690 0003634 223.4011 136.5596 02.00553679009768")); + prnToTLE.put(4, new TLE("1 34661U 09014A 16059.51658765 -.00000072 00000-0 00000+0 0 9997", + "2 34661 56.3471 0.6400 0080177 48.0430 129.2467 2.00572451 50844")); + prnToTLE.put(5, new TLE("1 35752U 09043A 16059.39819238 .00000011 00000-0 00000+0 0 9993", + "2 35752 54.2243 178.7652 0044753 24.8598 29.4422 2.00555538 47893")); + prnToTLE.put(6, new TLE("1 39741U 14026A 16059.18044747 -.00000016 00000-0 00000+0 0 9990", + "2 39741 55.2140 119.2493 0005660 259.2190 100.7882 2.00566982 13061")); + prnToTLE.put(7, new TLE("1 32711U 08012A 16059.36304856 -.00000033 +00000-0 +00000-0 0 9998", + "2 32711 055.4269 300.7399 0091867 207.6311 151.9340 02.00564257058321")); + prnToTLE.put(8, new TLE("1 40730U 15033A 16059.44106931 -.00000026 +00000-0 +00000-0 0 9994", + "2 40730 055.1388 059.0069 0020452 282.1769 077.6168 02.00566073004562")); + prnToTLE.put(9, new TLE("1 40105U 14045A 16059.27451329 .00000045 00000-0 00000+0 0 9996", + "2 40105 54.7529 238.9873 0004485 121.4766 238.5557 2.00568637 11512")); + prnToTLE.put(10, new TLE("1 41019U 15062A 16059.49433942 .00000013 00000-0 00000+0 0 9991", + "2 41019 54.9785 179.1399 0012328 204.9013 155.0292 2.00561967 2382")); + prnToTLE.put(11, new TLE("1 25933U 99055A 16059.51073770 -.00000024 00000-0 00000+0 0 9997", + "2 25933 51.3239 98.4815 0159812 86.1576 266.7718 2.00565163120122")); + prnToTLE.put(12, new TLE("1 29601U 06052A 16059.62966898 -.00000070 +00000-0 +00000-0 0 9994", + "2 29601 056.7445 002.2755 0057667 037.0706 323.3313 02.00552237067968")); + prnToTLE.put(13, new TLE("1 24876U 97035A 16059.41696335 .00000046 00000-0 00000+0 0 9998", + "2 24876 55.6966 245.8203 0044339 114.8899 245.5712 2.00562657136305")); + prnToTLE.put(14, new TLE("1 26605U 00071A 16059.56211888 .00000047 00000-0 00000+0 0 9997", + "2 26605 55.2663 243.7251 0085518 248.7231 95.5323 2.00557009112094")); + prnToTLE.put(15, new TLE("1 32260U 07047A 16059.45678257 +.00000044 +00000-0 +00000-0 0 9994", + "2 32260 053.3641 236.0940 0079746 026.4105 333.9774 02.00547771061402")); + prnToTLE.put(16, new TLE("1 27663U 03005A 16059.14440417 -.00000071 00000-0 00000+0 0 9996", + "2 27663 56.7743 3.3691 0085346 17.6322 214.4333 2.00559487 95843")); + prnToTLE.put(17, new TLE("1 28874U 05038A 16059.21070933 -.00000024 00000-0 00000+0 0 9997", + "2 28874 55.8916 61.9596 0112077 248.9647 205.7384 2.00567116 76379")); + prnToTLE.put(18, new TLE("1 26690U 01004A 16059.51332910 .00000008 00000-0 00000+0 0 9990", + "2 26690 52.9999 177.6630 0169501 250.8579 153.6293 2.00563995110499")); + prnToTLE.put(19, new TLE("1 28190U 04009A 16058.12363503 -.00000030 00000-0 00000+0 0 9999", + "2 28190 55.7230 64.8110 0105865 40.0254 321.4519 2.00572212 87500")); + prnToTLE.put(20, new TLE("1 26360U 00025A 16059.44770263 .00000005 00000-0 00000+0 0 9992", + "2 26360 53.0712 174.6895 0046205 76.0615 334.4302 2.00559931115818")); + prnToTLE.put(21, new TLE("1 27704U 03010A 16059.50719524 -.00000019 00000-0 00000+0 0 9998", + "2 27704 53.6134 117.9454 0234081 255.6874 199.2128 2.00564673 94659")); + prnToTLE.put(22, new TLE("1 28129U 03058A 16059.06680941 .00000008 00000-0 00000+0 0 9990", + "2 28129 52.8771 177.7253 0079127 245.1376 114.0279 2.00398763 89357")); + prnToTLE.put(23, new TLE("1 28361U 04023A 16059.54310021 .00000046 00000-0 00000+0 0 9995", + "2 28361 54.2347 239.3240 0106509 211.5355 11.7648 2.00557932 85613")); + prnToTLE.put(24, new TLE("1 38833U 12053A 16059.04618549 -.00000032 00000-0 00000+0 0 9999", + "2 38833 54.4591 298.1383 0042253 18.7074 341.5041 2.00568407 24895")); + prnToTLE.put(25, new TLE("1 36585U 10022A 16059.29300735 -.00000074 00000-0 00000+0 0 9993", + "2 36585 56.0738 359.4320 0050768 38.3425 49.1794 2.00578535 42134")); + prnToTLE.put(26, new TLE("1 40534U 15013A 16059.28299301 -.00000076 00000-0 00000+0 0 9994", + "2 40534 55.0430 359.0082 0009349 342.4081 17.5685 2.00558853 6801")); + prnToTLE.put(27, new TLE("1 39166U 13023A 16059.40401153 -.00000025 00000-0 00000+0 0 9990", + "2 39166 55.6020 59.1224 0032420 7.7969 352.2759 2.00568484 20414")); + prnToTLE.put(28, new TLE("1 26407U 00040A 16059.80383354 -.00000069 +00000-0 +00000-0 0 9994", + "2 26407 056.6988 003.6328 0201499 267.0948 317.6209 02.00569902114508")); + prnToTLE.put(29, new TLE("1 32384U 07062A 16059.44770263 -.00000021 00000-0 00000+0 0 9992", + "2 32384 55.9456 62.5022 0011922 319.9531 172.6730 2.00571577 60128")); + prnToTLE.put(30, new TLE("1 39533U 14008A 16059.40267873 -.00000038 +00000-0 +00000-0 0 9996", + "2 39533 054.6126 303.3404 0017140 179.4267 180.6311 02.00568364014251")); + prnToTLE.put(31, new TLE("1 29486U 06042A 16059.50651990 -.00000032 00000-0 00000+0 0 9992", + "2 29486 55.7041 301.2472 0084115 334.2804 254.9897 2.00560606 69098")); + prnToTLE.put(32, new TLE("1 41328U 16007A 16059.56873502 .00000049 00000-0 00000+0 0 9991", + "2 41328 55.0137 239.0304 0002157 298.9074 61.0768 1.99172830 453")); + + for (final GPSPropagator gpsPropagator : gpsPropagators) { + final int prn = gpsPropagator.getGPSOrbitalElements().getPRN(); + TLE tle = prnToTLE.get(prn); + TLEPropagator tlePropagator = TLEPropagator.selectExtrapolator(tle); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final AbsoluteDate date = tlePropagator.getInitialState().getDate().shiftedBy(dt); + final PVCoordinates gpsPV = gpsPropagator.getPVCoordinates(date, gpsPropagator.getECI()); + final PVCoordinates tlePV = tlePropagator.getPVCoordinates(date, gpsPropagator.getECI()); + Assert.assertEquals(0.0, Vector3D.distance(gpsPV.getPosition(), tlePV.getPosition()), 8400.0); + } + } + } + + @Test + public void testDerivativesConsistency() { + + final Frame eme2000 = FramesFactory.getEME2000(); + double errorP = 0; + double errorV = 0; + double errorA = 0; + for (final GPSAlmanac almanac : almanacs) { + GPSPropagator propagator = new GPSPropagator.Builder(almanac).build(); + GPSOrbitalElements elements = propagator.getGPSOrbitalElements(); + AbsoluteDate t0 = new GNSSDate(elements.getWeek(), 0.001 * elements.getTime(), SatelliteSystem.GPS) + .getDate(); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final AbsoluteDate central = t0.shiftedBy(dt); + final PVCoordinates pv = propagator.getPVCoordinates(central, eme2000); + final double h = 10.0; + List sample = new ArrayList(); + for (int i = -3; i <= 3; ++i) { + sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); + } + final PVCoordinates interpolated = TimeStampedPVCoordinates.interpolate(central, + CartesianDerivativesFilter.USE_P, sample); + errorP = FastMath.max(errorP, Vector3D.distance(pv.getPosition(), interpolated.getPosition())); + errorV = FastMath.max(errorV, Vector3D.distance(pv.getVelocity(), interpolated.getVelocity())); + errorA = FastMath.max(errorA, Vector3D.distance(pv.getAcceleration(), interpolated.getAcceleration())); + } + } + Assert.assertEquals(0.0, errorP, 3.8e-9); + Assert.assertEquals(0.0, errorV, 3.5e-8); + Assert.assertEquals(0.0, errorA, 1.1e-8); + + } + + @Test + public void testPosition() { + // Initial GPS orbital elements (Ref: IGS) + GPSOrbitalElements goe = new GPSEphemeris(7, 0, 288000, 5153.599830627441, 0.012442796607501805, + 4.419469802942352E-9, 0.9558937988021613, -2.4608167886110235E-10, 1.0479401362158658, + -7.967117576712062E-9, -2.4719019944000538, -1.0899023379614294, 4.3995678424835205E-6, + 1.002475619316101E-5, 183.40625, 87.03125, 3.203749656677246E-7, 4.0978193283081055E-8); + + // Date of the GPS orbital elements + final AbsoluteDate target = goe.getDate(); + // Build the GPS propagator + final GPSPropagator propagator = new GPSPropagator.Builder(goe).build(); + // Compute the PV coordinates at the date of the GPS orbital elements + final PVCoordinates pv = propagator.getPVCoordinates(target, + FramesFactory.getITRF(IERSConventions.IERS_2010, true)); + // Computed position + final Vector3D computedPos = pv.getPosition(); + // Expected position (reference from IGS file igu20484_00.sp3) + final Vector3D expectedPos = new Vector3D(-4920705.292, 24248099.200, 9236130.101); + + Assert.assertEquals(0., Vector3D.distance(expectedPos, computedPos), 3.2); + } + + @Test + public void testIssue544() { + // Builds the GPSPropagator from the almanac + final GPSPropagator propagator = new GPSPropagator.Builder(almanacs.get(0)).build(); + // In order to test the issue, we volontary set a Double.NaN value in the date. + final AbsoluteDate date0 = new AbsoluteDate(2010, 5, 7, 7, 50, Double.NaN, TimeScalesFactory.getUTC()); + final PVCoordinates pv0 = propagator.propagateInEcef(date0); + // Verify that an infinite loop did not occur + Assert.assertEquals(Vector3D.NaN, pv0.getPosition()); + Assert.assertEquals(Vector3D.NaN, pv0.getVelocity()); + + } + + private class GPSEphemeris implements GPSOrbitalElements { + + private int prn; + private int week; + private double toe; + private double sma; + private double deltaN; + private double ecc; + private double inc; + private double iDot; + private double om0; + private double dom; + private double aop; + private double anom; + private double cuc; + private double cus; + private double crc; + private double crs; + private double cic; + private double cis; + + /** + * Build a new instance. + */ + GPSEphemeris(int prn, int week, double toe, double sqa, double ecc, double deltaN, double inc, double iDot, + double om0, double dom, double aop, double anom, double cuc, double cus, double crc, double crs, + double cic, double cis) { + this.prn = prn; + this.week = week; + this.toe = toe; + this.sma = sqa * sqa; + this.ecc = ecc; + this.deltaN = deltaN; + this.inc = inc; + this.iDot = iDot; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.cuc = cuc; + this.cus = cus; + this.crc = crc; + this.crs = crs; + this.cic = cic; + this.cis = cis; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public double getTime() { + return toe; + } + + @Override + public double getSma() { + return sma; + } + + @Override + public double getMeanMotion() { + final double absA = FastMath.abs(sma); + return FastMath.sqrt(GPS_MU / absA) / absA + deltaN; + } + + @Override + public double getE() { + return ecc; + } + + @Override + public double getI0() { + return inc; + } + + @Override + public double getIDot() { + return iDot; + } + + @Override + public double getOmega0() { + return om0; + } + + @Override + public double getOmegaDot() { + return dom; + } + + @Override + public double getPa() { + return aop; + } + + @Override + public double getM0() { + return anom; + } + + @Override + public double getCuc() { + return cuc; + } + + @Override + public double getCus() { + return cus; + } + + @Override + public double getCrc() { + return crc; + } + + @Override + public double getCrs() { + return crs; + } + + @Override + public double getCic() { + return cic; + } + + @Override + public double getCis() { + return cis; + } + + @Override + public AbsoluteDate getDate() { + return new GNSSDate(week, toe * 1000., SatelliteSystem.GPS).getDate(); + } + + } } -- GitLab From d4cbdbc366ae18ebb4b7807d543b8d488f381de2 Mon Sep 17 00:00:00 2001 From: nfialton Date: Fri, 2 Apr 2021 09:37:30 +0200 Subject: [PATCH 02/26] Commit of FieldSpacecraftState and FieldSpacecraftStateTest --- .../propagation/FieldSpacecraftState.java | 2 +- .../propagation/FieldSpacecraftStateTest.java | 74 +++++++++++++++++++ 2 files changed, 75 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/orekit/propagation/FieldSpacecraftState.java b/src/main/java/org/orekit/propagation/FieldSpacecraftState.java index 0abf20b6b..3779af962 100644 --- a/src/main/java/org/orekit/propagation/FieldSpacecraftState.java +++ b/src/main/java/org/orekit/propagation/FieldSpacecraftState.java @@ -622,7 +622,7 @@ public class FieldSpacecraftState > absPvas.add(state.getAbsPVA()); } attitudes.add(state.getAttitude()); - final T[] mm = MathArrays.buildArray(orbit.getA().getField(), 1); + final T[] mm = MathArrays.buildArray(date.getField(), 1); mm[0] = state.getMass(); massInterpolator.addSamplePoint(deltaT, mm); diff --git a/src/test/java/org/orekit/propagation/FieldSpacecraftStateTest.java b/src/test/java/org/orekit/propagation/FieldSpacecraftStateTest.java index c89b0554c..a097c813d 100644 --- a/src/test/java/org/orekit/propagation/FieldSpacecraftStateTest.java +++ b/src/test/java/org/orekit/propagation/FieldSpacecraftStateTest.java @@ -22,6 +22,7 @@ import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.Map; +import java.util.stream.Stream; import org.hipparchus.Field; import org.hipparchus.RealFieldElement; @@ -45,16 +46,19 @@ import org.junit.Test; import org.orekit.Utils; import org.orekit.attitudes.BodyCenterPointing; import org.orekit.attitudes.FieldAttitude; +import org.orekit.attitudes.LofOffset; import org.orekit.bodies.OneAxisEllipsoid; import org.orekit.errors.OrekitException; import org.orekit.errors.OrekitMessages; import org.orekit.frames.FramesFactory; +import org.orekit.frames.LOFType; import org.orekit.frames.Transform; import org.orekit.orbits.FieldKeplerianOrbit; import org.orekit.orbits.FieldOrbit; import org.orekit.orbits.KeplerianOrbit; import org.orekit.orbits.Orbit; import org.orekit.orbits.PositionAngle; +import org.orekit.propagation.analytical.Ephemeris; import org.orekit.propagation.analytical.FieldEcksteinHechlerPropagator; import org.orekit.propagation.analytical.FieldKeplerianPropagator; import org.orekit.propagation.events.FieldDateDetector; @@ -65,11 +69,13 @@ import org.orekit.time.DateComponents; import org.orekit.time.FieldAbsoluteDate; import org.orekit.time.TimeComponents; import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.AbsolutePVCoordinates; import org.orekit.utils.Constants; import org.orekit.utils.FieldAbsolutePVCoordinates; import org.orekit.utils.FieldPVCoordinates; import org.orekit.utils.IERSConventions; import org.orekit.utils.PVCoordinates; +import org.orekit.utils.TimeStampedPVCoordinates; public class FieldSpacecraftStateTest { @@ -143,6 +149,11 @@ public class FieldSpacecraftStateTest { public void testResetOnEventNumerical() { doTestAdditionalTestResetOnEventNumerical(Decimal64Field.getInstance()); } + + @Test + public void testIssue775() { + doTestIssue775(Decimal64Field.getInstance()); + } private > void doTestFieldVsReal(final Field field) { T zero = field.getZero(); @@ -924,7 +935,70 @@ public class FieldSpacecraftStateTest { Assert.assertEquals(+1, finalState.getAdditionalState(name)[0].getReal(), 1.0e-15); } + + + + private > void doTestIssue775(final Field field) { + + final List> states = new ArrayList>(); + + states.add(new FieldSpacecraftState<>( new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), + new FieldAbsoluteDate<>(field, new AbsoluteDate()), + new FieldVector3D<>(field, new Vector3D(1, 0, 0)), + new FieldVector3D<>(field, new Vector3D(1, 0, 0))))); + + states.add(new FieldSpacecraftState<>( new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), + new FieldAbsoluteDate<>(field, new AbsoluteDate()), + new FieldVector3D<>(field, new Vector3D(2, 0, 0)), + new FieldVector3D<>(field, new Vector3D(2, 0, 0))))); + + states.get(0).interpolate(states.get(0).getDate(), Stream> states); + + Assert.assertNotNull(states); + + + + // + // Initial PV coordinates + AbsolutePVCoordinates initPV = new AbsolutePVCoordinates(inertialFrame, + new TimeStampedPVCoordinates(initDate, + new PVCoordinates(new Vector3D(-29536113.0, 30329259.0, -100125.0), + new Vector3D(-2194.0, -2141.0, -8.0)))); + // Input parameters + int numberOfInterals = 1440; + double deltaT = finalDate.durationFrom(initDate)/((double)numberOfInterals); + + // Build the list of spacecraft states + List states = new ArrayList(numberOfInterals + 1); + for (int j = 0; j<= numberOfInterals; j++) { + states.add(new SpacecraftState(initPV).shiftedBy(j * deltaT)); + } + + // Build the epemeris propagator + Ephemeris ephemPropagator = new Ephemeris(states, 2); + + // Get initial state without attitude provider + SpacecraftState withoutAttitudeProvider = ephemPropagator.getInitialState(); + Assert.assertEquals(0.0, Vector3D.distance(withoutAttitudeProvider.getAbsPVA().getPosition(), initPV.getPosition()), 1.0e-10); + Assert.assertEquals(0.0, Vector3D.distance(withoutAttitudeProvider.getAbsPVA().getVelocity(), initPV.getVelocity()), 1.0e-10); + + // Set an attitude provider + ephemPropagator.setAttitudeProvider(new LofOffset(inertialFrame, LOFType.LVLH_CCSDS)); + + // Get initial state with attitude provider + SpacecraftState withAttitudeProvider = ephemPropagator.getInitialState(); + Assert.assertEquals(0.0, Vector3D.distance(withAttitudeProvider.getAbsPVA().getPosition(), initPV.getPosition()), 1.0e-10); + Assert.assertEquals(0.0, Vector3D.distance(withAttitudeProvider.getAbsPVA().getVelocity(), initPV.getVelocity()), 1.0e-10); + + + } + + + + + + @Before public void setUp(){ try { -- GitLab From 1c311af2d6fbd361b3c2571e40b359ee9cb6f573 Mon Sep 17 00:00:00 2001 From: nfialton Date: Wed, 14 Apr 2021 15:13:01 +0200 Subject: [PATCH 03/26] JavaDoc correction in BeidouPropagator (bdsOrbElt instead of gpsOrbElt) --- .../orekit/propagation/analytical/gnss/BeidouPropagator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/BeidouPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/BeidouPropagator.java index 4a64f4f01..7bc1e63f3 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/BeidouPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/BeidouPropagator.java @@ -85,7 +85,7 @@ public class BeidouPropagator extends AbstractGNSSPropagator { * *

This constructor uses the {@link DataContext#getDefault() default data context}. * Another data context can be set using - * {@code Builder(final BeidouOrbitalElements gpsOrbElt, final Frames frames)}

+ * {@code Builder(final BeidouOrbitalElements bdsOrbElt, final Frames frames)}

* * @param bdsOrbElt the Beidou orbital elements to be used by the Beidou propagator. * @see #attitudeProvider(AttitudeProvider provider) -- GitLab From 7ef5ad5f860fe0db7b14a43d308e3e4bba307573 Mon Sep 17 00:00:00 2001 From: nfialton Date: Wed, 14 Apr 2021 15:16:33 +0200 Subject: [PATCH 04/26] Addition of some getters in GLONASSAlmanac (glonass, day, month, year) --- .../java/org/orekit/gnss/GLONASSAlmanac.java | 20 +++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/orekit/gnss/GLONASSAlmanac.java b/src/main/java/org/orekit/gnss/GLONASSAlmanac.java index 942cdcc5b..7037338f5 100644 --- a/src/main/java/org/orekit/gnss/GLONASSAlmanac.java +++ b/src/main/java/org/orekit/gnss/GLONASSAlmanac.java @@ -204,8 +204,12 @@ public class GLONASSAlmanac implements GLONASSOrbitalElements { public double getDeltaTDot() { return deltaTDot; } + + public TimeScale getGlonass() { + return glonass; + } - /** + /** * Get the Health status. * * @return the Health status @@ -250,7 +254,19 @@ public class GLONASSAlmanac implements GLONASSOrbitalElements { return tGlo; } - @Override + public int getDay() { + return day; + } + + public int getMonth() { + return month; + } + + public int getYear() { + return year; + } + + @Override public int getNa() { final GLONASSDate gloDate = new GLONASSDate(getDate(), glonass); return gloDate.getDayNumber(); -- GitLab From 4e519c78381cb062661342361bf1597ae80e5edc Mon Sep 17 00:00:00 2001 From: nfialton Date: Wed, 14 Apr 2021 15:27:09 +0200 Subject: [PATCH 05/26] Field analytical propagators (GNSS and more) + Tests --- .../FieldAbstractAnalyticalPropagator.java | 2 - .../analytical/FieldAdapterPropagator.java | 39 +- .../analytical/FieldEphemeris.java | 11 +- .../FieldSmallManeuverAnalyticalModel.java | 2 +- .../gnss/FieldAbstractGNSSPropagator.java | 597 +++---- .../analytical/gnss/FieldBeidouAlmanac.java | 32 +- .../gnss/FieldBeidouPropagator.java | 101 +- .../analytical/gnss/FieldGLONASSAlmanac.java | 351 ++++ .../FieldGLONASSAnalyticalPropagator.java | 1519 ++++++++--------- .../analytical/gnss/FieldGLONASSDate.java | 508 +++--- .../gnss/FieldGNSSOrbitalElements.java | 28 +- .../analytical/gnss/FieldGPSAlmanac.java | 7 +- .../gnss/FieldGPSOrbitalElements.java | 2 +- .../analytical/gnss/FieldGPSPropagator.java | 86 +- .../analytical/gnss/FieldGalileoAlmanac.java | 31 + .../gnss/FieldGalileoOrbitalElements.java | 16 + .../gnss/FieldGalileoPropagator.java | 105 +- .../analytical/gnss/FieldIRNSSAlmanac.java | 3 +- .../gnss/FieldIRNSSOrbitalElements.java | 25 + .../analytical/gnss/FieldIRNSSPropagator.java | 68 +- .../analytical/gnss/FieldQZSSAlmanac.java | 40 +- .../analytical/gnss/FieldQZSSPropagator.java | 119 +- .../gnss/FieldSBASOrbitalElements.java | 2 +- .../analytical/gnss/FieldSBASPropagator.java | 125 +- ...FieldSmallManeuverAnalyticalModelTest.java | 15 +- .../analytical/FieldEphemerisTest.java | 30 +- .../analytical/FieldJ2DifferentialEffect.java | 21 +- .../FieldGLONASSAnalyticalPropagatorTest.java | 199 +++ .../gnss/FieldGPSPropagatorTest.java | 16 + .../gnss/FieldIRNSSPropagatorTest.java | 20 +- .../gnss/FieldQZSSPropagatorTest.java | 41 +- .../gnss/FieldSBASPropagatorTest.java | 20 +- 32 files changed, 2670 insertions(+), 1511 deletions(-) create mode 100644 src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAlmanac.java create mode 100644 src/test/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagatorTest.java diff --git a/src/main/java/org/orekit/propagation/analytical/FieldAbstractAnalyticalPropagator.java b/src/main/java/org/orekit/propagation/analytical/FieldAbstractAnalyticalPropagator.java index 2306eb7c2..99a0071c3 100644 --- a/src/main/java/org/orekit/propagation/analytical/FieldAbstractAnalyticalPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/FieldAbstractAnalyticalPropagator.java @@ -169,8 +169,6 @@ public abstract class FieldAbstractAnalyticalPropagator current = updateAdditionalStates(basicPropagate(t)); final FieldBasicStepInterpolator interpolator = new FieldBasicStepInterpolator(dt.getReal() >= 0, previous, current); - - // accept the step, trigger events and step handlers state = acceptStep(interpolator, target, epsilon); } while (!isLastStep); diff --git a/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java b/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java index 750626d5f..7b39de84b 100644 --- a/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java @@ -1,3 +1,19 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical; import java.util.ArrayList; @@ -12,22 +28,19 @@ import org.orekit.errors.OrekitMessages; import org.orekit.orbits.FieldOrbit; import org.orekit.propagation.FieldPropagator; import org.orekit.propagation.FieldSpacecraftState; -import org.orekit.propagation.Propagator; -import org.orekit.propagation.SpacecraftState; -import org.orekit.propagation.analytical.AdapterPropagator.DifferentialEffect; import org.orekit.time.FieldAbsoluteDate; import org.orekit.utils.ParameterDriver; /** - * Orbit propagator that adapts an underlying propagator, adding - * {@link DifferentialEffect differential effects}. + * Field Orbit propagator that adapts an underlying propagator, adding + * {@link FieldDifferentialEffect differential effects}. *

* This propagator is used when a reference propagator does not handle some * effects that we need. A typical example would be an ephemeris that was * computed for a reference orbit, and we want to compute a station-keeping * maneuver on top of this ephemeris, changing its final state. The principal is * to add one or more - * {@link org.orekit.forces.maneuvers.SmallManeuverAnalyticalModel small + * {@link org.orekit.forces.maneuvers.FieldSmallManeuverAnalyticalModel small * maneuvers analytical models} to it and use it as a new propagator, which * takes the maneuvers into account. *

@@ -37,8 +50,8 @@ import org.orekit.utils.ParameterDriver; * decorator design pattern. *

* - * @see Propagator - * @see org.orekit.forces.maneuvers.SmallManeuverAnalyticalModel + * @see FieldPropagator + * @see org.orekit.forces.maneuvers.FieldSmallManeuverAnalyticalModel * @author Luc Maisonobe * @author Nicolas Fialton (field translation) */ @@ -46,7 +59,7 @@ public class FieldAdapterPropagator> extends Field /** Interface for orbit differential effects. */ public interface FieldDifferentialEffect> { - /** Apply the effect to a {@link SpacecraftState spacecraft state}. + /** Apply the effect to a {@link FieldSpacecraftState spacecraft state}. *

* Applying the effect may be a no-op in some cases. A typical example * is maneuvers, for which the state is changed only for time after @@ -70,6 +83,7 @@ public class FieldAdapterPropagator> extends Field *

The reference propagator can be almost anything, numerical, * analytical, and even an ephemeris. It may already take some maneuvers * into account.

+ * @param field * @param reference reference propagator */ public FieldAdapterPropagator(Field field, final FieldPropagator reference) { @@ -159,10 +173,13 @@ public class FieldAdapterPropagator> extends Field return null; } + /** Get the parameters driver for the Field SBAS propagation model. + * @return an empty list. + */ @Override protected List getParametersDrivers() { - // TODO Auto-generated method stub - return null; + // The Field Adapter propagation model does not have parameter drivers. + return Collections.emptyList(); } } diff --git a/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java b/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java index e653a6f46..656dc9704 100644 --- a/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java +++ b/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java @@ -100,13 +100,14 @@ public class FieldEphemeris> extends FieldAbstract * This constructor uses the {@link DataContext#getDefault() default data * context}. * + * @param field * @param states tabulates states * @param interpolationPoints number of points to use in interpolation * @exception MathIllegalArgumentException if the number of states is smaller * than the number of points to use in * interpolation - * @see #Ephemeris(List, int, double) - * @see #Ephemeris(List, int, double, AttitudeProvider) + * @see #Ephemeris(List, int, T) + * @see #Ephemeris(List, int, T, AttitudeProvider) */ @DefaultDataContext public FieldEphemeris(Field field, final List states, final int interpolationPoints) @@ -121,6 +122,7 @@ public class FieldEphemeris> extends FieldAbstract * This constructor uses the {@link DataContext#getDefault() default data * context}. * + * @param field * @param states tabulates states * @param interpolationPoints number of points to use in interpolation * @param extrapolationThreshold the largest time difference in seconds between @@ -129,8 +131,7 @@ public class FieldEphemeris> extends FieldAbstract * @exception MathIllegalArgumentException if the number of states is smaller * than the number of points to use in * interpolation - * @since 9.0 - * @see #Ephemeris(List, int, double, AttitudeProvider) + * @see #FieldEphemeris(field, List, int, T, AttitudeProvider) */ @DefaultDataContext public FieldEphemeris(Field field, final List states, final int interpolationPoints, @@ -142,6 +143,7 @@ public class FieldEphemeris> extends FieldAbstract /** * Constructor with tabulated states. * + * @param field * @param states tabulates states * @param interpolationPoints number of points to use in interpolation * @param extrapolationThreshold the largest time difference in seconds between @@ -151,7 +153,6 @@ public class FieldEphemeris> extends FieldAbstract * @exception MathIllegalArgumentException if the number of states is smaller * than the number of points to use in * interpolation - * @since 10.1 */ public FieldEphemeris(Field field, final List states, final int interpolationPoints, final T extrapolationThreshold, final AttitudeProvider attitudeProvider) diff --git a/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java b/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java index 7986f0f3e..cf77417cc 100644 --- a/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java +++ b/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java @@ -286,7 +286,7 @@ public class FieldSmallManeuverAnalyticalModel> if (dt.getReal() < 0) { // the maneuver has not occurred yet, Jacobian is null for (int i = 0; i < 6; ++i) { - Arrays.fill(jacobian[i], 0, 4, 0.0); + Arrays.fill(jacobian[i], 0, 4, dt.getField().getZero()); } return; } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java index c966649db..ed0e442d3 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java @@ -16,9 +16,6 @@ */ package org.orekit.propagation.analytical.gnss; -import java.util.Collections; -import java.util.List; - import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2; @@ -36,14 +33,14 @@ import org.orekit.propagation.FieldSpacecraftState; import org.orekit.propagation.analytical.FieldAbstractAnalyticalPropagator; import org.orekit.time.FieldAbsoluteDate; import org.orekit.utils.FieldPVCoordinates; -import org.orekit.utils.ParameterDriver; /** * Common handling of {@link FieldAbstractAnalyticalPropagator} methods for GNSS * propagators. *

* This abstract class allows to provide easily a subset of - * {@link FieldAbstractAnalyticalPropagator} methods for specific GNSS propagators. + * {@link FieldAbstractAnalyticalPropagator} methods for specific GNSS + * propagators. *

* * @author Pascal Parraud @@ -89,300 +86,304 @@ public abstract class FieldAbstractGNSSPropagator> /** The ECEF frame used for GNSS propagation. */ private final Frame ecef; - /** Build a new instance. - * @param gnssOrbit the common GNSS orbital elements to be used by the Abstract GNSS propagator - * @param attitudeProvider provider for attitude computation - * @param eci the ECI frame used for GNSS propagation - * @param ecef the ECEF frame used for GNSS propagation - * @param mass the spacecraft mass (kg) - * @param av mean angular velocity of the Earth (rad/s) - * @param cycleDuration duration of the GNSS cycle in seconds - * @param mu the Earth gravity coefficient used for GNSS propagation - */ - - protected FieldAbstractGNSSPropagator(final Field field, final FieldGNSSOrbitalElements gnssOrbit, - final AttitudeProvider attitudeProvider, - final Frame eci, final Frame ecef, final double mass, - final double av, final double cycleDuration, final double mu) { - super(field, attitudeProvider); - this.gnssOrbit = gnssOrbit; - this.av = av; - this.cycleDuration = cycleDuration; - this.mass = field.getZero().add(mass); - this.mu = field.getZero().add(mu); - // Sets the Earth Centered Inertial frame - this.eci = eci; - // Sets the Earth Centered Earth Fixed frame - this.ecef = ecef; - // Sets the start date as the date of the orbital elements - setStartDate(gnssOrbit.getDate()); - } - - /** Get the duration from GNSS Reference epoch. - *

This takes the GNSS week roll-over into account.

- * @param date the considered date - * @return the duration from GNSS orbit Reference epoch (s) - */ - private T getTk(final FieldAbsoluteDate date) { - // Time from ephemeris reference epoch - T tk = date.durationFrom(gnssOrbit.getDate()); - // Adjusts the time to take roll over week into account - while (tk.getReal() > 0.5 * cycleDuration) { - tk = tk.subtract(cycleDuration); - } - while (tk.getReal() < -0.5 * cycleDuration) { - tk = tk.add(cycleDuration); - } - // Returns the time from ephemeris reference epoch - return tk; - } - - - /** - * Gets the PVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}. - * - *

The algorithm uses automatic differentiation to compute velocity and - * acceleration.

- * - * @param date the computation date - * @return the GNSS SV PVCoordinates in {@link #getECEF() ECEF frame} - */ - public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { - // Field - final Field field = date.getField(); - // Duration from GNSS ephemeris Reference date - final FieldUnivariateDerivative2 tk = new FieldUnivariateDerivative2<>(getTk(date), field.getOne(), field.getZero()); - // Mean anomaly - final FieldUnivariateDerivative2 mk = tk.multiply(gnssOrbit.getMeanMotion()).add(gnssOrbit.getM0()); - // Eccentric Anomaly - final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk); - // True Anomaly - final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek); - // Argument of Latitude - final FieldUnivariateDerivative2 phik = vk.add(gnssOrbit.getPa()); - final FieldUnivariateDerivative2 twoPhik = phik.multiply(2); - final FieldUnivariateDerivative2 c2phi = twoPhik.cos(); - final FieldUnivariateDerivative2 s2phi = twoPhik.sin(); - // Argument of Latitude Correction - final FieldUnivariateDerivative2 dphik = c2phi.multiply(gnssOrbit.getCuc()).add(s2phi.multiply(gnssOrbit.getCus())); - // Radius Correction - final FieldUnivariateDerivative2 drk = c2phi.multiply(gnssOrbit.getCrc()).add(s2phi.multiply(gnssOrbit.getCrs())); - // Inclination Correction - final FieldUnivariateDerivative2 dik = c2phi.multiply(gnssOrbit.getCic()).add(s2phi.multiply(gnssOrbit.getCis())); - // Corrected Argument of Latitude - final FieldUnivariateDerivative2 uk = phik.add(dphik); - // Corrected Radius - final FieldUnivariateDerivative2 rk = ek.cos().multiply((gnssOrbit.getE()).negate()).add(1).multiply(gnssOrbit.getSma()).add(drk); - // Corrected Inclination - final FieldUnivariateDerivative2 ik = tk.multiply(gnssOrbit.getIDot()).add(gnssOrbit.getI0()).add(dik); - final FieldUnivariateDerivative2 cik = ik.cos(); - // Positions in orbital plane - final FieldUnivariateDerivative2 xk = uk.cos().multiply(rk); - final FieldUnivariateDerivative2 yk = uk.sin().multiply(rk); - // Corrected longitude of ascending node - final FieldUnivariateDerivative2 omk = tk.multiply((gnssOrbit.getOmegaDot()).subtract(av)). - add(gnssOrbit.getOmega0().subtract(gnssOrbit.getTime().multiply(av)) ); - final FieldUnivariateDerivative2 comk = omk.cos(); - final FieldUnivariateDerivative2 somk = omk.sin(); - // returns the Earth-fixed coordinates - final FieldVector3D> positionwithDerivatives = - new FieldVector3D<>(xk.multiply(comk).subtract(yk.multiply(somk).multiply(cik)), - xk.multiply(somk).add(yk.multiply(comk).multiply(cik)), - yk.multiply(ik.sin())); - return new FieldPVCoordinates(new FieldVector3D(positionwithDerivatives.getX().getValue(), - positionwithDerivatives.getY().getValue(), - positionwithDerivatives.getZ().getValue()), - new FieldVector3D(positionwithDerivatives.getX().getFirstDerivative(), - positionwithDerivatives.getY().getFirstDerivative(), - positionwithDerivatives.getZ().getFirstDerivative()), - new FieldVector3D(positionwithDerivatives.getX().getSecondDerivative(), - positionwithDerivatives.getY().getSecondDerivative(), - positionwithDerivatives.getZ().getSecondDerivative())); - } - - - - - - - /** - * Gets eccentric anomaly from mean anomaly. - *

The algorithm used to solve the Kepler equation has been published in: - * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. Gooding, - * Celestial Mechanics 38 (1986) 307-334

- *

It has been copied from the OREKIT library (KeplerianOrbit class).

- * - * @param mk the mean anomaly (rad) - * @return the eccentric anomaly (rad) - */ - private FieldUnivariateDerivative2 getEccentricAnomaly(final FieldUnivariateDerivative2 mk) { - - final T zero = mk.getValue().getField().getZero(); - // reduce M to [-PI PI] interval - final FieldUnivariateDerivative2 reducedM = new FieldUnivariateDerivative2(MathUtils.normalizeAngle(mk.getValue(), zero), - mk.getFirstDerivative(), - mk.getSecondDerivative()); - - // compute start value according to A. W. Odell and R. H. Gooding S12 starter - FieldUnivariateDerivative2 ek; - if (FastMath.abs(reducedM.getValue().getReal()) < 1.0 / 6.0) { - if (FastMath.abs(reducedM.getValue().getReal()) < Precision.SAFE_MIN) { - // this is an Orekit change to the S12 starter. - // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN appearing later in - // the computation. As in this case E and M are almost equal, we initialize ek with reducedM - ek = reducedM; - } else { - // this is the standard S12 starter - ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(gnssOrbit.getE())); - } - } else { - if (reducedM.getValue().getReal() < 0) { - final FieldUnivariateDerivative2 w = reducedM.add(FastMath.PI); - ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(gnssOrbit.getE())); - } else { - final FieldUnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI); - ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(gnssOrbit.getE())); - } - } - - final T e1 = gnssOrbit.getE().negate().add(1.0); - final boolean noCancellationRisk = (e1.getReal() + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; - - // perform two iterations, each consisting of one Halley step and one Newton-Raphson step - for (int j = 0; j < 2; ++j) { - final FieldUnivariateDerivative2 f; - FieldUnivariateDerivative2 fd; - final FieldUnivariateDerivative2 fdd = ek.sin().multiply(gnssOrbit.getE()); - final FieldUnivariateDerivative2 fddd = ek.cos().multiply(gnssOrbit.getE()); - if (noCancellationRisk) { - f = ek.subtract(fdd).subtract(reducedM); - fd = fddd.subtract(1).negate(); - } else { - f = eMeSinE(ek).subtract(reducedM); - final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); - fd = s.multiply(s).multiply(gnssOrbit.getE().multiply(2.0)).add(e1); - } - final FieldUnivariateDerivative2 dee = f.multiply(fd).divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd))); - - // update eccentric anomaly, using expressions that limit underflow problems - final FieldUnivariateDerivative2 w = fd.add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3)))); - fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); - ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd)); - } - - // expand the result back to original range - ek = ek.add((mk.getValue()).subtract(reducedM.getValue())); - - // Returns the eccentric anomaly - return ek; - } - - /** - * Accurate computation of E - e sin(E). - * - * @param E eccentric anomaly - * @return E - e sin(E) - */ - private FieldUnivariateDerivative2 eMeSinE(final FieldUnivariateDerivative2 E) { - FieldUnivariateDerivative2 x = E.sin().multiply(gnssOrbit.getE().negate().add(1)); - final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); - FieldUnivariateDerivative2 term = E; - FieldUnivariateDerivative2 d = E.getField().getZero(); - // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance - for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); !(x.getValue()).equals(x0.getValue());) { - d = d.add(2); - term = term.multiply(mE2.divide(d.multiply(d.add(1)))); - x0 = x; - x = x.subtract(term); - } - return x; - } - - /** Gets true anomaly from eccentric anomaly. - * - * @param ek the eccentric anomaly (rad) - * @return the true anomaly (rad) - */ - private FieldUnivariateDerivative2 getTrueAnomaly(final FieldUnivariateDerivative2 ek) { - final FieldUnivariateDerivative2 svk = ek.sin().multiply(FastMath.sqrt((gnssOrbit.getE().multiply(gnssOrbit.getE())).negate().add(1.0) )); - final FieldUnivariateDerivative2 cvk = ek.cos().subtract(gnssOrbit.getE()); - return svk.atan2(cvk); - } - - /** {@inheritDoc} */ - protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, final T[] parameters) { - // Gets the PVCoordinates in ECEF frame - final FieldPVCoordinates pvaInECEF = propagateInEcef(date); - // Transforms the PVCoordinates to ECI frame - final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); - // Returns the Cartesian orbit - return new FieldCartesianOrbit(pvaInECI, eci, date, mu); - } - - /** - * Get the Earth gravity coefficient used for GNSS propagation. - * @return the Earth gravity coefficient. - */ - public T getMU() { - return mu; - } - - /** {@inheritDoc} */ - public Frame getFrame() { - return eci; - } - - /** {@inheritDoc} */ - protected T getMass(final FieldAbsoluteDate date) { - return mass; - } - - /** {@inheritDoc} */ - public void resetInitialState(final FieldSpacecraftState state) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - /** {@inheritDoc} */ - protected void resetIntermediateState(final FieldSpacecraftState state, final boolean forward) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - - - - - /** {@inheritDoc} */ - @Override - protected List getParametersDrivers() { - // GNSS propagation model does not have parameter drivers. - return Collections.emptyList(); - } - - - - - /** - * Gets the Earth Centered Inertial frame used to propagate the orbit. - * - * @return the ECI frame - */ - public Frame getECI() { - return eci; - } - - /** - * Gets the Earth Centered Earth Fixed frame used to propagate GNSS orbits according to the - * Interface Control Document. - * - * @return the ECEF frame - */ - public Frame getECEF() { - return ecef; - } + /** + * Build a new instance. + * + * @param field + * @param gnssOrbit the common Field GNSS orbital elements to be used by the Field + * Abstract GNSS propagator + * @param attitudeProvider provider for attitude computation + * @param eci the ECI frame used for GNSS propagation + * @param ecef the ECEF frame used for GNSS propagation + * @param mass the spacecraft mass (kg) + * @param av mean angular velocity of the Earth (rad/s) + * @param cycleDuration duration of the GNSS cycle in seconds + * @param mu the Earth gravity coefficient used for GNSS + * propagation + */ + + protected FieldAbstractGNSSPropagator(final Field field, final FieldGNSSOrbitalElements gnssOrbit, + final AttitudeProvider attitudeProvider, final Frame eci, final Frame ecef, final double mass, + final double av, final double cycleDuration, final double mu) { + super(field, attitudeProvider); + this.gnssOrbit = gnssOrbit; + this.av = av; + this.cycleDuration = cycleDuration; + this.mass = field.getZero().add(mass); + this.mu = field.getZero().add(mu); + // Sets the Earth Centered Inertial frame + this.eci = eci; + // Sets the Earth Centered Earth Fixed frame + this.ecef = ecef; + // Sets the start date as the date of the orbital elements + setStartDate(gnssOrbit.getDate()); + } -} + /** + * Get the duration from GNSS Reference epoch. + *

+ * This takes the GNSS week roll-over into account. + *

+ * + * @param date the considered date + * @return the duration from GNSS orbit Reference epoch (s) + */ + private T getTk(final FieldAbsoluteDate date) { + // Time from ephemeris reference epoch + T tk = date.durationFrom(gnssOrbit.getDate()); + // Adjusts the time to take roll over week into account + while (tk.getReal() > 0.5 * cycleDuration) { + tk = tk.subtract(cycleDuration); + } + while (tk.getReal() < -0.5 * cycleDuration) { + tk = tk.add(cycleDuration); + } + // Returns the time from ephemeris reference epoch + return tk; + } + + /** + * Gets the FieldPVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}. + * + *

+ * The algorithm uses automatic differentiation to compute velocity and + * acceleration. + *

+ * + * @param date the computation date + * @return the GNSS SV FieldPVCoordinates in {@link #getECEF() ECEF frame} + */ + public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { + // Field + final Field field = date.getField(); + // Duration from GNSS ephemeris Reference date + final FieldUnivariateDerivative2 tk = new FieldUnivariateDerivative2<>(getTk(date), field.getOne(), + field.getZero()); + // Mean anomaly + final FieldUnivariateDerivative2 mk = tk.multiply(gnssOrbit.getMeanMotion()).add(gnssOrbit.getM0()); + // Eccentric Anomaly + final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk); + // True Anomaly + final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek); + // Argument of Latitude + final FieldUnivariateDerivative2 phik = vk.add(gnssOrbit.getPa()); + final FieldUnivariateDerivative2 twoPhik = phik.multiply(2); + final FieldUnivariateDerivative2 c2phi = twoPhik.cos(); + final FieldUnivariateDerivative2 s2phi = twoPhik.sin(); + // Argument of Latitude Correction + final FieldUnivariateDerivative2 dphik = c2phi.multiply(gnssOrbit.getCuc()) + .add(s2phi.multiply(gnssOrbit.getCus())); + // Radius Correction + final FieldUnivariateDerivative2 drk = c2phi.multiply(gnssOrbit.getCrc()) + .add(s2phi.multiply(gnssOrbit.getCrs())); + // Inclination Correction + final FieldUnivariateDerivative2 dik = c2phi.multiply(gnssOrbit.getCic()) + .add(s2phi.multiply(gnssOrbit.getCis())); + // Corrected Argument of Latitude + final FieldUnivariateDerivative2 uk = phik.add(dphik); + // Corrected Radius + final FieldUnivariateDerivative2 rk = ek.cos().multiply((gnssOrbit.getE()).negate()).add(1) + .multiply(gnssOrbit.getSma()).add(drk); + // Corrected Inclination + final FieldUnivariateDerivative2 ik = tk.multiply(gnssOrbit.getIDot()).add(gnssOrbit.getI0()).add(dik); + final FieldUnivariateDerivative2 cik = ik.cos(); + // Positions in orbital plane + final FieldUnivariateDerivative2 xk = uk.cos().multiply(rk); + final FieldUnivariateDerivative2 yk = uk.sin().multiply(rk); + // Corrected longitude of ascending node + final FieldUnivariateDerivative2 omk = tk.multiply((gnssOrbit.getOmegaDot()).subtract(av)) + .add(gnssOrbit.getOmega0().subtract(gnssOrbit.getTime().multiply(av))); + final FieldUnivariateDerivative2 comk = omk.cos(); + final FieldUnivariateDerivative2 somk = omk.sin(); + // returns the Earth-fixed coordinates + final FieldVector3D> positionwithDerivatives = new FieldVector3D<>( + xk.multiply(comk).subtract(yk.multiply(somk).multiply(cik)), + xk.multiply(somk).add(yk.multiply(comk).multiply(cik)), yk.multiply(ik.sin())); + return new FieldPVCoordinates( + new FieldVector3D(positionwithDerivatives.getX().getValue(), + positionwithDerivatives.getY().getValue(), positionwithDerivatives.getZ().getValue()), + new FieldVector3D(positionwithDerivatives.getX().getFirstDerivative(), + positionwithDerivatives.getY().getFirstDerivative(), + positionwithDerivatives.getZ().getFirstDerivative()), + new FieldVector3D(positionwithDerivatives.getX().getSecondDerivative(), + positionwithDerivatives.getY().getSecondDerivative(), + positionwithDerivatives.getZ().getSecondDerivative())); + } + + /** + * Gets eccentric anomaly from mean anomaly. + *

+ * The algorithm used to solve the Kepler equation has been published in: + * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. Gooding, + * Celestial Mechanics 38 (1986) 307-334 + *

+ *

+ * It has been copied from the OREKIT library (KeplerianOrbit class). + *

+ * + * @param mk the mean anomaly (rad) + * @return the eccentric anomaly (rad) + */ + private FieldUnivariateDerivative2 getEccentricAnomaly(final FieldUnivariateDerivative2 mk) { + + final T zero = mk.getValue().getField().getZero(); + // reduce M to [-PI PI] interval + final FieldUnivariateDerivative2 reducedM = new FieldUnivariateDerivative2( + MathUtils.normalizeAngle(mk.getValue(), zero), mk.getFirstDerivative(), mk.getSecondDerivative()); + + // compute start value according to A. W. Odell and R. H. Gooding S12 starter + FieldUnivariateDerivative2 ek; + if (FastMath.abs(reducedM.getValue().getReal()) < 1.0 / 6.0) { + if (FastMath.abs(reducedM.getValue().getReal()) < Precision.SAFE_MIN) { + // this is an Orekit change to the S12 starter. + // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN + // appearing later in + // the computation. As in this case E and M are almost equal, we initialize ek + // with reducedM + ek = reducedM; + } else { + // this is the standard S12 starter + ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(gnssOrbit.getE())); + } + } else { + if (reducedM.getValue().getReal() < 0) { + final FieldUnivariateDerivative2 w = reducedM.add(FastMath.PI); + ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM) + .multiply(gnssOrbit.getE())); + } else { + final FieldUnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI); + ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM) + .multiply(gnssOrbit.getE())); + } + } + + final T e1 = gnssOrbit.getE().negate().add(1.0); + final boolean noCancellationRisk = (e1.getReal() + + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; + + // perform two iterations, each consisting of one Halley step and one + // Newton-Raphson step + for (int j = 0; j < 2; ++j) { + final FieldUnivariateDerivative2 f; + FieldUnivariateDerivative2 fd; + final FieldUnivariateDerivative2 fdd = ek.sin().multiply(gnssOrbit.getE()); + final FieldUnivariateDerivative2 fddd = ek.cos().multiply(gnssOrbit.getE()); + if (noCancellationRisk) { + f = ek.subtract(fdd).subtract(reducedM); + fd = fddd.subtract(1).negate(); + } else { + f = eMeSinE(ek).subtract(reducedM); + final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); + fd = s.multiply(s).multiply(gnssOrbit.getE().multiply(2.0)).add(e1); + } + final FieldUnivariateDerivative2 dee = f.multiply(fd) + .divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd))); + + // update eccentric anomaly, using expressions that limit underflow problems + final FieldUnivariateDerivative2 w = fd + .add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3)))); + fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); + ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd)); + } + + // expand the result back to original range + ek = ek.add((mk.getValue()).subtract(reducedM.getValue())); + + // Returns the eccentric anomaly + return ek; + } + + /** + * Accurate computation of E - e sin(E). + * + * @param E eccentric anomaly + * @return E - e sin(E) + */ + private FieldUnivariateDerivative2 eMeSinE(final FieldUnivariateDerivative2 E) { + FieldUnivariateDerivative2 x = E.sin().multiply(gnssOrbit.getE().negate().add(1)); + final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); + FieldUnivariateDerivative2 term = E; + FieldUnivariateDerivative2 d = E.getField().getZero(); + // the inequality test below IS intentional and should NOT be replaced by a + // check with a small tolerance + for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); !(x.getValue()).equals(x0.getValue());) { + d = d.add(2); + term = term.multiply(mE2.divide(d.multiply(d.add(1)))); + x0 = x; + x = x.subtract(term); + } + return x; + } + + /** + * Gets true anomaly from eccentric anomaly. + * + * @param ek the eccentric anomaly (rad) + * @return the true anomaly (rad) + */ + private FieldUnivariateDerivative2 getTrueAnomaly(final FieldUnivariateDerivative2 ek) { + final FieldUnivariateDerivative2 svk = ek.sin() + .multiply(FastMath.sqrt((gnssOrbit.getE().multiply(gnssOrbit.getE())).negate().add(1.0))); + final FieldUnivariateDerivative2 cvk = ek.cos().subtract(gnssOrbit.getE()); + return svk.atan2(cvk); + } + + /** {@inheritDoc} */ + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, final T[] parameters) { + // Gets the PVCoordinates in ECEF frame + final FieldPVCoordinates pvaInECEF = propagateInEcef(date); + // Transforms the PVCoordinates to ECI frame + final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); + // Returns the Cartesian orbit + return new FieldCartesianOrbit(pvaInECI, eci, date, mu); + } + + /** + * Get the Earth gravity coefficient used for GNSS propagation. + * + * @return the Earth gravity coefficient. + */ + public T getMU() { + return mu; + } + + /** {@inheritDoc} */ + public Frame getFrame() { + return eci; + } + + /** {@inheritDoc} */ + protected T getMass(final FieldAbsoluteDate date) { + return mass; + } - - + /** {@inheritDoc} */ + public void resetInitialState(final FieldSpacecraftState state) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected void resetIntermediateState(final FieldSpacecraftState state, final boolean forward) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + /** + * Gets the Earth Centered Inertial frame used to propagate the orbit. + * + * @return the ECI frame + */ + public Frame getECI() { + return eci; + } + /** + * Gets the Earth Centered Earth Fixed frame used to propagate GNSS orbits + * according to the Interface Control Document. + * + * @return the ECEF frame + */ + public Frame getECEF() { + return ecef; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java index 1934b7e31..962e587fb 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java @@ -16,16 +16,18 @@ */ package org.orekit.propagation.analytical.gnss; +import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.hipparchus.util.FastMath; import org.orekit.annotation.DefaultDataContext; import org.orekit.data.DataContext; +import org.orekit.gnss.BeidouAlmanac; import org.orekit.gnss.SatelliteSystem; import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; /** - * Class for BeiDou almanac. + * Class for Field BeiDou almanac. * * @see "BeiDou Navigation Satellite System, Signal In Space, Interface Control * Document, Version 2.1, Table 5-12" @@ -36,6 +38,7 @@ import org.orekit.time.FieldAbsoluteDate; */ public class FieldBeidouAlmanac> implements FieldBeidouOrbitalElements { + private final T zero; /** PRN number. */ private final int prn; @@ -139,6 +142,7 @@ public class FieldBeidouAlmanac> implements FieldB public FieldBeidouAlmanac(final int prn, final int week, final T toa, final T sqa, final T ecc, final T inc0, final T dinc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, final int health, final FieldAbsoluteDate date) { + this.zero = toa.getField().getZero(); this.prn = prn; this.week = week; this.toa = toa; @@ -154,6 +158,32 @@ public class FieldBeidouAlmanac> implements FieldB this.health = health; this.date = date; } + + /** + * Constructor + * + * This constructor converts a BeidouAlmanac into a FieldBeidouAlmanac + * + * @param field + * @param almanac a BeidouAlmanac + */ + public FieldBeidouAlmanac(Field field, BeidouAlmanac almanac) { + this.zero = field.getZero(); + this.prn = almanac.getPRN(); + this.week = almanac.getWeek(); + this.toa = zero.add(almanac.getTime()); + this.sma = zero.add(almanac.getSma()); + this.ecc = zero.add(almanac.getE()); + this.inc = zero.add(almanac.getI0()); + this.om0 = zero.add(almanac.getOmega0()); + this.dom = zero.add(almanac.getOmegaDot()); + this.aop = zero.add(almanac.getPa()); + this.anom = zero.add(almanac.getM0()); + this.af0 = zero.add(almanac.getAf0()); + this.af1 = zero.add(almanac.getAf1()); + this.health = almanac.getHealth(); + this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); + } @Override public FieldAbsoluteDate getDate() { diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java index e64f54cc7..d869a2a21 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java @@ -1,5 +1,24 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical.gnss; +import java.util.Collections; +import java.util.List; + import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.orekit.annotation.DefaultDataContext; @@ -9,6 +28,7 @@ import org.orekit.frames.Frame; import org.orekit.frames.Frames; import org.orekit.propagation.Propagator; import org.orekit.utils.IERSConventions; +import org.orekit.utils.ParameterDriver; /** * This class aims at propagating a Beidou orbit from @@ -38,8 +58,31 @@ public class FieldBeidouPropagator> extends FieldA /** * Default constructor. * + *

The Field Beidou orbital elements is the only requested parameter to build a FieldBeidouPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, + final Frames frames)}

+ * * @param field - * @param bdsOrbit + * @param bdsOrbit the Field Beidou orbital elements to be used by the Field Beidou propagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) */ @DefaultDataContext public FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit) { @@ -49,9 +92,32 @@ public class FieldBeidouPropagator> extends FieldA /** * Constructor. * + *

The Field Beidou orbital elements is the only requested parameter to build a FieldBeidouPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)}

+ * * @param field - * @param bdsOrbit - * @param frames + * @param bdsOrbit the Field Beidou orbital elements to be used by the Field Beidou propagator. + * @param frames set of frames to use building the propagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) */ public FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, final Frames frames) { @@ -62,8 +128,22 @@ public class FieldBeidouPropagator> extends FieldA /** * Constructor. * + *

The Field Beidou orbital elements is the only requested parameter to build a FieldBeidouPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * * @param field - * @param bdsOrbit + * @param bdsOrbit the Field Beidou orbital elements to be used by the Field Beidou propagator. * @param attitudeProvider * @param mass * @param eci @@ -78,12 +158,21 @@ public class FieldBeidouPropagator> extends FieldA } /** - * Get the underlying Beidou orbital elements. + * Get the underlying Field Beidou orbital elements. * - * @return the underlying Beidou orbital elements + * @return the underlying Field Beidou orbital elements */ public FieldBeidouOrbitalElements getFieldBeidouOrbitalElements() { return bdsOrbit; } + /** Get the parameters driver for the Field Beidou propagation model. + * @return an empty list. + */ + @Override + protected List getParametersDrivers() { + // Field Beidou propagation model does not have parameter drivers. + return Collections.emptyList(); + } + } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAlmanac.java new file mode 100644 index 000000000..c79cfa866 --- /dev/null +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAlmanac.java @@ -0,0 +1,351 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.propagation.analytical.gnss; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.orekit.annotation.DefaultDataContext; +import org.orekit.data.DataContext; +import org.orekit.gnss.GLONASSAlmanac; +import org.orekit.time.DateComponents; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeComponents; +import org.orekit.time.TimeScale; + +/** + * This class holds a Field GLONASS almanac as read from .agl files. + * + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + * + */ +public class FieldGLONASSAlmanac> implements FieldGLONASSOrbitalElements { + private final T zero; + /** Frequency channel (-7...6). */ + private final int channel; + + /** Health status. */ + private final int health; + + /** Day of Almanac. */ + private final int day; + + /** Month of Almanac. */ + private final int month; + + /** Year of Almanac. */ + private final int year; + + /** Reference time of the almanac. */ + private final T ta; + + /** Greenwich longitude of ascending node of orbit. */ + private final T lambda; + + /** Correction to the mean value of inclination. */ + private final T deltaI; + + /** Argument of perigee. */ + private final T pa; + + /** Eccentricity. */ + private final T ecc; + + /** Correction to the mean value of Draconian period. */ + private final T deltaT; + + /** Rate of change of orbital period. */ + private final T deltaTDot; + + /** Correction from GLONASS to UTC. */ + private final T tGlo2UTC; + + /** Correction to GPS time relative GLONASS. */ + private final T tGPS2Glo; + + /** Correction of time relative to GLONASS system time. */ + private final T tGlo; + + /** GLONASS time scale. */ + private final TimeScale glonass; + + + /** + * Constructor. + * + *

+ * This method uses the {@link DataContext#getDefault() default data context}. + * + * @param channel the frequency channel from -7 to 6) + * @param health the Health status + * @param day the day of Almanac + * @param month the month of Almanac + * @param year the year of Almanac + * @param ta the reference time of the almanac (s) + * @param lambda the Greenwich longitude of ascending node of orbit (rad) + * @param deltaI the correction to the mean value of inclination (rad) + * @param pa the argument of perigee (rad) + * @param ecc the eccentricity + * @param deltaT the correction to the mean value of Draconian period (s) + * @param deltaTDot the rate of change of orbital period + * @param tGlo2UTC the correction from GLONASS to UTC (s) + * @param tGPS2Glo the correction to GPS time relative GLONASS (s) + * @param tGlo the correction of time relative to GLONASS system time (s) + * @see #GLONASSAlmanac(int, int, int, int, int, T, T, T, T, T, T, T, T, T, T, + * TimeScale) + */ + @DefaultDataContext + public FieldGLONASSAlmanac(final int channel, final int health, final int day, final int month, final int year, + final T ta, final T lambda, final T deltaI, final T pa, final T ecc, final T deltaT, final T deltaTDot, + final T tGlo2UTC, final T tGPS2Glo, final T tGlo) { + this(channel, health, day, month, year, ta, lambda, deltaI, pa, ecc, deltaT, deltaTDot, tGlo2UTC, tGPS2Glo, + tGlo, DataContext.getDefault().getTimeScales().getGLONASS()); + } + + /** + * Constructor. + * + * @param channel the frequency channel from -7 to 6) + * @param health the Health status + * @param day the day of Almanac + * @param month the month of Almanac + * @param year the year of Almanac + * @param ta the reference time of the almanac (s) + * @param lambda the Greenwich longitude of ascending node of orbit (rad) + * @param deltaI the correction to the mean value of inclination (rad) + * @param pa the argument of perigee (rad) + * @param ecc the eccentricity + * @param deltaT the correction to the mean value of Draconian period (s) + * @param deltaTDot the rate of change of orbital period + * @param tGlo2UTC the correction from GLONASS to UTC (s) + * @param tGPS2Glo the correction to GPS time relative GLONASS (s) + * @param tGlo the correction of time relative to GLONASS system time (s) + * @param glonass GLONASS time scale. + * @since 10.1 + */ + public FieldGLONASSAlmanac(final int channel, final int health, final int day, final int month, final int year, + final T ta, final T lambda, final T deltaI, final T pa, final T ecc, final T deltaT, final T deltaTDot, + final T tGlo2UTC, final T tGPS2Glo, final T tGlo, final TimeScale glonass) { + this.zero = ta.getField().getZero(); + this.channel = channel; + this.health = health; + this.day = day; + this.month = month; + this.year = year; + this.ta = ta; + this.lambda = lambda; + this.deltaI = deltaI; + this.pa = pa; + this.ecc = ecc; + this.deltaT = deltaT; + this.deltaTDot = deltaTDot; + this.tGlo2UTC = tGlo2UTC; + this.tGPS2Glo = tGPS2Glo; + this.tGlo = tGlo; + this.glonass = glonass; + } + + /** + * Constructor + * + * This constructor converts a GLONASSAlmanac into a FieldGLONASSAlmanac + * + * @param field + * @param almanac + */ + public FieldGLONASSAlmanac(Field field, GLONASSAlmanac almanac) { + this.zero = field.getZero(); + this.channel = almanac.getFrequencyChannel(); + this.health = almanac.getHealth(); + this.day = almanac.getDay(); + this.month = almanac.getMonth(); + this.year = almanac.getYear(); + this.ta = zero.add(almanac.getTime()); + this.lambda = zero.add(almanac.getLambda()); + this.deltaI = zero.add(almanac.getDeltaI()); + this.pa = zero.add(almanac.getPa()); + this.ecc = zero.add(almanac.getE()); + this.deltaT = zero.add(almanac.getDeltaT()); + this.deltaTDot = zero.add(almanac.getDeltaTDot()); + this.tGlo2UTC = zero.add(almanac.getGlo2UTC()); + this.tGPS2Glo = zero.add(almanac.getGPS2Glo()); + this.tGlo = zero.add(almanac.getGloOffset()); + this.glonass = almanac.getGlonass(); + } + + + @Override + public FieldAbsoluteDate getDate() { + final DateComponents date = new DateComponents(year, month, day); + final TimeComponents time = new TimeComponents(ta.getReal()); + return new FieldAbsoluteDate<>(ta.getField(), date, time, glonass); + } + + @Override + public T getTime() { + return ta; + } + + @Override + public T getLambda() { + return lambda; + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getPa() { + return pa; + } + + @Override + public T getDeltaI() { + return deltaI; + } + + @Override + public T getDeltaT() { + return deltaT; + } + + @Override + public T getDeltaTDot() { + return deltaTDot; + } + + /** + * Get the Health status. + * + * @return the Health status + */ + public int getHealth() { + return health; + } + + /** + * Get the frequency channel. + * + * @return the frequency channel + */ + public int getFrequencyChannel() { + return channel; + } + + /** + * Get the correction from GLONASS to UTC. + * + * @return the correction from GLONASS to UTC (s) + */ + public T getGlo2UTC() { + return tGlo2UTC; + } + + /** + * Get the correction to GPS time relative GLONASS. + * + * @return the to GPS time relative GLONASS (s) + */ + public T getGPS2Glo() { + return tGPS2Glo; + } + + /** + * Get the correction of time relative to GLONASS system time. + * + * @return the correction of time relative to GLONASS system time (s) + */ + public T getGloOffset() { + return tGlo; + } + + @Override + public int getNa() { + final FieldGLONASSDate gloDate = new FieldGLONASSDate<>(zero.getField(), getDate(), glonass); + return gloDate.getDayNumber(); + } + + @Override + public int getN4() { + final FieldGLONASSDate gloDate = new FieldGLONASSDate<>(zero.getField(), getDate(), glonass); + return gloDate.getIntervalNumber(); + } + + + @Override + public T getGammaN() { + return zero; + } + + @Override + public T getTN() { + return zero; + } + + @Override + public T getXDot() { + return zero; + } + + @Override + public T getX() { + return zero; + } + + @Override + public T getXDotDot() { + return zero; + } + + @Override + public T getYDot() { + return zero; + } + + @Override + public T getY() { + return zero; + } + + @Override + public T getYDotDot() { + return zero; + } + + @Override + public T getZDot() { + return zero; + } + + @Override + public T getZ() { + return zero; + } + + @Override + public T getZDotDot() { + return zero; + } + + @Override + public int getIOD() { + return 0; + } + +} diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java index 690ebaf79..26042decf 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java @@ -16,6 +16,7 @@ */ package org.orekit.propagation.analytical.gnss; +import java.util.Collections; import java.util.List; import org.hipparchus.Field; @@ -33,14 +34,11 @@ import org.orekit.data.DataContext; import org.orekit.errors.OrekitException; import org.orekit.errors.OrekitMessages; import org.orekit.frames.Frame; -import org.orekit.frames.Frames; import org.orekit.orbits.FieldCartesianOrbit; import org.orekit.orbits.FieldOrbit; import org.orekit.propagation.FieldSpacecraftState; import org.orekit.propagation.Propagator; -import org.orekit.propagation.SpacecraftState; import org.orekit.propagation.analytical.FieldAbstractAnalyticalPropagator; -import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; import org.orekit.time.TimeScale; import org.orekit.utils.FieldPVCoordinates; @@ -48,813 +46,782 @@ import org.orekit.utils.IERSConventions; import org.orekit.utils.ParameterDriver; /** - * This class aims at propagating a GLONASS orbit from {@link GLONASSOrbitalElements}. + * This class aims at propagating a GLONASS orbit from + * {@link GLONASSOrbitalElements}. * - * @see - * GLONASS Interface Control Document + * @see + * GLONASS Interface Control Document * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) */ -public class FieldGLONASSAnalyticalPropagator> extends FieldAbstractAnalyticalPropagator { - +public class FieldGLONASSAnalyticalPropagator> +extends FieldAbstractAnalyticalPropagator { + // Constants - /** Constant 7.0 / 3.0. */ - private static final double SEVEN_THIRD = 7.0 / 3.0; + /** Constant 7.0 / 3.0. */ + private static final double SEVEN_THIRD = 7.0 / 3.0; - /** Constant 7.0 / 6.0. */ - private static final double SEVEN_SIXTH = 7.0 / 6.0; + /** Constant 7.0 / 6.0. */ + private static final double SEVEN_SIXTH = 7.0 / 6.0; - /** Constant 7.0 / 24.0. */ - private static final double SEVEN_24TH = 7.0 / 24.0; + /** Constant 7.0 / 24.0. */ + private static final double SEVEN_24TH = 7.0 / 24.0; - /** Constant 49.0 / 72.0. */ - private static final double FN_72TH = 49.0 / 72.0; + /** Constant 49.0 / 72.0. */ + private static final double FN_72TH = 49.0 / 72.0; - /** Value of the earth's rotation rate in rad/s. */ - private static final double GLONASS_AV = 7.2921150e-5; + /** Value of the earth's rotation rate in rad/s. */ + private static final double GLONASS_AV = 7.2921150e-5; - /** Mean value of inclination for Glonass orbit is equal to 63°. */ - private static final double GLONASS_MEAN_INCLINATION = 64.8; + /** Mean value of inclination for Glonass orbit is equal to 63°. */ + private static final double GLONASS_MEAN_INCLINATION = 64.8; - /** Mean value of Draconian period for Glonass orbit is equal to 40544s : 11 hours 15 minutes 44 seconds. */ - private static final double GLONASS_MEAN_DRACONIAN_PERIOD = 40544; + /** + * Mean value of Draconian period for Glonass orbit is equal to 40544s : 11 + * hours 15 minutes 44 seconds. + */ + private static final double GLONASS_MEAN_DRACONIAN_PERIOD = 40544; - /** Second degree zonal coefficient of normal potential. */ - private static final double GLONASS_J20 = 1.08262575e-3; + /** Second degree zonal coefficient of normal potential. */ + private static final double GLONASS_J20 = 1.08262575e-3; - /** Equatorial radius of Earth (m). */ - private static final double GLONASS_EARTH_EQUATORIAL_RADIUS = 6378136; + /** Equatorial radius of Earth (m). */ + private static final double GLONASS_EARTH_EQUATORIAL_RADIUS = 6378136; - // Data used to solve Kepler's equation - /** First coefficient to compute Kepler equation solver starter. */ - private static final double A; + // Data used to solve Kepler's equation + /** First coefficient to compute Kepler equation solver starter. */ + private static final double A; - /** Second coefficient to compute Kepler equation solver starter. */ - private static final double B; + /** Second coefficient to compute Kepler equation solver starter. */ + private static final double B; - static { - final double k1 = 3 * FastMath.PI + 2; - final double k2 = FastMath.PI - 1; - final double k3 = 6 * FastMath.PI - 1; - A = 3 * k2 * k2 / k1; - B = k3 * k3 / (6 * k1); - } + static { + final double k1 = 3 * FastMath.PI + 2; + final double k2 = FastMath.PI - 1; + final double k3 = 6 * FastMath.PI - 1; + A = 3 * k2 * k2 / k1; + B = k3 * k3 / (6 * k1); + } - // Fields - /** The GLONASS orbital elements used. */ - private final FieldGLONASSOrbitalElements glonassOrbit; + // Fields + /** The GLONASS orbital elements used. */ + private final FieldGLONASSOrbitalElements glonassOrbit; + + /** The spacecraft mass (kg). */ + private final T mass; + + /** The ECI frame used for GLONASS propagation. */ + private final Frame eci; + + /** The ECEF frame used for GLONASS propagation. */ + private final Frame ecef; + + /** Data context for propagation. */ + private final DataContext dataContext; + + /** + * Default constructor. + * + *

The Field GLONASS orbital elements is the only requested parameter to build a FieldGLONASSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The data context is by default to the + * {@link DataContext#getDefault() default data context}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, final DataContext dataContext)}

+ * + * @param field + * @param glonassOrbElt the Field GLONASS orbital elements to be used by the Field GLONASS propagator. + */ + @DefaultDataContext + public FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt) { + this(field, glonassOrbElt, DataContext.getDefault()); + } - /** The spacecraft mass (kg). */ - private final double mass; + /** + * Constructor. + * + *

The Field GLONASS orbital elements is the only requested parameter to build a FieldGLONASSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The data context is by default to the + * {@link DataContext#getDefault() default data context}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, + final DataContext dataContext, final AttitudeProvider attitudeProvider, T mass, final Frame eci, final Frame ecef)}

+ * + * @param field + * @param glonassOrbElt the Field GLONASS orbital elements to be used by the Field GLONASS propagator. + * @param dataContext the data + */ + public FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, final DataContext dataContext) { + this(field, glonassOrbElt, dataContext, Propagator.getDefaultLaw(dataContext.getFrames()), field.getZero().add(DEFAULT_MASS),dataContext.getFrames().getEME2000(), + dataContext.getFrames().getITRF(IERSConventions.IERS_2010, true)); + } - /** The ECI frame used for GLONASS propagation. */ - private final Frame eci; + /** + * Constructor. + * + * *

The Field GLONASS orbital elements is the only requested parameter to build a FieldGLONASSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The data context is by default to the + * {@link DataContext#getDefault() default data context}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + * @param field + * @param glonassOrbElt + * @param dataContext + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, + final DataContext dataContext, final AttitudeProvider attitudeProvider, T mass, final Frame eci, final Frame ecef) { + + super(field, attitudeProvider); + this.dataContext = dataContext; + // Stores the GLONASS orbital elements + this.glonassOrbit = glonassOrbElt; + // Sets the start date as the date of the orbital elements + setStartDate(glonassOrbit.getDate()); + // Sets the mass + this.mass = mass; + // Sets the Earth Centered Inertial frame + this.eci = eci; + // Sets the Earth Centered Earth Fixed frame + this.ecef = ecef; + } - /** The ECEF frame used for GLONASS propagation. */ - private final Frame ecef; + /** + * Gets the FieldPVCoordinates of the GLONASS SV in {@link #getECEF() ECEF frame}. + * + *

+ * The algorithm is defined at Appendix M.1 from GLONASS Interface Control + * Document, with automatic differentiation added to compute velocity and + * acceleration. + *

+ * + * @param date the computation date + * @return the GLONASS SV FieldPVCoordinates in {@link #getECEF() ECEF frame} + */ + public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { + + // Interval of prediction dTpr + final FieldUnivariateDerivative2 dTpr = getdTpr(date); + + // Zero + final FieldUnivariateDerivative2 zero = dTpr.getField().getZero(); + + // The number of whole orbits "w" on a prediction interval + final FieldUnivariateDerivative2 w = FastMath.floor(dTpr.divide(glonassOrbit.getDeltaT().add(GLONASS_MEAN_DRACONIAN_PERIOD))); + + // Current inclination + final FieldUnivariateDerivative2 i = zero.add(zero.add(GLONASS_MEAN_INCLINATION / 180 * GLONASSOrbitalElements.GLONASS_PI).add(glonassOrbit.getDeltaI())); + + // Eccentricity + final FieldUnivariateDerivative2 e = zero.add(glonassOrbit.getE()); + + // Mean draconique period in orbite w+1 and mean motion + final FieldUnivariateDerivative2 tDR = w.multiply(2.0).add(1.0).multiply(glonassOrbit.getDeltaTDot()) + .add(glonassOrbit.getDeltaT()) + .add(GLONASS_MEAN_DRACONIAN_PERIOD); + final FieldUnivariateDerivative2 n = tDR.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI).reciprocal(); + + // Semi-major axis : computed by successive approximation + final FieldUnivariateDerivative2 sma = computeSma(tDR, i, e); + + // (ae / p)^2 term + final FieldUnivariateDerivative2 p = sma.multiply(e.multiply(e).negate().add(1.0)); + final FieldUnivariateDerivative2 aeop = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); + final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); + + // Current longitude of the ascending node + final FieldUnivariateDerivative2 lambda = computeLambda(dTpr, n, aeop2, i); + + // Current argument of perigee + final FieldUnivariateDerivative2 pa = computePA(dTpr, n, aeop2, i); + + // Mean longitude at the instant the spacecraft passes the current ascending + // node + final FieldUnivariateDerivative2 tanPAo2 = FastMath.tan(pa.divide(2.0)); + final FieldUnivariateDerivative2 coef = tanPAo2 + .multiply(FastMath.sqrt(e.negate().add(1.0).divide(e.add(1.0)))); + final FieldUnivariateDerivative2 e0 = FastMath.atan(coef).multiply(2.0).negate(); + final FieldUnivariateDerivative2 m1 = pa.add(e0).subtract(FastMath.sin(e0).multiply(e)); + + // Current mean longitude + final FieldUnivariateDerivative2 correction = dTpr + .subtract(w.multiply(zero.add(GLONASS_MEAN_DRACONIAN_PERIOD).add(glonassOrbit.getDeltaT()))) + .subtract(w.multiply(w).multiply(glonassOrbit.getDeltaTDot())); + final FieldUnivariateDerivative2 m = m1.add(n.multiply(correction)); + + // Take into consideration the periodic perturbations + final FieldSinCos> scPa = FastMath.sinCos(pa); + final FieldUnivariateDerivative2 h = e.multiply(scPa.sin()); + final FieldUnivariateDerivative2 l = e.multiply(scPa.cos()); + // δa1 + final FieldUnivariateDerivative2[] d1 = getParameterDifferentials(sma, i, h, l, m1); + // δa2 + final FieldUnivariateDerivative2[] d2 = getParameterDifferentials(sma, i, h, l, m); + // Apply corrections + final FieldUnivariateDerivative2 smaCorr = sma.add(d2[0]).subtract(d1[0]); + final FieldUnivariateDerivative2 hCorr = h.add(d2[1]).subtract(d1[1]); + final FieldUnivariateDerivative2 lCorr = l.add(d2[2]).subtract(d1[2]); + final FieldUnivariateDerivative2 lambdaCorr = lambda.add(d2[3]).subtract(d1[3]); + final FieldUnivariateDerivative2 iCorr = i.add(d2[4]).subtract(d1[4]); + final FieldUnivariateDerivative2 mCorr = m.add(d2[5]).subtract(d1[5]); + final FieldUnivariateDerivative2 eCorr = FastMath.sqrt(hCorr.multiply(hCorr).add(lCorr.multiply(lCorr))); + final FieldUnivariateDerivative2 paCorr; + if (eCorr.getValue().getReal() == 0.) { + paCorr = zero; + } else { + if (lCorr.getValue() == eCorr.getValue()) { + paCorr = zero.add(0.5 * FieldGLONASSOrbitalElements.GLONASS_PI); + } else if (lCorr.getValue().getReal() == -eCorr.getValue().getReal()) { + paCorr = zero.add(-0.5 * FieldGLONASSOrbitalElements.GLONASS_PI); + } else { + paCorr = FastMath.atan2(hCorr, lCorr); + } + } + + // Eccentric Anomaly + final FieldUnivariateDerivative2 mk = mCorr.subtract(paCorr); + final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk, eCorr); + + // True Anomaly + final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek, eCorr); + + // Argument of Latitude + final FieldUnivariateDerivative2 phik = vk.add(paCorr); + + // Corrected Radius + final FieldUnivariateDerivative2 pCorr = smaCorr.multiply(eCorr.multiply(eCorr).negate().add(1.0)); + final FieldUnivariateDerivative2 rk = pCorr.divide(eCorr.multiply(FastMath.cos(vk)).add(1.0)); + + // Positions in orbital plane + final FieldSinCos> scPhik = FastMath.sinCos(phik); + final FieldUnivariateDerivative2 xk = scPhik.cos().multiply(rk); + final FieldUnivariateDerivative2 yk = scPhik.sin().multiply(rk); + + // Coordinates of position + final FieldSinCos> scL = FastMath.sinCos(lambdaCorr); + final FieldSinCos> scI = FastMath.sinCos(iCorr); + final FieldVector3D> positionwithDerivatives = + new FieldVector3D<>(xk.multiply(scL.cos()).subtract(yk.multiply(scL.sin()).multiply(scI.cos())), + xk.multiply(scL.sin()).add(yk.multiply(scL.cos()).multiply(scI.cos())), + yk.multiply(scI.sin())); + + return new FieldPVCoordinates( + new FieldVector3D(positionwithDerivatives.getX().getValue(), + positionwithDerivatives.getY().getValue(), + positionwithDerivatives.getZ().getValue()), + new FieldVector3D(positionwithDerivatives.getX().getFirstDerivative(), + positionwithDerivatives.getY().getFirstDerivative(), + positionwithDerivatives.getZ().getFirstDerivative()), + new FieldVector3D(positionwithDerivatives.getX().getSecondDerivative(), + positionwithDerivatives.getY().getSecondDerivative(), + positionwithDerivatives.getZ().getSecondDerivative())); + } - /** Data context for propagation. */ - private final DataContext dataContext; - - private final Field field; - /** - * This nested class aims at building a GLONASSPropagator. - *

It implements the classical FieldBuilder pattern.

- * - */ - public class FieldBuilder { - - // Required parameter - /** The GLONASS orbital elements. */ - private final FieldGLONASSOrbitalElements orbit; - - // Optional parameters - /** The attitude provider. */ - private AttitudeProvider attitudeProvider; - /** The mass. */ - private double mass = DEFAULT_MASS; - /** The ECI frame. */ - private Frame eci = null; - /** The ECEF frame. */ - private Frame ecef = null; - /** Data context. */ - private DataContext dataContext; - - /** Initializes the FieldBuilder. - *

The GLONASS orbital elements is the only requested parameter to build a GLONASSPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The data context is by default to the - * {@link DataContext#getDefault() default data context}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldBuilder(final GLONASSOrbitalElements gpsOrbElt, final DataContext dataContext)}

- * - * @param glonassOrbElt the GLONASS orbital elements to be used by the GLONASS propagator. - * @see #attitudeProvider(AttitudeProvider provider) - * @see #mass(double mass) - * @see #eci(Frame inertial) - * @see #ecef(Frame bodyFixed) - */ - @DefaultDataContext - public FieldBuilder(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt) { - this(field, glonassOrbElt, DataContext.getDefault()); - } - - /** Initializes the FieldBuilder. - *

The GLONASS orbital elements is the only requested parameter to build a GLONASSPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#getDefaultLaw(Frames)}.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link Frames#getEME2000() EME2000 frame}.
- * The ECEF frame is set by default to the - * {@link Frames#getITRF(IERSConventions, boolean) CIO/2010-based ITRF simple - * EOP}. - *

- * - * @param glonassOrbElt the GLONASS orbital elements to be used by the GLONASS propagator. - * @param dataContext the data context to use for frames and time scales. - * @see #attitudeProvider(AttitudeProvider provider) - * @see #mass(double mass) - * @see #eci(Frame inertial) - * @see #ecef(Frame bodyFixed) - * @since 10.1 - */ - public FieldBuilder(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, - final DataContext dataContext) { - this.orbit = glonassOrbElt; - this.dataContext = dataContext; - final Frames frames = dataContext.getFrames(); - this.eci = frames.getEME2000(); - this.ecef = frames.getITRF(IERSConventions.IERS_2010, true); - attitudeProvider = Propagator.getDefaultLaw(frames); - } - - /** Sets the attitude provider. - * - * @param userProvider the attitude provider - * @return the updated FieldBuilder - */ - public FieldBuilder attitudeProvider(final AttitudeProvider userProvider) { - this.attitudeProvider = userProvider; - return this; - } - - /** Sets the mass. - * - * @param userMass the mass (in kg) - * @return the updated FieldBuilder - */ - public FieldBuilder mass(final double userMass) { - this.mass = userMass; - return this; - } - - /** Sets the Earth Centered Inertial frame used for propagation. - * - * @param inertial the ECI frame - * @return the updated FieldBuilder - */ - public FieldBuilder eci(final Frame inertial) { - this.eci = inertial; - return this; - } - - /** Sets the Earth Centered Earth Fixed frame assimilated to the WGS84 ECEF. - * - * @param bodyFixed the ECEF frame - * @return the updated FieldBuilder - */ - public FieldBuilder ecef(final Frame bodyFixed) { - this.ecef = bodyFixed; - return this; - } - - /** - * Sets the data context used by the propagator. Does not update the ECI or ECEF - * frames which must be done separately using {@link #eci(Frame)} and {@link - * #ecef(Frame)}. - * - * @param context used for propagation. - * @return the updated FieldBuilder. - */ - public FieldBuilder dataContext(final DataContext context) { - this.dataContext = context; - return this; - } - - /** Finalizes the build. - * - * @return the built GLONASSPropagator - */ - public FieldGLONASSAnalyticalPropagator build() { - return new FieldGLONASSAnalyticalPropagator<>(field, this); - } - - } - - /** - * Private constructor. - * @param FieldBuilder the FieldBuilder - */ - private FieldGLONASSAnalyticalPropagator(final Field field, final FieldBuilder FieldBuilder) { - super(field, FieldBuilder.attitudeProvider); - this.dataContext = FieldBuilder.dataContext; - // Stores the GLONASS orbital elements - this.glonassOrbit = FieldBuilder.orbit; - // Sets the start date as the date of the orbital elements - setStartDate(glonassOrbit.getDate()); - // Sets the mass - this.mass = FieldBuilder.mass; - // Sets the Earth Centered Inertial frame - this.eci = FieldBuilder.eci; - // Sets the Earth Centered Earth Fixed frame - this.ecef = FieldBuilder.ecef; - this.field = null; - } - - /** - * Gets the PVCoordinates of the GLONASS SV in {@link #getECEF() ECEF frame}. - * - *

The algorithm is defined at Appendix M.1 from GLONASS Interface Control Document, - * with automatic differentiation added to compute velocity and - * acceleration.

- * - * @param date the computation date - * @return the GLONASS SV PVCoordinates in {@link #getECEF() ECEF frame} - */ - public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { - - // Interval of prediction dTpr - final FieldUnivariateDerivative2 dTpr = getdTpr(date); - - // Zero - final FieldUnivariateDerivative2 zero = dTpr.getField().getZero(); - - // The number of whole orbits "w" on a prediction interval - final FieldUnivariateDerivative2 w = FastMath.floor(dTpr.divide(glonassOrbit.getDeltaT().add(GLONASS_MEAN_DRACONIAN_PERIOD))); - - // Current inclination - final FieldUnivariateDerivative2 i = zero.add(zero.add(GLONASS_MEAN_INCLINATION / 180 * GLONASSOrbitalElements.GLONASS_PI).add( glonassOrbit.getDeltaI())); - - // Eccentricity - final FieldUnivariateDerivative2 e = zero.add(glonassOrbit.getE()); - - // Mean draconique period in orbite w+1 and mean motion - final FieldUnivariateDerivative2 tDR = w.multiply(2.0).add(1.0).multiply(glonassOrbit.getDeltaTDot()). - add(glonassOrbit.getDeltaT()). - add(GLONASS_MEAN_DRACONIAN_PERIOD); - final FieldUnivariateDerivative2 n = tDR.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI).reciprocal(); - - // Semi-major axis : computed by successive approximation - final FieldUnivariateDerivative2 sma = computeSma(tDR, i, e); - - // (ae / p)^2 term - final FieldUnivariateDerivative2 p = sma.multiply(e.multiply(e).negate().add(1.0)); - final FieldUnivariateDerivative2 aeop = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); - final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); - - // Current longitude of the ascending node - final FieldUnivariateDerivative2 lambda = computeLambda(dTpr, n, aeop2, i); - - // Current argument of perigee - final FieldUnivariateDerivative2 pa = computePA(dTpr, n, aeop2, i); - - // Mean longitude at the instant the spacecraft passes the current ascending node - final FieldUnivariateDerivative2 tanPAo2 = FastMath.tan(pa.divide(2.0)); - final FieldUnivariateDerivative2 coef = tanPAo2.multiply(FastMath.sqrt(e.negate().add(1.0).divide(e.add(1.0)))); - final FieldUnivariateDerivative2 e0 = FastMath.atan(coef).multiply(2.0).negate(); - final FieldUnivariateDerivative2 m1 = pa.add(e0).subtract(FastMath.sin(e0).multiply(e)); - - // Current mean longitude - final FieldUnivariateDerivative2 correction = dTpr. - subtract(w.multiply(zero.add(GLONASS_MEAN_DRACONIAN_PERIOD).add(glonassOrbit.getDeltaT()) )). - subtract(w.multiply(w).multiply(glonassOrbit.getDeltaTDot())); - final FieldUnivariateDerivative2 m = m1.add(n.multiply(correction)); - - // Take into consideration the periodic perturbations - final FieldSinCos> scPa = FastMath.sinCos(pa); - final FieldUnivariateDerivative2 h = e.multiply(scPa.sin()); - final FieldUnivariateDerivative2 l = e.multiply(scPa.cos()); - // δa1 - final FieldUnivariateDerivative2[] d1 = getParameterDifferentials(sma, i, h, l, m1); - // δa2 - final FieldUnivariateDerivative2[] d2 = getParameterDifferentials(sma, i, h, l, m); - // Apply corrections - final FieldUnivariateDerivative2 smaCorr = sma.add(d2[0]).subtract(d1[0]); - final FieldUnivariateDerivative2 hCorr = h.add(d2[1]).subtract(d1[1]); - final FieldUnivariateDerivative2 lCorr = l.add(d2[2]).subtract(d1[2]); - final FieldUnivariateDerivative2 lambdaCorr = lambda.add(d2[3]).subtract(d1[3]); - final FieldUnivariateDerivative2 iCorr = i.add(d2[4]).subtract(d1[4]); - final FieldUnivariateDerivative2 mCorr = m.add(d2[5]).subtract(d1[5]); - final FieldUnivariateDerivative2 eCorr = FastMath.sqrt(hCorr.multiply(hCorr).add(lCorr.multiply(lCorr))); - final FieldUnivariateDerivative2 paCorr; - if (eCorr.getValue().getReal() == 0.) { - paCorr = zero; - } else { - if (lCorr.getValue() == eCorr.getValue()) { - paCorr = zero.add(0.5 * GLONASSOrbitalElements.GLONASS_PI); - } else if (lCorr.getValue().getReal() == -eCorr.getValue().getReal()) { - paCorr = zero.add(-0.5 * GLONASSOrbitalElements.GLONASS_PI); - } else { - paCorr = FastMath.atan2(hCorr, lCorr); - } - } - - // Eccentric Anomaly - final FieldUnivariateDerivative2 mk = mCorr.subtract(paCorr); - final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk, eCorr); - - // True Anomaly - final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek, eCorr); - - // Argument of Latitude - final FieldUnivariateDerivative2 phik = vk.add(paCorr); - - // Corrected Radius - final FieldUnivariateDerivative2 pCorr = smaCorr.multiply(eCorr.multiply(eCorr).negate().add(1.0)); - final FieldUnivariateDerivative2 rk = pCorr.divide(eCorr.multiply(FastMath.cos(vk)).add(1.0)); - - // Positions in orbital plane - final FieldSinCos> scPhik = FastMath.sinCos(phik); - final FieldUnivariateDerivative2 xk = scPhik.cos().multiply(rk); - final FieldUnivariateDerivative2 yk = scPhik.sin().multiply(rk); - - // Coordinates of position - final FieldSinCos> scL = FastMath.sinCos(lambdaCorr); - final FieldSinCos> scI = FastMath.sinCos(iCorr); - final FieldVector3D> positionwithDerivatives = - new FieldVector3D<>(xk.multiply(scL.cos()).subtract(yk.multiply(scL.sin()).multiply(scI.cos())), - xk.multiply(scL.sin()).add(yk.multiply(scL.cos()).multiply(scI.cos())), - yk.multiply(scI.sin())); - - return new FieldPVCoordinates(new FieldVector3D(positionwithDerivatives.getX().getValue(), - positionwithDerivatives.getY().getValue(), - positionwithDerivatives.getZ().getValue()), - new FieldVector3D(positionwithDerivatives.getX().getFirstDerivative(), - positionwithDerivatives.getY().getFirstDerivative(), - positionwithDerivatives.getZ().getFirstDerivative()), - new FieldVector3D(positionwithDerivatives.getX().getSecondDerivative(), - positionwithDerivatives.getY().getSecondDerivative(), - positionwithDerivatives.getZ().getSecondDerivative())); - } - - /** - * Gets eccentric anomaly from mean anomaly. - *

The algorithm used to solve the Kepler equation has been published in: - * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. Gooding, - * Celestial Mechanics 38 (1986) 307-334

- *

It has been copied from the OREKIT library (KeplerianOrbit class).

- * - * @param mk the mean anomaly (rad) - * @param e the eccentricity - * @return the eccentric anomaly (rad) - */ - private FieldUnivariateDerivative2 getEccentricAnomaly(final FieldUnivariateDerivative2 mk, final FieldUnivariateDerivative2 e) { - - // reduce M to [-PI PI] interval - final T zero = mk.getValue().getField().getZero(); - final FieldUnivariateDerivative2 reducedM = new FieldUnivariateDerivative2(MathUtils.normalizeAngle(mk.getValue(), zero), - mk.getFirstDerivative(), - mk.getSecondDerivative()); - - // compute start value according to A. W. Odell and R. H. Gooding S12 starter - FieldUnivariateDerivative2 ek; - if (FastMath.abs(reducedM.getValue()).getReal() < 1.0 / 6.0) { - if (FastMath.abs(reducedM.getValue()).getReal() < Precision.SAFE_MIN) { - // this is an Orekit change to the S12 starter. - // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN appearing later in - // the computation. As in this case E and M are almost equal, we initialize ek with reducedM - ek = reducedM; - } else { - // this is the standard S12 starter - ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(e)); - } - } else { - if (reducedM.getValue().getReal() < 0) { - final FieldUnivariateDerivative2 w = reducedM.add(FastMath.PI); - ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(e)); - } else { - final FieldUnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI); - ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(e)); - } - } - - final FieldUnivariateDerivative2 e1 = e.negate().add(1.0); - final boolean noCancellationRisk = (e1.getReal() + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; - - // perform two iterations, each consisting of one Halley step and one Newton-Raphson step - for (int j = 0; j < 2; ++j) { - final FieldUnivariateDerivative2 f; - FieldUnivariateDerivative2 fd; - final FieldUnivariateDerivative2 fdd = ek.sin().multiply(e); - final FieldUnivariateDerivative2 fddd = ek.cos().multiply(e); - if (noCancellationRisk) { - f = ek.subtract(fdd).subtract(reducedM); - fd = fddd.subtract(1).negate(); - } else { - f = eMeSinE(ek, e).subtract(reducedM); - final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); - fd = s.multiply(s).multiply(e.multiply(2.0)).add(e1); - } - final FieldUnivariateDerivative2 dee = f.multiply(fd).divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd))); - - // update eccentric anomaly, using expressions that limit underflow problems - final FieldUnivariateDerivative2 w = fd.add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3)))); - fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); - ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd)); - } - - // expand the result back to original range - ek = ek.add(mk.getValue().subtract(reducedM.getValue()) ); - - // Returns the eccentric anomaly - return ek; - } - - /** - * Accurate computation of E - e sin(E). - * - * @param E eccentric anomaly - * @param ecc the eccentricity - * @return E - e sin(E) - */ - private FieldUnivariateDerivative2 eMeSinE(final FieldUnivariateDerivative2 E, final FieldUnivariateDerivative2 ecc) { - FieldUnivariateDerivative2 x = E.sin().multiply(ecc.negate().add(1.0)); - final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); - FieldUnivariateDerivative2 term = E; - FieldUnivariateDerivative2 d = E.getField().getZero(); - // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance - for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); !(x.getValue()).equals(x0.getValue());) { - d = d.add(2); - term = term.multiply(mE2.divide(d.multiply(d.add(1)))); - x0 = x; - x = x.subtract(term); - } - return x; - } - - /** Gets true anomaly from eccentric anomaly. - * - * @param ek the eccentric anomaly (rad) - * @param ecc the eccentricity - * @return the true anomaly (rad) - */ - private FieldUnivariateDerivative2 getTrueAnomaly(final FieldUnivariateDerivative2 ek, final FieldUnivariateDerivative2 ecc) { - final FieldUnivariateDerivative2 svk = ek.sin().multiply(FastMath.sqrt( ecc.multiply(ecc).negate().add(1.0))); - final FieldUnivariateDerivative2 cvk = ek.cos().subtract(ecc); - return svk.atan2(cvk); - } - - /** - * Get the interval of prediction. - * - * @param date the considered date - * @return the duration from GLONASS orbit Reference epoch (s) - */ - private FieldUnivariateDerivative2 getdTpr(final FieldAbsoluteDate date) { - final TimeScale glonass = dataContext.getTimeScales().getGLONASS(); - final FieldGLONASSDate tEnd = new FieldGLONASSDate(date, glonass); - final FieldGLONASSDate tSta = new FieldGLONASSDate(glonassOrbit.getDate(), glonass); - final int n = tEnd.getDayNumber(); - final int na = tSta.getDayNumber(); - final int deltaN; - if (na == 27) { - deltaN = n - na - FastMath.round((float) (n - na) / 1460) * 1460; - } else { - deltaN = n - na - FastMath.round((float) (n - na) / 1461) * 1461; - } - - final T zero = date.getField().getZero(); - final FieldUnivariateDerivative2 ti = new FieldUnivariateDerivative2(zero.add(tEnd.getSecInDay()), zero.add(1.0), zero.add(1.0)); - - return ti.subtract(glonassOrbit.getTime()).add(86400 * deltaN); - } - - /** - * Computes the semi-major axis of orbit using technique of successive approximations. - * @param tDR mean draconique period (s) - * @param i current inclination (rad) - * @param e eccentricity - * @return the semi-major axis (m). - */ - private FieldUnivariateDerivative2 computeSma(final FieldUnivariateDerivative2 tDR, - final FieldUnivariateDerivative2 i, - final FieldUnivariateDerivative2 e) { - - // Zero - final FieldUnivariateDerivative2 zero = tDR.getField().getZero(); - - // If one of the input parameter is equal to Double.NaN, an infinite loop can occur. - // In that case, we do not compute the value of the semi major axis. - // We decided to return a Double.NaN value instead. - if (Double.isNaN(tDR.getValue().getReal()) || Double.isNaN(i.getValue().getReal()) || Double.isNaN(e.getValue().getReal())) { - return zero.add(Double.NaN); - } - - // Common parameters - final FieldUnivariateDerivative2 sinI = FastMath.sin(i); - final FieldUnivariateDerivative2 sin2I = sinI.multiply(sinI); - final FieldUnivariateDerivative2 ome2 = e.multiply(e).negate().add(1.0); - final FieldUnivariateDerivative2 ome2Pow3o2 = FastMath.sqrt(ome2).multiply(ome2); - final FieldUnivariateDerivative2 pa = zero.add(glonassOrbit.getPa()); - final FieldUnivariateDerivative2 cosPA = FastMath.cos(pa); - final FieldUnivariateDerivative2 opecosPA = e.multiply(cosPA).add(1.0); - final FieldUnivariateDerivative2 opecosPAPow2 = opecosPA.multiply(opecosPA); - final FieldUnivariateDerivative2 opecosPAPow3 = opecosPAPow2.multiply(opecosPA); - - // Initial approximation - FieldUnivariateDerivative2 tOCK = tDR; - - // Successive approximations - // The process of approximation ends when fulfilling the following condition: |a(n+1) - a(n)| < 1cm - FieldUnivariateDerivative2 an = zero; - FieldUnivariateDerivative2 anp1 = zero; - boolean isLastStep = false; - while (!isLastStep) { - - // a(n+1) computation - final FieldUnivariateDerivative2 tOCKo2p = tOCK.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI); - final FieldUnivariateDerivative2 tOCKo2pPow2 = tOCKo2p.multiply(tOCKo2p); - anp1 = FastMath.cbrt(tOCKo2pPow2.multiply(GLONASSOrbitalElements.GLONASS_MU)); - - // p(n+1) computation - final FieldUnivariateDerivative2 p = anp1.multiply(ome2); - - // Tock(n+1) computation - final FieldUnivariateDerivative2 aeop = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); - final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); - final FieldUnivariateDerivative2 term1 = aeop2.multiply(GLONASS_J20).multiply(1.5); - final FieldUnivariateDerivative2 term2 = sin2I.multiply(2.5).negate().add(2.0); - final FieldUnivariateDerivative2 term3 = ome2Pow3o2.divide(opecosPAPow2); - final FieldUnivariateDerivative2 term4 = opecosPAPow3.divide(ome2); - tOCK = tDR.divide(term1.multiply(term2.multiply(term3).add(term4)).negate().add(1.0)); - - // Check convergence - if (FastMath.abs(anp1.subtract(an).getReal()) <= 0.01) { - isLastStep = true; - } - - an = anp1; - } - - return an; - - } - - /** - * Computes the current longitude of the ascending node. - * @param dTpr interval of prediction (s) - * @param n mean motion (rad/s) - * @param aeop2 square of the ratio between the radius of the ellipsoid and p, with p = sma * (1 - ecc²) - * @param i inclination (rad) - * @return the current longitude of the ascending node (rad) - */ - private FieldUnivariateDerivative2 computeLambda(final FieldUnivariateDerivative2 dTpr, - final FieldUnivariateDerivative2 n, - final FieldUnivariateDerivative2 aeop2, - final FieldUnivariateDerivative2 i) { - final FieldUnivariateDerivative2 cosI = FastMath.cos(i); - final FieldUnivariateDerivative2 precession = aeop2.multiply(n).multiply(cosI).multiply(1.5 * GLONASS_J20); - return dTpr.multiply(precession.add(GLONASS_AV)).negate().add(glonassOrbit.getLambda()); - } - - /** - * Computes the current argument of perigee. - * @param dTpr interval of prediction (s) - * @param n mean motion (rad/s) - * @param aeop2 square of the ratio between the radius of the ellipsoid and p, with p = sma * (1 - ecc²) - * @param i inclination (rad) - * @return the current argument of perigee (rad) - */ - private FieldUnivariateDerivative2 computePA(final FieldUnivariateDerivative2 dTpr, - final FieldUnivariateDerivative2 n, - final FieldUnivariateDerivative2 aeop2, - final FieldUnivariateDerivative2 i) { - final FieldUnivariateDerivative2 cosI = FastMath.cos(i); - final FieldUnivariateDerivative2 cos2I = cosI.multiply(cosI); - final FieldUnivariateDerivative2 precession = aeop2.multiply(n).multiply(cos2I.multiply(5.0).negate().add(1.0)).multiply(0.75 * GLONASS_J20); - return dTpr.multiply(precession).negate().add(glonassOrbit.getPa()); - } - - /** - * Computes the differentials δai. - *

- * The value of i depends of the type of longitude (i = 2 for the current mean longitude; - * i = 1 for the mean longitude at the instant the spacecraft passes the current ascending node) - *

- * @param a semi-major axis (m) - * @param i inclination (rad) - * @param h x component of the eccentricity (rad) - * @param l y component of the eccentricity (rad) - * @param m longitude (current or at the ascending node instant) - * @return the differentials of the orbital parameters - */ - private FieldUnivariateDerivative2[] getParameterDifferentials(final FieldUnivariateDerivative2 a, final FieldUnivariateDerivative2 i, - final FieldUnivariateDerivative2 h, final FieldUnivariateDerivative2 l, - final FieldUnivariateDerivative2 m) { - - // B constant - final FieldUnivariateDerivative2 aeoa = a.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); - final FieldUnivariateDerivative2 aeoa2 = aeoa.multiply(aeoa); - final FieldUnivariateDerivative2 b = aeoa2.multiply(1.5 * GLONASS_J20); - - // Commons Parameters - final FieldSinCos> scI = FastMath.sinCos(i); - final FieldSinCos> scLk = FastMath.sinCos(m); - final FieldSinCos> sc2Lk = FieldSinCos.sum(scLk, scLk); - final FieldSinCos> sc3Lk = FieldSinCos.sum(scLk, sc2Lk); - final FieldSinCos> sc4Lk = FieldSinCos.sum(sc2Lk, sc2Lk); - final FieldUnivariateDerivative2 cosI = scI.cos(); - final FieldUnivariateDerivative2 sinI = scI.sin(); - final FieldUnivariateDerivative2 cosI2 = cosI.multiply(cosI); - final FieldUnivariateDerivative2 sinI2 = sinI.multiply(sinI); - final FieldUnivariateDerivative2 cosLk = scLk.cos(); - final FieldUnivariateDerivative2 sinLk = scLk.sin(); - final FieldUnivariateDerivative2 cos2Lk = sc2Lk.cos(); - final FieldUnivariateDerivative2 sin2Lk = sc2Lk.sin(); - final FieldUnivariateDerivative2 cos3Lk = sc3Lk.cos(); - final FieldUnivariateDerivative2 sin3Lk = sc3Lk.sin(); - final FieldUnivariateDerivative2 cos4Lk = sc4Lk.cos(); - final FieldUnivariateDerivative2 sin4Lk = sc4Lk.sin(); - - // h*cos(nLk), l*cos(nLk), h*sin(nLk) and l*sin(nLk) - // n = 1 - final FieldUnivariateDerivative2 hCosLk = h.multiply(cosLk); - final FieldUnivariateDerivative2 hSinLk = h.multiply(sinLk); - final FieldUnivariateDerivative2 lCosLk = l.multiply(cosLk); - final FieldUnivariateDerivative2 lSinLk = l.multiply(sinLk); - // n = 2 - final FieldUnivariateDerivative2 hCos2Lk = h.multiply(cos2Lk); - final FieldUnivariateDerivative2 hSin2Lk = h.multiply(sin2Lk); - final FieldUnivariateDerivative2 lCos2Lk = l.multiply(cos2Lk); - final FieldUnivariateDerivative2 lSin2Lk = l.multiply(sin2Lk); - // n = 3 - final FieldUnivariateDerivative2 hCos3Lk = h.multiply(cos3Lk); - final FieldUnivariateDerivative2 hSin3Lk = h.multiply(sin3Lk); - final FieldUnivariateDerivative2 lCos3Lk = l.multiply(cos3Lk); - final FieldUnivariateDerivative2 lSin3Lk = l.multiply(sin3Lk); - // n = 4 - final FieldUnivariateDerivative2 hCos4Lk = h.multiply(cos4Lk); - final FieldUnivariateDerivative2 hSin4Lk = h.multiply(sin4Lk); - final FieldUnivariateDerivative2 lCos4Lk = l.multiply(cos4Lk); - final FieldUnivariateDerivative2 lSin4Lk = l.multiply(sin4Lk); - - // 1 - (3 / 2)*sin²i - final FieldUnivariateDerivative2 om3o2xSinI2 = sinI2.multiply(1.5).negate().add(1.0); - - // Compute Differentials - // δa - final FieldUnivariateDerivative2 dakT1 = b.multiply(2.0).multiply(om3o2xSinI2).multiply(lCosLk.add(hSinLk)); - final FieldUnivariateDerivative2 dakT2 = b.multiply(sinI2).multiply(hSinLk.multiply(0.5).subtract(lCosLk.multiply(0.5)). - add(cos2Lk).add(lCos3Lk.multiply(3.5)).add(hSin3Lk.multiply(3.5))); - final FieldUnivariateDerivative2 dak = dakT1.add(dakT2); - - // δh - final FieldUnivariateDerivative2 dhkT1 = b.multiply(om3o2xSinI2).multiply(sinLk.add(lSin2Lk.multiply(1.5)).subtract(hCos2Lk.multiply(1.5))); - final FieldUnivariateDerivative2 dhkT2 = b.multiply(sinI2).multiply(0.25).multiply(sinLk.subtract(sin3Lk.multiply(SEVEN_THIRD)).add(lSin2Lk.multiply(5.0)). - subtract(lSin4Lk.multiply(8.5)).add(hCos4Lk.multiply(8.5)).add(hCos2Lk)); - final FieldUnivariateDerivative2 dhkT3 = lSin2Lk.multiply(cosI2).multiply(b).multiply(0.5).negate(); - final FieldUnivariateDerivative2 dhk = dhkT1.subtract(dhkT2).add(dhkT3); - - // δl - final FieldUnivariateDerivative2 dlkT1 = b.multiply(om3o2xSinI2).multiply(cosLk.add(lCos2Lk.multiply(1.5)).add(hSin2Lk.multiply(1.5))); - final FieldUnivariateDerivative2 dlkT2 = b.multiply(sinI2).multiply(0.25).multiply(cosLk.negate().subtract(cos3Lk.multiply(SEVEN_THIRD)).subtract(hSin2Lk.multiply(5.0)). - subtract(lCos4Lk.multiply(8.5)).subtract(hSin4Lk.multiply(8.5)).add(lCos2Lk)); - final FieldUnivariateDerivative2 dlkT3 = hSin2Lk.multiply(cosI2).multiply(b).multiply(0.5); - final FieldUnivariateDerivative2 dlk = dlkT1.subtract(dlkT2).add(dlkT3); - - // δλ - final FieldUnivariateDerivative2 dokT1 = b.negate().multiply(cosI); - final FieldUnivariateDerivative2 dokT2 = lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)).subtract(sin2Lk.multiply(0.5)). - subtract(lSin3Lk.multiply(SEVEN_SIXTH)).add(hCos3Lk.multiply(SEVEN_SIXTH)); - final FieldUnivariateDerivative2 dok = dokT1.multiply(dokT2); - - // δi - final FieldUnivariateDerivative2 dik = b.multiply(sinI).multiply(cosI).multiply(0.5). - multiply(lCosLk.negate().add(hSinLk).add(cos2Lk).add(lCos3Lk.multiply(SEVEN_THIRD)).add(hSin3Lk.multiply(SEVEN_THIRD))); - - // δL - final FieldUnivariateDerivative2 dLkT1 = b.multiply(2.0).multiply(om3o2xSinI2).multiply(lSinLk.multiply(1.75).subtract(hCosLk.multiply(1.75))); - final FieldUnivariateDerivative2 dLkT2 = b.multiply(sinI2).multiply(3.0).multiply(hCosLk.multiply(SEVEN_24TH).negate().subtract(lSinLk.multiply(SEVEN_24TH)). - subtract(hCos3Lk.multiply(FN_72TH)).add(lSin3Lk.multiply(FN_72TH)).add(sin2Lk.multiply(0.25))); - final FieldUnivariateDerivative2 dLkT3 = b.multiply(cosI2).multiply(lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)).subtract(sin2Lk.multiply(0.5)). - subtract(lSin3Lk.multiply(SEVEN_SIXTH)).add(hCos3Lk.multiply(SEVEN_SIXTH))); - final FieldUnivariateDerivative2 dLk = dLkT1.add(dLkT2).add(dLkT3); - - // Final array - final FieldUnivariateDerivative2[] differentials = MathArrays.buildArray(a.getField(), 6); - differentials[0] = dak.multiply(a); - differentials[1] = dhk; - differentials[2] = dlk; - differentials[3] = dok; - differentials[4] = dik; - differentials[5] = dLk; - - return differentials; - } - - /** {@inheritDoc} */ - protected double getMass(final AbsoluteDate date) { - return mass; - } - - /** - * Get the Earth gravity coefficient used for GLONASS propagation. - * @return the Earth gravity coefficient. - */ - public T getMU() { - return field.getZero().add(GLONASSOrbitalElements.GLONASS_MU); - } - - /** - * Gets the underlying GLONASS orbital elements. - * - * @return the underlying GLONASS orbital elements - */ - public FieldGLONASSOrbitalElements getGLONASSOrbitalElements() { - return glonassOrbit; - } + /** + * Gets eccentric anomaly from mean anomaly. + *

+ * The algorithm used to solve the Kepler equation has been published in: + * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. Gooding, + * Celestial Mechanics 38 (1986) 307-334 + *

+ *

+ * It has been copied from the OREKIT library (KeplerianOrbit class). + *

+ * + * @param mk the mean anomaly (rad) + * @param e the eccentricity + * @return the eccentric anomaly (rad) + */ + private FieldUnivariateDerivative2 getEccentricAnomaly(final FieldUnivariateDerivative2 mk, + final FieldUnivariateDerivative2 e) { + + // reduce M to [-PI PI] interval + final T zero = mk.getValue().getField().getZero(); + final FieldUnivariateDerivative2 reducedM = new FieldUnivariateDerivative2( + MathUtils.normalizeAngle(mk.getValue(), zero), mk.getFirstDerivative(), mk.getSecondDerivative()); + + // compute start value according to A. W. Odell and R. H. Gooding S12 starter + FieldUnivariateDerivative2 ek; + if (FastMath.abs(reducedM.getValue()).getReal() < 1.0 / 6.0) { + if (FastMath.abs(reducedM.getValue()).getReal() < Precision.SAFE_MIN) { + // this is an Orekit change to the S12 starter. + // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN + // appearing later in + // the computation. As in this case E and M are almost equal, we initialize ek + // with reducedM + ek = reducedM; + } else { + // this is the standard S12 starter + ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(e)); + } + } else { + if (reducedM.getValue().getReal() < 0) { + final FieldUnivariateDerivative2 w = reducedM.add(FastMath.PI); + ek = reducedM + .add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(e)); + } else { + final FieldUnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI); + ek = reducedM + .add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(e)); + } + } + + final FieldUnivariateDerivative2 e1 = e.negate().add(1.0); + final boolean noCancellationRisk = (e1.getReal() + + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; + + // perform two iterations, each consisting of one Halley step and one + // Newton-Raphson step + for (int j = 0; j < 2; ++j) { + final FieldUnivariateDerivative2 f; + FieldUnivariateDerivative2 fd; + final FieldUnivariateDerivative2 fdd = ek.sin().multiply(e); + final FieldUnivariateDerivative2 fddd = ek.cos().multiply(e); + if (noCancellationRisk) { + f = ek.subtract(fdd).subtract(reducedM); + fd = fddd.subtract(1).negate(); + } else { + f = eMeSinE(ek, e).subtract(reducedM); + final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); + fd = s.multiply(s).multiply(e.multiply(2.0)).add(e1); + } + final FieldUnivariateDerivative2 dee = f.multiply(fd) + .divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd))); + + // update eccentric anomaly, using expressions that limit underflow problems + final FieldUnivariateDerivative2 w = fd + .add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3)))); + fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); + ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd)); + } + + // expand the result back to original range + ek = ek.add(mk.getValue().subtract(reducedM.getValue())); + + // Returns the eccentric anomaly + return ek; + } - /** - * Gets the Earth Centered Inertial frame used to propagate the orbit. - * @return the ECI frame - */ - public Frame getECI() { - return eci; - } + /** + * Accurate computation of E - e sin(E). + * + * @param E eccentric anomaly + * @param ecc the eccentricity + * @return E - e sin(E) + */ + private FieldUnivariateDerivative2 eMeSinE(final FieldUnivariateDerivative2 E, + final FieldUnivariateDerivative2 ecc) { + FieldUnivariateDerivative2 x = E.sin().multiply(ecc.negate().add(1.0)); + final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); + FieldUnivariateDerivative2 term = E; + FieldUnivariateDerivative2 d = E.getField().getZero(); + // the inequality test below IS intentional and should NOT be replaced by a + // check with a small tolerance + for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); !(x.getValue()).equals(x0.getValue());) { + d = d.add(2); + term = term.multiply(mE2.divide(d.multiply(d.add(1)))); + x0 = x; + x = x.subtract(term); + } + return x; + } - /** - * Gets the Earth Centered Earth Fixed frame used to propagate GLONASS orbits. - * @return the ECEF frame - */ - public Frame getECEF() { - return ecef; - } - - /** {@inheritDoc} */ - public Frame getFrame() { - return eci; - } - - /** {@inheritDoc} */ - public void resetInitialState(final SpacecraftState state) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - /** {@inheritDoc} */ - protected void resetIntermediateState(final SpacecraftState state, final boolean forward) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - /** {@inheritDoc} */ - protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date) { - // Gets the PVCoordinates in ECEF frame - final FieldPVCoordinates pvaInECEF = propagateInEcef(date); - // Transforms the PVCoordinates to ECI frame - final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); - // Returns the Cartesian orbit - final T zero = date.getField().getZero(); - return new FieldCartesianOrbit(pvaInECI, eci, date, zero.add(FieldGLONASSOrbitalElements.GLONASS_MU)); - } + /** + * Gets true anomaly from eccentric anomaly. + * + * @param ek the eccentric anomaly (rad) + * @param ecc the eccentricity + * @return the true anomaly (rad) + */ + private FieldUnivariateDerivative2 getTrueAnomaly(final FieldUnivariateDerivative2 ek, + final FieldUnivariateDerivative2 ecc) { + final FieldUnivariateDerivative2 svk = ek.sin().multiply(FastMath.sqrt(ecc.multiply(ecc).negate().add(1.0))); + final FieldUnivariateDerivative2 cvk = ek.cos().subtract(ecc); + return svk.atan2(cvk); + } - @Override - protected T getMass(FieldAbsoluteDate date) { - // TODO Auto-generated method stub - return null; + /** + * Get the interval of prediction. + * + * @param date the considered date + * @return the duration from GLONASS orbit Reference epoch (s) + */ + private FieldUnivariateDerivative2 getdTpr(final FieldAbsoluteDate date) { + final TimeScale glonass = dataContext.getTimeScales().getGLONASS(); + final FieldGLONASSDate tEnd = new FieldGLONASSDate(date.getField(), date, glonass); + final FieldGLONASSDate tSta = new FieldGLONASSDate(date.getField(), glonassOrbit.getDate(), glonass); + final int n = tEnd.getDayNumber(); + final int na = tSta.getDayNumber(); + final int deltaN; + if (na == 27) { + deltaN = n - na - FastMath.round((float) (n - na) / 1460) * 1460; + } else { + deltaN = n - na - FastMath.round((float) (n - na) / 1461) * 1461; + } + + final T zero = date.getField().getZero(); + final FieldUnivariateDerivative2 ti = new FieldUnivariateDerivative2(zero.add(tEnd.getSecInDay()), + zero.add(1.0), zero); + + return ti.subtract(glonassOrbit.getTime()).add(86400 * deltaN); } - @Override + /** + * Computes the semi-major axis of orbit using technique of successive + * approximations. + * + * @param tDR mean draconique period (s) + * @param i current inclination (rad) + * @param e eccentricity + * @return the semi-major axis (m). + */ + private FieldUnivariateDerivative2 computeSma(final FieldUnivariateDerivative2 tDR, + final FieldUnivariateDerivative2 i, final FieldUnivariateDerivative2 e) { + + // Zero + final FieldUnivariateDerivative2 zero = tDR.getField().getZero(); + + // If one of the input parameter is equal to Double.NaN, an infinite loop can + // occur. + // In that case, we do not compute the value of the semi major axis. + // We decided to return a Double.NaN value instead. + if (Double.isNaN(tDR.getValue().getReal()) || Double.isNaN(i.getValue().getReal()) + || Double.isNaN(e.getValue().getReal())) { + return zero.add(Double.NaN); + } + + // Common parameters + final FieldUnivariateDerivative2 sinI = FastMath.sin(i); + final FieldUnivariateDerivative2 sin2I = sinI.multiply(sinI); + final FieldUnivariateDerivative2 ome2 = e.multiply(e).negate().add(1.0); + final FieldUnivariateDerivative2 ome2Pow3o2 = FastMath.sqrt(ome2).multiply(ome2); + final FieldUnivariateDerivative2 pa = zero.add(glonassOrbit.getPa()); + final FieldUnivariateDerivative2 cosPA = FastMath.cos(pa); + final FieldUnivariateDerivative2 opecosPA = e.multiply(cosPA).add(1.0); + final FieldUnivariateDerivative2 opecosPAPow2 = opecosPA.multiply(opecosPA); + final FieldUnivariateDerivative2 opecosPAPow3 = opecosPAPow2.multiply(opecosPA); + + // Initial approximation + FieldUnivariateDerivative2 tOCK = tDR; + + // Successive approximations + // The process of approximation ends when fulfilling the following condition: + // |a(n+1) - a(n)| < 1cm + FieldUnivariateDerivative2 an = zero; + FieldUnivariateDerivative2 anp1 = zero; + boolean isLastStep = false; + while (!isLastStep) { + + // a(n+1) computation + final FieldUnivariateDerivative2 tOCKo2p = tOCK.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI); + final FieldUnivariateDerivative2 tOCKo2pPow2 = tOCKo2p.multiply(tOCKo2p); + anp1 = FastMath.cbrt(tOCKo2pPow2.multiply(GLONASSOrbitalElements.GLONASS_MU)); + + // p(n+1) computation + final FieldUnivariateDerivative2 p = anp1.multiply(ome2); + + // Tock(n+1) computation + final FieldUnivariateDerivative2 aeop = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); + final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); + final FieldUnivariateDerivative2 term1 = aeop2.multiply(GLONASS_J20).multiply(1.5); + final FieldUnivariateDerivative2 term2 = sin2I.multiply(2.5).negate().add(2.0); + final FieldUnivariateDerivative2 term3 = ome2Pow3o2.divide(opecosPAPow2); + final FieldUnivariateDerivative2 term4 = opecosPAPow3.divide(ome2); + tOCK = tDR.divide(term1.multiply(term2.multiply(term3).add(term4)).negate().add(1.0)); + + // Check convergence + if (FastMath.abs(anp1.subtract(an).getReal()) <= 0.01) { + isLastStep = true; + } + + an = anp1; + } + + return an; + + } + + /** + * Computes the current longitude of the ascending node. + * + * @param dTpr interval of prediction (s) + * @param n mean motion (rad/s) + * @param aeop2 square of the ratio between the radius of the ellipsoid and p, + * with p = sma * (1 - ecc²) + * @param i inclination (rad) + * @return the current longitude of the ascending node (rad) + */ + private FieldUnivariateDerivative2 computeLambda(final FieldUnivariateDerivative2 dTpr, + final FieldUnivariateDerivative2 n, final FieldUnivariateDerivative2 aeop2, + final FieldUnivariateDerivative2 i) { + final FieldUnivariateDerivative2 cosI = FastMath.cos(i); + final FieldUnivariateDerivative2 precession = aeop2.multiply(n).multiply(cosI).multiply(1.5 * GLONASS_J20); + return dTpr.multiply(precession.add(GLONASS_AV)).negate().add(glonassOrbit.getLambda()); + } + + /** + * Computes the current argument of perigee. + * + * @param dTpr interval of prediction (s) + * @param n mean motion (rad/s) + * @param aeop2 square of the ratio between the radius of the ellipsoid and p, + * with p = sma * (1 - ecc²) + * @param i inclination (rad) + * @return the current argument of perigee (rad) + */ + private FieldUnivariateDerivative2 computePA(final FieldUnivariateDerivative2 dTpr, + final FieldUnivariateDerivative2 n, final FieldUnivariateDerivative2 aeop2, + final FieldUnivariateDerivative2 i) { + final FieldUnivariateDerivative2 cosI = FastMath.cos(i); + final FieldUnivariateDerivative2 cos2I = cosI.multiply(cosI); + final FieldUnivariateDerivative2 precession = aeop2.multiply(n) + .multiply(cos2I.multiply(5.0).negate().add(1.0)).multiply(0.75 * GLONASS_J20); + return dTpr.multiply(precession).negate().add(glonassOrbit.getPa()); + } + + /** + * Computes the differentials δai. + *

+ * The value of i depends of the type of longitude (i = 2 for the current mean + * longitude; i = 1 for the mean longitude at the instant the spacecraft passes + * the current ascending node) + *

+ * + * @param a semi-major axis (m) + * @param i inclination (rad) + * @param h x component of the eccentricity (rad) + * @param l y component of the eccentricity (rad) + * @param m longitude (current or at the ascending node instant) + * @return the differentials of the orbital parameters + */ + private FieldUnivariateDerivative2[] getParameterDifferentials(final FieldUnivariateDerivative2 a, + final FieldUnivariateDerivative2 i, final FieldUnivariateDerivative2 h, + final FieldUnivariateDerivative2 l, final FieldUnivariateDerivative2 m) { + + // B constant + final FieldUnivariateDerivative2 aeoa = a.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); + final FieldUnivariateDerivative2 aeoa2 = aeoa.multiply(aeoa); + final FieldUnivariateDerivative2 b = aeoa2.multiply(1.5 * GLONASS_J20); + + // Commons Parameters + final FieldSinCos> scI = FastMath.sinCos(i); + final FieldSinCos> scLk = FastMath.sinCos(m); + final FieldSinCos> sc2Lk = FieldSinCos.sum(scLk, scLk); + final FieldSinCos> sc3Lk = FieldSinCos.sum(scLk, sc2Lk); + final FieldSinCos> sc4Lk = FieldSinCos.sum(sc2Lk, sc2Lk); + final FieldUnivariateDerivative2 cosI = scI.cos(); + final FieldUnivariateDerivative2 sinI = scI.sin(); + final FieldUnivariateDerivative2 cosI2 = cosI.multiply(cosI); + final FieldUnivariateDerivative2 sinI2 = sinI.multiply(sinI); + final FieldUnivariateDerivative2 cosLk = scLk.cos(); + final FieldUnivariateDerivative2 sinLk = scLk.sin(); + final FieldUnivariateDerivative2 cos2Lk = sc2Lk.cos(); + final FieldUnivariateDerivative2 sin2Lk = sc2Lk.sin(); + final FieldUnivariateDerivative2 cos3Lk = sc3Lk.cos(); + final FieldUnivariateDerivative2 sin3Lk = sc3Lk.sin(); + final FieldUnivariateDerivative2 cos4Lk = sc4Lk.cos(); + final FieldUnivariateDerivative2 sin4Lk = sc4Lk.sin(); + + // h*cos(nLk), l*cos(nLk), h*sin(nLk) and l*sin(nLk) + // n = 1 + final FieldUnivariateDerivative2 hCosLk = h.multiply(cosLk); + final FieldUnivariateDerivative2 hSinLk = h.multiply(sinLk); + final FieldUnivariateDerivative2 lCosLk = l.multiply(cosLk); + final FieldUnivariateDerivative2 lSinLk = l.multiply(sinLk); + // n = 2 + final FieldUnivariateDerivative2 hCos2Lk = h.multiply(cos2Lk); + final FieldUnivariateDerivative2 hSin2Lk = h.multiply(sin2Lk); + final FieldUnivariateDerivative2 lCos2Lk = l.multiply(cos2Lk); + final FieldUnivariateDerivative2 lSin2Lk = l.multiply(sin2Lk); + // n = 3 + final FieldUnivariateDerivative2 hCos3Lk = h.multiply(cos3Lk); + final FieldUnivariateDerivative2 hSin3Lk = h.multiply(sin3Lk); + final FieldUnivariateDerivative2 lCos3Lk = l.multiply(cos3Lk); + final FieldUnivariateDerivative2 lSin3Lk = l.multiply(sin3Lk); + // n = 4 + final FieldUnivariateDerivative2 hCos4Lk = h.multiply(cos4Lk); + final FieldUnivariateDerivative2 hSin4Lk = h.multiply(sin4Lk); + final FieldUnivariateDerivative2 lCos4Lk = l.multiply(cos4Lk); + final FieldUnivariateDerivative2 lSin4Lk = l.multiply(sin4Lk); + + // 1 - (3 / 2)*sin²i + final FieldUnivariateDerivative2 om3o2xSinI2 = sinI2.multiply(1.5).negate().add(1.0); + + // Compute Differentials + // δa + final FieldUnivariateDerivative2 dakT1 = b.multiply(2.0).multiply(om3o2xSinI2).multiply(lCosLk.add(hSinLk)); + final FieldUnivariateDerivative2 dakT2 = b.multiply(sinI2).multiply(hSinLk.multiply(0.5) + .subtract(lCosLk.multiply(0.5)).add(cos2Lk).add(lCos3Lk.multiply(3.5)).add(hSin3Lk.multiply(3.5))); + final FieldUnivariateDerivative2 dak = dakT1.add(dakT2); + + // δh + final FieldUnivariateDerivative2 dhkT1 = b.multiply(om3o2xSinI2) + .multiply(sinLk.add(lSin2Lk.multiply(1.5)).subtract(hCos2Lk.multiply(1.5))); + final FieldUnivariateDerivative2 dhkT2 = b.multiply(sinI2).multiply(0.25) + .multiply(sinLk.subtract(sin3Lk.multiply(SEVEN_THIRD)).add(lSin2Lk.multiply(5.0)) + .subtract(lSin4Lk.multiply(8.5)).add(hCos4Lk.multiply(8.5)).add(hCos2Lk)); + final FieldUnivariateDerivative2 dhkT3 = lSin2Lk.multiply(cosI2).multiply(b).multiply(0.5).negate(); + final FieldUnivariateDerivative2 dhk = dhkT1.subtract(dhkT2).add(dhkT3); + + // δl + final FieldUnivariateDerivative2 dlkT1 = b.multiply(om3o2xSinI2) + .multiply(cosLk.add(lCos2Lk.multiply(1.5)).add(hSin2Lk.multiply(1.5))); + final FieldUnivariateDerivative2 dlkT2 = b.multiply(sinI2).multiply(0.25) + .multiply(cosLk.negate().subtract(cos3Lk.multiply(SEVEN_THIRD)).subtract(hSin2Lk.multiply(5.0)) + .subtract(lCos4Lk.multiply(8.5)).subtract(hSin4Lk.multiply(8.5)).add(lCos2Lk)); + final FieldUnivariateDerivative2 dlkT3 = hSin2Lk.multiply(cosI2).multiply(b).multiply(0.5); + final FieldUnivariateDerivative2 dlk = dlkT1.subtract(dlkT2).add(dlkT3); + + // δλ + final FieldUnivariateDerivative2 dokT1 = b.negate().multiply(cosI); + final FieldUnivariateDerivative2 dokT2 = lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)) + .subtract(sin2Lk.multiply(0.5)).subtract(lSin3Lk.multiply(SEVEN_SIXTH)) + .add(hCos3Lk.multiply(SEVEN_SIXTH)); + final FieldUnivariateDerivative2 dok = dokT1.multiply(dokT2); + + // δi + final FieldUnivariateDerivative2 dik = b.multiply(sinI).multiply(cosI).multiply(0.5).multiply(lCosLk.negate() + .add(hSinLk).add(cos2Lk).add(lCos3Lk.multiply(SEVEN_THIRD)).add(hSin3Lk.multiply(SEVEN_THIRD))); + + // δL + final FieldUnivariateDerivative2 dLkT1 = b.multiply(2.0).multiply(om3o2xSinI2) + .multiply(lSinLk.multiply(1.75).subtract(hCosLk.multiply(1.75))); + final FieldUnivariateDerivative2 dLkT2 = b.multiply(sinI2).multiply(3.0) + .multiply(hCosLk.multiply(SEVEN_24TH).negate().subtract(lSinLk.multiply(SEVEN_24TH)) + .subtract(hCos3Lk.multiply(FN_72TH)).add(lSin3Lk.multiply(FN_72TH)).add(sin2Lk.multiply(0.25))); + final FieldUnivariateDerivative2 dLkT3 = b.multiply(cosI2) + .multiply(lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)).subtract(sin2Lk.multiply(0.5)) + .subtract(lSin3Lk.multiply(SEVEN_SIXTH)).add(hCos3Lk.multiply(SEVEN_SIXTH))); + final FieldUnivariateDerivative2 dLk = dLkT1.add(dLkT2).add(dLkT3); + + // Final array + final FieldUnivariateDerivative2[] differentials = MathArrays.buildArray(a.getField(), 6); + differentials[0] = dak.multiply(a); + differentials[1] = dhk; + differentials[2] = dlk; + differentials[3] = dok; + differentials[4] = dik; + differentials[5] = dLk; + + return differentials; + } + + /** {@inheritDoc} */ + protected T getMass(final FieldAbsoluteDate date) { + return mass; + } + + /** + * Get the Earth gravity coefficient used for GLONASS propagation. + * + * @return the Earth gravity coefficient. + */ + public T getMU() { + return field.getZero().add(GLONASSOrbitalElements.GLONASS_MU); + } + + /** + * Gets the underlying Field GLONASS orbital elements. + * + * @return the underlying Field GLONASS orbital elements + */ + public FieldGLONASSOrbitalElements getGLONASSOrbitalElements() { + return glonassOrbit; + } + + /** + * Gets the Earth Centered Inertial frame used to propagate the orbit. + * + * @return the ECI frame + */ + public Frame getECI() { + return eci; + } + + /** + * Gets the Earth Centered Earth Fixed frame used to propagate GLONASS orbits. + * + * @return the ECEF frame + */ + public Frame getECEF() { + return ecef; + } + + /** {@inheritDoc} */ + public Frame getFrame() { + return eci; + } + + /** {@inheritDoc} */ + public void resetInitialState(final FieldSpacecraftState state) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ protected void resetIntermediateState(FieldSpacecraftState state, boolean forward) { - // TODO Auto-generated method stub - + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); } - @Override + /** {@inheritDoc} */ protected FieldOrbit propagateOrbit(FieldAbsoluteDate date, T[] parameters) { - // TODO Auto-generated method stub - return null; + // Gets the PVCoordinates in ECEF frame + final FieldPVCoordinates pvaInECEF = propagateInEcef(date); + // Transforms the PVCoordinates to ECI frame + final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); + // Returns the Cartesian orbit + return new FieldCartesianOrbit<>(pvaInECI, eci, date, date.getField().getZero().add(FieldGLONASSOrbitalElements.GLONASS_MU)); } - + + /** Get the parameters driver for the Field Glonass propagation model. + * @return an empty list. + */ @Override protected List getParametersDrivers() { - // TODO Auto-generated method stub - return null; + // The Field Glonass propagation model does not have parameter drivers. + return Collections.emptyList(); } - } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java index 6674b08a3..8d9689499 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java @@ -3,7 +3,6 @@ package org.orekit.propagation.analytical.gnss; import java.io.Serializable; import org.hipparchus.Field; -import org.hipparchus.FieldElement; import org.hipparchus.RealFieldElement; import org.hipparchus.util.FastMath; import org.orekit.annotation.DefaultDataContext; @@ -13,265 +12,276 @@ import org.orekit.time.DateComponents; import org.orekit.time.DateTimeComponents; import org.orekit.time.FieldAbsoluteDate; import org.orekit.time.FieldTimeStamped; -import org.orekit.time.GLONASSDate; import org.orekit.time.TimeComponents; import org.orekit.time.TimeScale; import org.orekit.utils.Constants; -public class FieldGLONASSDate> implements Serializable, FieldTimeStamped, Field { +public class FieldGLONASSDate> implements Serializable, FieldTimeStamped { - /** Serializable UID. */ - private static final long serialVersionUID = 20190131L; - - /** Constant for date computation. */ - private static final int C1 = 44195; - - /** Constant for date computation. */ - private static final int C2 = 45290; - - /** The number of the current day in a four year interval Na. */ - private final int na; - - /** The number of the current four year interval N4. */ - private final int n4; - - /** Number of seconds since Na. */ - private final double secInNa; - - /** Current Julian date JD0. */ - private double jd0; - - /** Greenwich Mean Sidereal Time (rad). */ - private double gmst; - - /** Corresponding date. */ - private final transient FieldAbsoluteDate date; - - /** Build an instance corresponding to a GLONASS date. - * - *

This method uses the {@link DataContext#getDefault() default data context}. - * - * @param na the number of the current day in a four year interval - * @param n4 the number of the current four year interval - * @param secInNa the number of seconds since na start - * @see #GLONASSDate(int, int, double, TimeScale) - */ - @DefaultDataContext - public FieldGLONASSDate(final int na, final int n4, final double secInNa) { - this(na, n4, secInNa, DataContext.getDefault().getTimeScales().getGLONASS()); - } - - /** - * Build an instance corresponding to a GLONASS date. - * - * @param na the number of the current day in a four year interval - * @param n4 the number of the current four year interval - * @param secInNa the number of seconds since na start - * @param glonass time scale. - * @since 10.1 - */ - public FieldGLONASSDate(final int na, - final int n4, - final double secInNa, - final TimeScale glonass) { - this.na = na; - this.n4 = n4; - this.secInNa = secInNa; - // Compute JD0 - final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); - this.jd0 = 1461 * (n4 - 1) + na + 2450082.5 - ratio; - // GMST - this.gmst = computeGMST(); - this.date = computeDate(glonass); - } - - /** Build an instance from an absolute date. - * - *

This method uses the {@link DataContext#getDefault() default data context}. - * - * @param date absolute date to consider - * @see #GLONASSDate(AbsoluteDate, TimeScale) - */ - @DefaultDataContext - public FieldGLONASSDate(final FieldAbsoluteDate date) { - this(date, DataContext.getDefault().getTimeScales().getGLONASS()); - } - - /** - * Build an instance from an absolute date. - * - * @param date absolute date to consider - * @param glonass time scale. - * @since 10.1 - */ - public FieldGLONASSDate(final FieldAbsoluteDate date, final TimeScale glonass) { - final DateTimeComponents dateTime = date.getComponents(glonass); - // N4 - final int year = dateTime.getDate().getYear(); - this.n4 = ((int) (year - 1996) / 4) + 1; - // Na - final int start = 1996 + 4 * (n4 - 1); - - final Field field = null; - final double duration = date.durationFrom(new FieldAbsoluteDate(field, start, 1, 1, glonass)).getReal(); - - this.na = (int) (duration / 86400) + 1; - this.secInNa = dateTime.getTime().getSecondsInLocalDay(); - // Compute JD0 - final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); - this.jd0 = 1461 * (n4 - 1) + na + 2450082.5 - ratio; - // GMST - this.gmst = computeGMST(); - this.date = date; - } - - @Override - public FieldAbsoluteDate getDate() { - return date; - } - - /** Get the number of seconds since Na start. - * @return number of seconds since Na start - */ - public double getSecInDay() { - return secInNa; - } - - /** Get the number of the current day in a four year interval. - * @return the number of the current day in a four year interval - */ - public int getDayNumber() { - return na; - } - - /** Get the number of the current four year interval. - * @return the number of the current four year interval - */ - public int getIntervalNumber() { - return n4; - } - - /** Get the current Julian date JD0. - * @return the current date JD0 - */ - public double getJD0() { - return jd0; - } - - /** Get the Greenwich Mean Sidereal Time. - * @return the Greenwich Mean Sidereal Time (rad) - */ - public double getGMST() { - return gmst; - } - - /** Compute the Greenwich Mean Sidereal Time using the current Julian date JD0. - * @return the Greenwich Mean Sidereal Time (rad) - */ - private double computeGMST() { - final double ref = 2451545.0; - // Earth's rotation angle in radians - final double era = 2. * GLONASSOrbitalElements.GLONASS_PI * - (0.7790572732640 + 1.00273781191135448 * (jd0 - ref)); - // Time from Epoch 2000 (1st January, 00:00 UTC) till current Epoch in Julian centuries - final double time = (jd0 - ref) / Constants.JULIAN_CENTURY; - // Time to the power n - final double time2 = time * time; - final double time3 = time2 * time; - final double time4 = time2 * time2; - final double time5 = time2 * time3; - // GMST computation - final double gTime = era + 7.03270726e-8 + time * 2.23603658710194e-2 + - time2 * 6.7465784654e-6 - time3 * 2.1332e-12 - time4 * 1.452308e-10 - time5 * 1.784e-13; - return gTime; - } - - /** Compute the GLONASS date. - * @return the date - * @param glonass time scale. - */ - private FieldAbsoluteDate computeDate(final TimeScale glonass) { - // Compute the number of Julian day for the current date - final double jdn = jd0 + 0.5; - // Coefficients - final int a = (int) (jdn + 32044); - final int b = (4 * a + 3) / 146097; - final int c = a - (146097 * b) / 4; - final int d = (4 * c + 3) / 1461; - final int e = c - (1461 * d) / 4; - final int m = (5 * e + 2) / 153; - // Year, month and day - final int day = e - (153 * m + 2) / 5 + 1; - final int month = m + 3 - 12 * (m / 10); - final int year = 100 * b + d - 4800 + m / 10; - - - final Field field = null; - return new FieldAbsoluteDate(field, new DateComponents(year, month, day), - new TimeComponents(secInNa), - glonass); - } - - /** Replace the instance with a data transfer object for serialization. - * @return data transfer object that will be serialized - */ - @DefaultDataContext - private Object writeReplace() { - return new DataTransferObject(na, n4, secInNa); - } - - /** Internal class used only for serialization. */ - @DefaultDataContext - private static class DataTransferObject implements Serializable { - - /** Serializable UID. */ - private static final long serialVersionUID = 20190131L; - - /** The number of the current day in a four year interval Na. */ - private final int na; - - /** The number of the current four year interval N4. */ - private final int n4; - - /** Number of seconds since Na. */ - private final double secInNa; - - /** Simple constructor. - * @param na the number of the current day in a four year interval - * @param n4 the number of the current four year interval - * @param secInNa the number of seconds since na start - */ - DataTransferObject(final int na, final int n4, final double secInNa) { - this.na = na; - this.n4 = n4; - this.secInNa = secInNa; - } - - /** Replace the deserialized data transfer object with a {@link GPSDate}. - * @return replacement {@link GPSDate} - */ - private Object readResolve() { - return new GLONASSDate(na, n4, secInNa); - } - - } + private static final long serialVersionUID = 20190131L; - @Override - public T getZero() { - // TODO Auto-generated method stub - return null; + /** Constant for date computation. */ + private static final int C1 = 44195; + + /** Constant for date computation. */ + private static final int C2 = 45290; + + /** The number of the current day in a four year interval Na. */ + private final int na; + + /** The number of the current four year interval N4. */ + private final int n4; + + /** Number of seconds since Na. */ + private final T secInNa; + + /** Current Julian date JD0. */ + private T jd0; + + /** Greenwich Mean Sidereal Time (rad). */ + private T gmst; + + /** Corresponding date. */ + private final transient FieldAbsoluteDate date; + + private final Field field; + + + /** + * Build an instance corresponding to a GLONASS date. + * + *

+ * This method uses the {@link DataContext#getDefault() default data context}. + * + * @param na the number of the current day in a four year interval + * @param n4 the number of the current four year interval + * @param secInNa the number of seconds since na start + * @see #GLONASSDate(int, int, T, TimeScale) + */ + @DefaultDataContext + public FieldGLONASSDate(final Field field, final int na, final int n4, final T secInNa) { + this(field, na, n4, secInNa, DataContext.getDefault().getTimeScales().getGLONASS()); } - @Override - public T getOne() { - // TODO Auto-generated method stub - return null; + /** + * Build an instance corresponding to a GLONASS date. + * + * @param na the number of the current day in a four year interval + * @param n4 the number of the current four year interval + * @param secInNa the number of seconds since na start + * @param glonass time scale. + * @since 10.1 + */ + public FieldGLONASSDate(final Field field, final int na, final int n4, final T secInNa, final TimeScale glonass) { + this.field = field; + this.na = na; + this.n4 = n4; + final T zero = field.getZero(); + this.secInNa = zero.add(secInNa); + // Compute JD0 + final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); + this.jd0 = zero.add(1461 * (n4 - 1) + na + 2450082.5 - ratio); + // GMST + this.gmst = computeGMST(); + this.date = computeDate(field, glonass); + } + + /** + * Build an instance from an absolute date. + * + *

+ * This method uses the {@link DataContext#getDefault() default data context}. + * + * @param date absolute date to consider + * @see #GLONASSDate(AbsoluteDate, TimeScale) + */ + @DefaultDataContext + public FieldGLONASSDate(final Field field, final FieldAbsoluteDate date) { + this(field, date, DataContext.getDefault().getTimeScales().getGLONASS()); + } + + /** + * Build an instance from an absolute date. + * + * @param date absolute date to consider + * @param glonass time scale. + * @since 10.1 + */ + public FieldGLONASSDate(final Field field, final FieldAbsoluteDate date, final TimeScale glonass) { + this.field = field; + final DateTimeComponents dateTime = date.getComponents(glonass); + // N4 + final int year = dateTime.getDate().getYear(); + this.n4 = ((int) (year - 1996) / 4) + 1; + // Na + final int start = 1996 + 4 * (n4 - 1); + + final T zero = field.getZero(); + final T duration = zero.add(date.durationFrom(new FieldAbsoluteDate(field, start, 1, 1, glonass)) + .getReal()); + + this.na = (int) (duration.getReal() / 86400) + 1; + this.secInNa = zero.add(dateTime.getTime().getSecondsInLocalDay()); + // Compute JD0 + final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); + this.jd0 = zero.add(1461 * (n4 - 1) + na + 2450082.5 - ratio); + // GMST + this.gmst = computeGMST(); + this.date = date; } @Override - public Class> getRuntimeClass() { - // TODO Auto-generated method stub - return null; + public FieldAbsoluteDate getDate() { + return date; + } + + /** + * Get the number of seconds since Na start. + * + * @return number of seconds since Na start + */ + public T getSecInDay() { + return secInNa; } + + /** + * Get the number of the current day in a four year interval. + * + * @return the number of the current day in a four year interval + */ + public int getDayNumber() { + return na; + } + + /** + * Get the number of the current four year interval. + * + * @return the number of the current four year interval + */ + public int getIntervalNumber() { + return n4; + } + + /** + * Get the current Julian date JD0. + * + * @return the current date JD0 + */ + public T getJD0() { + return jd0; + } + + /** + * Get the Greenwich Mean Sidereal Time. + * + * @return the Greenwich Mean Sidereal Time (rad) + */ + public T getGMST() { + return gmst; + } + + /** + * Compute the Greenwich Mean Sidereal Time using the current Julian date JD0. + * + * @return the Greenwich Mean Sidereal Time (rad) + */ + private T computeGMST() { + final T zero = field.getZero(); + final T ref = zero.add(2451545.0); + // Earth's rotation angle in radians + final T era = zero.add(2. * GLONASSOrbitalElements.GLONASS_PI + * (0.7790572732640)).add((jd0.subtract(ref)).multiply(1.00273781191135448)); + // Time from Epoch 2000 (1st January, 00:00 UTC) till current Epoch in Julian + // centuries + final T time = (jd0.subtract(ref)).divide(Constants.JULIAN_CENTURY); + // Time to the power n + final T time2 = time.multiply(time); + final T time3 = time2.multiply(time); + final T time4 = time2.multiply(time2); + final T time5 = time2.multiply(time3); + // GMST computation + final T gTime = era.add(7.03270726e-8).add(time.multiply(2.23603658710194e-2)).add(time2.add(6.7465784654e-6)) + .subtract(time3.multiply(2.1332e-12)).subtract(time4.multiply(1.452308e-10)).subtract(time5.multiply(1.784e-13)); + return gTime; + } + + /** + * Compute the GLONASS date. + * + * @return the date + * @param glonass time scale. + */ + private FieldAbsoluteDate computeDate(final Field field, final TimeScale glonass) { + // Compute the number of Julian day for the current date + final T jdn = jd0.add(0.5); + // Coefficients + final int a = (int) (jdn.getReal() + 32044); + final int b = (4 * a + 3) / 146097; + final int c = a - (146097 * b) / 4; + final int d = (4 * c + 3) / 1461; + final int e = c - (1461 * d) / 4; + final int m = (5 * e + 2) / 153; + // Year, month and day + final int day = e - (153 * m + 2) / 5 + 1; + final int month = m + 3 - 12 * (m / 10); + final int year = 100 * b + d - 4800 + m / 10; + + return new FieldAbsoluteDate(field, new DateComponents(year, month, day), + new TimeComponents(secInNa.getReal()), glonass); + } + + /** + * Replace the instance with a data transfer object for serialization. + * + * @return data transfer object that will be serialized + */ + @DefaultDataContext + private Object writeReplace() { + return new DataTransferObject(field, na, n4, secInNa); + } + + /** Internal class used only for serialization. */ + @DefaultDataContext + private class DataTransferObject implements Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 20190131L; + + /** The number of the current day in a four year interval Na. */ + private final int na; + + /** The number of the current four year interval N4. */ + private final int n4; + + /** Number of seconds since Na. */ + private final T secInNa; + + private final Field field; + + /** + * Simple constructor. + * + * @param na the number of the current day in a four year interval + * @param n4 the number of the current four year interval + * @param secInNa the number of seconds since na start + */ + DataTransferObject(final Field field, final int na, final int n4, final T secInNa) { + this.field = field; + this.na = na; + this.n4 = n4; + this.secInNa = secInNa; + } + + /** + * Replace the deserialized data transfer object with a {@link GPSDate}. + * + * @return replacement {@link GPSDate} + */ + private Object readResolve() { + return new FieldGLONASSDate<>(field, na, n4, secInNa); + } + + } + } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java index 1ae26e458..db58f51e9 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java @@ -1,14 +1,30 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; import org.orekit.time.FieldTimeStamped; -/** - * This interface provides the minimal set of orbital elements needed by the {@link FieldAbstractGNSSPropagator}. - * @author nfialton - * - * @param - */ +/** This interface provides the minimal set of Field orbital elements needed by the {@link FieldAbstractGNSSPropagator}. +* +* @author Pascal Parraud +* @author Nicolas Fialton (field translation) +* +*/ public interface FieldGNSSOrbitalElements> extends FieldTimeStamped { diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java index 80f92068d..55c959306 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java @@ -24,10 +24,9 @@ import org.orekit.data.DataContext; import org.orekit.gnss.GPSAlmanac; import org.orekit.gnss.SatelliteSystem; import org.orekit.time.FieldAbsoluteDate; -import org.orekit.time.GNSSDate; /** - * This class holds a GPS almanac as read from SEM or YUMA files. + * This class holds a Field GPS almanac as read from SEM or YUMA files. * *

* Depending on the source (SEM or YUMA), some fields may be filled in or not. @@ -169,8 +168,10 @@ public class FieldGPSAlmanac> implements FieldGPSO /** * Constructor * + * This constructor converts a GPSAlmanac into a FieldGPSAlmanac + * * @param field - * @param almanac + * @param almanac a GPSAlmanac */ public FieldGPSAlmanac(Field field, GPSAlmanac almanac) { this.zero = field.getZero(); diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java index c04e01dc4..446f34c13 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java @@ -18,7 +18,7 @@ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; -/** This interface provides the minimal set of orbital elements needed by the {@link GPSPropagator}. +/** This interface provides the minimal set of Field orbital elements needed by the {@link FieldGPSPropagator}. * * @see GPS Interface Specification * @author Pascal Parraud diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java index e8427ff8f..4e55a5cd9 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java @@ -16,6 +16,9 @@ */ package org.orekit.propagation.analytical.gnss; +import java.util.Collections; +import java.util.List; + import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.orekit.annotation.DefaultDataContext; @@ -25,9 +28,10 @@ import org.orekit.frames.Frame; import org.orekit.frames.Frames; import org.orekit.propagation.Propagator; import org.orekit.utils.IERSConventions; +import org.orekit.utils.ParameterDriver; /** - * This class aims at propagating a GPS orbit from {@link GPSOrbitalElements}. + * This class aims at propagating a GPS orbit from {@link FieldGPSOrbitalElements}. * * @see GPS * Interface Specification @@ -50,8 +54,30 @@ public class FieldGPSPropagator> extends FieldAbst /** * Default constructor. * + *

The Field GPS orbital elements is the only requested parameter to build a FieldGPSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, final Frames frames)}

+ * * @param field - * @param gpsOrbit + * @param gpsOrbit the Field GPS orbital elements to be used by the GPSpropagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) */ @DefaultDataContext public FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit) { @@ -61,9 +87,32 @@ public class FieldGPSPropagator> extends FieldAbst /** * Constructor. * + *

The Field GPS orbital elements is the only requested parameter to build a FieldGPSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)}

+ * * @param field - * @param gpsOrbit - * @param frames + * @param gpsOrbit the Field GPS orbital elements to be used by the GPSpropagator. + * @param frames set of frames to use. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) */ public FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, final Frames frames) { this(field, gpsOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), @@ -73,8 +122,22 @@ public class FieldGPSPropagator> extends FieldAbst /** * Constructor. * + *

The Field GPS orbital elements is the only requested parameter to build a FieldGPSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * * @param field - * @param gpsOrbit + * @param gpsOrbit the Field GPS orbital elements to be used by the GPSpropagator. * @param attitudeProvider * @param mass * @param eci @@ -89,11 +152,20 @@ public class FieldGPSPropagator> extends FieldAbst } /** - * Get the underlying GPS orbital elements. + * Get the underlying Field GPS orbital elements. * - * @return the underlying GPS orbital elements + * @return the underlying Field GPS orbital elements */ public FieldGPSOrbitalElements getFieldGPSOrbitalElements() { return gpsOrbit; } + + /** Get the parameters driver for the Field GPS propagation model. + * @return an empty list. + */ + @Override + protected List getParametersDrivers() { + // The Field GPS propagation model does not have parameter drivers. + return Collections.emptyList(); + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java index eab9b66f3..ad04fb2af 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java @@ -16,10 +16,12 @@ */ package org.orekit.propagation.analytical.gnss; +import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.hipparchus.util.FastMath; import org.orekit.annotation.DefaultDataContext; import org.orekit.data.DataContext; +import org.orekit.gnss.GalileoAlmanac; import org.orekit.gnss.SatelliteSystem; import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; @@ -185,6 +187,35 @@ public class FieldGalileoAlmanac> implements Field final T sqa = dsqa.add(FastMath.sqrt(A0)); this.sma = sqa.multiply(sqa); } + + /** + * Constructor + * + * This constructor converts a GalileoAlmanac into a FieldGalileoAlmanac + * + * @param field + * @param almanac a GalileoAlmanac + */ + public FieldGalileoAlmanac(Field field, GalileoAlmanac almanac) { + this.zero = field.getZero(); + this.prn = almanac.getPRN(); + this.week = almanac.getWeek(); + this.toa = zero.add(almanac.getTime()); + this.ecc = zero.add(almanac.getE()); + this.inc = zero.add(almanac.getI0()); + this.iod = almanac.getIOD(); + this.om0 = zero.add(almanac.getOmega0()); + this.dom = zero.add(almanac.getOmegaDot()); + this.aop = zero.add(almanac.getPa()); + this.anom = zero.add(almanac.getM0()); + this.af0 = zero.add(almanac.getAf0()); + this.af1 = zero.add(almanac.getAf1()); + this.healthE1 = almanac.getHealthE1(); + this.healthE5a = almanac.getHealthE5a(); + this.healthE5b = almanac.getHealthE5b(); + this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); + this.sma = zero.add(almanac.getSma()); + } @Override public FieldAbsoluteDate getDate() { diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java index ed56f2a27..f67a14d4f 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java @@ -1,3 +1,19 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java index 508a0252a..8b315e1e1 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java @@ -16,6 +16,9 @@ */ package org.orekit.propagation.analytical.gnss; +import java.util.Collections; +import java.util.List; + import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.orekit.annotation.DefaultDataContext; @@ -25,10 +28,11 @@ import org.orekit.frames.Frame; import org.orekit.frames.Frames; import org.orekit.propagation.Propagator; import org.orekit.utils.IERSConventions; +import org.orekit.utils.ParameterDriver; /** - * This class aims at propagating a Galileo orbit from - * {@link GalileoOrbitalElements}. + * This class aims at propagating a Field Galileo orbit from + * {@link FieldGalileoOrbitalElements}. * * @see Galileo @@ -54,8 +58,37 @@ public class FieldGalileoPropagator> extends Field /** * Default constructor * + *

+ * The Field Galileo orbital elements is the only requested parameter to build a + * FieldGalileoPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. Another data context can be set using + * {@code FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, + final Frames frames)} + *

+ * * @param field - * @param galileoOrbit + * @param galileoOrbit the Field Galileo orbital elements to be used by the Field Galileo propagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) */ @DefaultDataContext public FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit) { @@ -65,9 +98,37 @@ public class FieldGalileoPropagator> extends Field /** * Constructor * + *

+ * The Field Galileo orbital elements is the only requested parameter to build a + * FieldGalileoPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)}

+ * * @param field - * @param galileoOrbit - * @param frames + * @param galileoOrbit the Field Galileo orbital elements to be used by the + * Field Galileo propagator. + * @param frames to use building the propagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) */ public FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, final Frames frames) { @@ -78,6 +139,25 @@ public class FieldGalileoPropagator> extends Field /** * Constructor * + * + *

+ * The Field Galileo orbital elements is the only requested parameter to build a + * FieldGalileoPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * * @param field * @param galileoOrbit * @param attitudeProvider @@ -89,17 +169,26 @@ public class FieldGalileoPropagator> extends Field final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { super(field, galileoOrbit, attitudeProvider, eci, ecef, mass, GALILEO_AV, GALILEO_CYCLE_DURATION, FieldGalileoOrbitalElements.GALILEO_MU); - // Stores the Beidou orbital elements + // Stores the Galileo orbital elements this.galileoOrbit = galileoOrbit; } /** - * Get the underlying Beidou orbital elements. + * Get the underlying Field Galileo orbital elements. * - * @return the underlying Beidou orbital elements + * @return the underlying Field Galileo orbital elements */ public FieldGalileoOrbitalElements getFieldGalileoOrbitalElements() { return galileoOrbit; } + /** Get the parameters driver for the Field Galileo propagation model. + * @return an empty list. + */ + @Override + protected List getParametersDrivers() { + // Field Galileo propagation model does not have parameter drivers. + return Collections.emptyList(); + } + } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java index 19271b643..71c1c713b 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java @@ -19,7 +19,6 @@ package org.orekit.propagation.analytical.gnss; import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.hipparchus.util.FastMath; -import org.orekit.gnss.IRNSSAlmanac; import org.orekit.time.FieldAbsoluteDate; /** @@ -114,6 +113,8 @@ public class FieldIRNSSAlmanac> implements FieldIR /** * Constructor * + * This constructor converts an IRNSSOrbitalElements (almanac) into a FieldIRNSSAlmanac + * * @param field * @param almanac */ diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java index 315f37180..7f9a39de3 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java @@ -1,7 +1,32 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; +/** This interface provides the minimal set of Field orbital elements needed by the {@link FieldIRNSSPropagator}. +* +* @see "Indian Regiona Navigation Satellite System, Signal In Space ICD +* for standard positioning service, version 1.1" +* +* @author Bryan Cazabonne +* @author Nicolas Fialton (field translation) +*/ + public interface FieldIRNSSOrbitalElements> extends FieldGNSSOrbitalElements { /** WGS 84 value of the Earth's universal gravitational parameter for IRNSS user in m³/s². */ diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java index fbcc53f02..d30b1e119 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java @@ -16,6 +16,9 @@ */ package org.orekit.propagation.analytical.gnss; +import java.util.Collections; +import java.util.List; + import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.orekit.annotation.DefaultDataContext; @@ -25,6 +28,7 @@ import org.orekit.frames.Frame; import org.orekit.frames.Frames; import org.orekit.propagation.Propagator; import org.orekit.utils.IERSConventions; +import org.orekit.utils.ParameterDriver; /** * This class aims at propagating a IRNSS orbit from @@ -52,8 +56,23 @@ public class FieldIRNSSPropagator> extends FieldAb /** * Default constructor. * + *

The Field IRNSS orbital elements is the only requested parameter to build a FieldIRNSSPropagator.

+ *

The attitude provider is set by default to be aligned with the J2000 frame.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame}.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP CIO/2010-based ITRF simple EOP}. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit, + final Frames frames)}

+ * * @param field - * @param irnssOrbit + * @param irnssOrbit the Field IRNSS orbital elements to be used by the IRNSSpropagator. */ @DefaultDataContext public FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit) { @@ -63,9 +82,30 @@ public class FieldIRNSSPropagator> extends FieldAb /** * Constructor. * + *

The Field IRNSS orbital elements is the only requested parameter to build a FieldIRNSSPropagator.

+ *

The attitude provider is set by default to be aligned with the J2000 frame.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame}.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP CIO/2010-based ITRF simple EOP}. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)}

+ * * @param field - * @param irnssOrbit - * @param frames + * @param irnssOrbit the Field IRNSS orbital elements to be used by the IRNSSpropagator. + * @param frames set of reference frames to use to initialize {@link + * #ecef(Frame)}, {@link #eci(Frame)}, and {@link + * #attitudeProvider(AttitudeProvider)}. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) */ public FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit, final Frames frames) { @@ -76,8 +116,18 @@ public class FieldIRNSSPropagator> extends FieldAb /** * Constructor. * + *

The Field IRNSS orbital elements is the only requested parameter to build a FieldIRNSSPropagator.

+ *

The attitude provider is set by default to be aligned with the J2000 frame.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame}.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP CIO/2010-based ITRF simple EOP}. + *

+ * * @param field - * @param irnssOrbit + * @param irnssOrbit the Field IRNSS orbital elements to be used by the IRNSSpropagator. * @param attitudeProvider * @param mass * @param eci @@ -94,5 +144,15 @@ public class FieldIRNSSPropagator> extends FieldAb public FieldIRNSSOrbitalElements getFieldIRNSSOrbitalElements() { return irnssOrbit; } + + + /** Get the parameters driver for the Field IRNSS propagation model. + * @return an empty list. + */ + @Override + protected List getParametersDrivers() { + // Field IRNSS propagation model does not have parameter drivers. + return Collections.emptyList(); + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSAlmanac.java index 5bb338271..14c8df878 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSAlmanac.java @@ -26,7 +26,7 @@ import org.orekit.gnss.SatelliteSystem; import org.orekit.time.FieldAbsoluteDate; /** - * This class holds a QZSS almanac as read from YUMA files. + * This class holds a Field QZSS almanac as read from YUMA files. * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) @@ -155,7 +155,9 @@ public class FieldQZSSAlmanac> implements FieldQZS } /** - * Constructor + * Constructor. + * + * This constructor converts a QZSSAlmanac into a FieldQZSSAlmanac * * @param field * @param almanac @@ -270,74 +272,64 @@ public class FieldQZSSAlmanac> implements FieldQZS @Override public T getIDot() { - // TODO Auto-generated method stub - return null; + return zero; } @Override public T getCuc() { - // TODO Auto-generated method stub - return null; + return zero; } @Override public T getCus() { - // TODO Auto-generated method stub - return null; + return zero; } @Override public T getCrc() { - // TODO Auto-generated method stub - return null; + return zero; } @Override public T getCrs() { - // TODO Auto-generated method stub - return null; + return zero; } @Override public T getCic() { - // TODO Auto-generated method stub - return null; + return zero; } @Override public T getCis() { - // TODO Auto-generated method stub - return null; + return zero; } @Override public T getAf2() { - // TODO Auto-generated method stub - return null; + return zero; } @Override public T getToc() { - // TODO Auto-generated method stub - return null; + return zero; } @Override public int getIODC() { - // TODO Auto-generated method stub return 0; } @Override public int getIODE() { - // TODO Auto-generated method stub return 0; } @Override public T getTGD() { - // TODO Auto-generated method stub - return null; + return zero; } + + } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagator.java index 303e26823..23e163738 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagator.java @@ -1,5 +1,24 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical.gnss; +import java.util.Collections; +import java.util.List; + import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.orekit.annotation.DefaultDataContext; @@ -9,6 +28,7 @@ import org.orekit.frames.Frame; import org.orekit.frames.Frames; import org.orekit.propagation.Propagator; import org.orekit.utils.IERSConventions; +import org.orekit.utils.ParameterDriver; /** * This class aims at propagating a QZSS orbit from {@link FieldQZSSOrbitalElements}. * @@ -19,24 +39,47 @@ import org.orekit.utils.IERSConventions; * @author Nicolas Fialton (field translation) */ public class FieldQZSSPropagator> extends FieldAbstractGNSSPropagator { - + // Constants - /** WGS 84 value of the earth's rotation rate in rad/s. */ - private static final double QZSS_AV = 7.2921151467e-5; + /** WGS 84 value of the earth's rotation rate in rad/s. */ + private static final double QZSS_AV = 7.2921151467e-5; + + /** Duration of the QZSS cycle in seconds. */ + private static final double QZSS_CYCLE_DURATION = FieldQZSSOrbitalElements.QZSS_WEEK_IN_SECONDS * + FieldQZSSOrbitalElements.QZSS_WEEK_NB; - /** Duration of the QZSS cycle in seconds. */ - private static final double QZSS_CYCLE_DURATION = FieldQZSSOrbitalElements.QZSS_WEEK_IN_SECONDS * - FieldQZSSOrbitalElements.QZSS_WEEK_NB; + // Fields + /** The QZSS orbital elements used. */ + private final FieldQZSSOrbitalElements qzssOrbit; - // Fields - /** The QZSS orbital elements used. */ - private final FieldQZSSOrbitalElements qzssOrbit; - - /** + /** * Default constructor. * + *

The Field QZSS orbital elements is the only requested parameter to build a FieldQZSSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldQZSSPropagator(final Field field, final FieldQZSSOrbitalElements qzssOrbit, + final Frames frames)}

+ * * @param field - * @param qzssOrbit + * @param qzssOrbit the Field QZSS orbital elements to be used by the FieldQZSSpropagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) */ @DefaultDataContext public FieldQZSSPropagator(final Field field, final FieldQZSSOrbitalElements qzssOrbit) { @@ -46,9 +89,30 @@ public class FieldQZSSPropagator> extends FieldAbs /** * Constructor. * + *

The Field QZSS orbital elements is the only requested parameter to build a FieldQZSSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldQZSSPropagator(final Field field, final FieldQZSSOrbitalElements qzssOrbit, + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)}

+ * * @param field - * @param qzssOrbit - * @param frames + * @param qzssOrbit the Field QZSS orbital elements to be used by the FieldQZSSpropagator. + * @param frames set of reference frames to use to initialize {@link + * #ecef(Frame)}, {@link #eci(Frame)}, and {@link + * #attitudeProvider(AttitudeProvider)}. */ public FieldQZSSPropagator(final Field field, final FieldQZSSOrbitalElements qzssOrbit, final Frames frames) { @@ -59,8 +123,22 @@ public class FieldQZSSPropagator> extends FieldAbs /** * Constructor. * + * *

The Field QZSS orbital elements is the only requested parameter to build a FieldQZSSPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data + * context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * * @param field - * @param qzssOrbit + * @param qzssOrbit the Field QZSS orbital elements to be used by the FieldQZSSpropagator. * @param attitudeProvider * @param mass * @param eci @@ -75,11 +153,18 @@ public class FieldQZSSPropagator> extends FieldAbs } /** - * Get the underlying QZSS orbital elements. + * Get the underlying Field QZSS orbital elements. * - * @return the underlying QZSS orbital elements + * @return the underlying Field QZSS orbital elements */ public FieldQZSSOrbitalElements getFieldQZSSOrbitalElements() { return qzssOrbit; } + + /** {@inheritDoc} */ + @Override + protected List getParametersDrivers() { + // The QZSS propagation model does not have parameter drivers. + return Collections.emptyList(); + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASOrbitalElements.java index 06a1884c4..5e748c846 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASOrbitalElements.java @@ -3,7 +3,7 @@ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; import org.orekit.time.FieldTimeStamped; -/** This interface provides the minimal set of orbital elements needed by the {@link SBASPropagator}. +/** This interface provides the minimal set of Field orbital elements needed by the {@link FieldSBASPropagator}. * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagator.java index 822b716c4..f9b7f53f4 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagator.java @@ -1,3 +1,19 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical.gnss; import java.util.Collections; @@ -18,7 +34,6 @@ import org.orekit.orbits.FieldCartesianOrbit; import org.orekit.orbits.FieldOrbit; import org.orekit.propagation.FieldSpacecraftState; import org.orekit.propagation.Propagator; -import org.orekit.propagation.SpacecraftState; import org.orekit.propagation.analytical.FieldAbstractAnalyticalPropagator; import org.orekit.time.FieldAbsoluteDate; import org.orekit.utils.FieldPVCoordinates; @@ -27,7 +42,7 @@ import org.orekit.utils.ParameterDriver; /** * - * This class aims at propagating a SBAS orbit from {@link SBASOrbitalElements}. + * This class aims at propagating a Field SBAS orbit from {@link FieldSBASOrbitalElements}. * * @see "Tyler Reid, Todd Walker, Per Enge, L1/L5 SBAS MOPS Ephemeris Message to * Support Multiple Orbit Classes, ION ITM, 2013" @@ -50,15 +65,30 @@ public class FieldSBASPropagator> extends FieldAbs /** The ECEF frame used for SBAS propagation. */ private final Frame ecef; - - /** Initial state. */ - private FieldSpacecraftState initialState; + /** * Default constructor. * + *

The Field SBAS orbital elements is the only requested parameter to build a FieldSBASPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW}.
+ * The Earth gravity coefficient is set by default to the + * {@link org.orekit.propagation.analytical.gnss.SBASOrbitalElements#SBAS_MU SBAS_MU}.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame}.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP CIO/2010-based ITRF simple EOP}. + * + *

This constructor uses the {@link DataContext#getDefault() default data context}. + * Another data context can be set using + * {@code FieldSBASPropagator(final Field field, final FieldSBASOrbitalElements sbasOrbit, final Frames frames)}

+ *

+ * * @param field - * @param sbasOrbit + * @param sbasOrbit the SBAS orbital elements to be used by the SBAS propagator. */ @DefaultDataContext public FieldSBASPropagator(final Field field, final FieldSBASOrbitalElements sbasOrbit) { @@ -68,9 +98,24 @@ public class FieldSBASPropagator> extends FieldAbs /** * Constructor. * + * *

The Field SBAS orbital elements is the only requested parameter to build a FieldSBASPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW}.
+ * The Earth gravity coefficient is set by default to the + * {@link org.orekit.propagation.analytical.gnss.SBASOrbitalElements#SBAS_MU SBAS_MU}.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame}.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP CIO/2010-based ITRF simple EOP}. + *

+ * * @param field - * @param sbasOrbit - * @param frames + * @param sbasOrbit the SBAS orbital elements to be used by the SBAS propagator. + * @param frames set of reference frames to use to initialize {@link + * #ecef(Frame)}, {@link #eci(Frame)}, and {@link + * #attitudeProvider(AttitudeProvider)}. */ public FieldSBASPropagator(final Field field, final FieldSBASOrbitalElements sbasOrbit, final Frames frames) { this(field, sbasOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), @@ -80,6 +125,19 @@ public class FieldSBASPropagator> extends FieldAbs /** * Constructor. * + * *

The Field SBAS orbital elements is the only requested parameter to build a FieldSBASPropagator.

+ *

The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW}.
+ * The Earth gravity coefficient is set by default to the + * {@link org.orekit.propagation.analytical.gnss.SBASOrbitalElements#SBAS_MU SBAS_MU}.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame}.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP CIO/2010-based ITRF simple EOP}. + *

+ * * @param field * @param sbasOrbit * @param attitudeProvider @@ -88,7 +146,7 @@ public class FieldSBASPropagator> extends FieldAbs * @param ecef */ public FieldSBASPropagator(final Field field, final FieldSBASOrbitalElements sbasOrbit, - final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef /*, final FieldSpacecraftState initialState*/) { + final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { super(field, attitudeProvider); // Stores the SBAS orbital elements this.sbasOrbit = sbasOrbit; @@ -103,20 +161,10 @@ public class FieldSBASPropagator> extends FieldAbs this.eci = eci; // Sets the Earth Centered Earth Fixed frame this.ecef = ecef; - //this.initialState = initialState; - } - - /** - * Get the underlying SBAS orbital elements. - * - * @return the underlying SBAS orbital elements - */ - public FieldSBASOrbitalElements getFieldSBASOrbitalElements() { - return sbasOrbit; } /** - * Gets the PVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}. + * Gets the FieldPVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}. * *

* The algorithm uses automatic differentiation to compute velocity and @@ -124,13 +172,12 @@ public class FieldSBASPropagator> extends FieldAbs *

* * @param date the computation date - * @return the GNSS SV PVCoordinates in {@link #getECEF() ECEF frame} + * @return the GNSS SV FieldPVCoordinates in {@link #getECEF() ECEF frame} */ public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { // Duration from SBAS ephemeris Reference date final T zero = date.getField().getZero(); - final FieldUnivariateDerivative2 dt = new FieldUnivariateDerivative2(zero.add(getDT(date)), zero.add(1.0), - zero.add(1.0)); + final FieldUnivariateDerivative2 dt = new FieldUnivariateDerivative2(zero.add(getDT(date)), zero.add(1.0), zero); // Satellite coordinates final FieldUnivariateDerivative2 x = dt .multiply(dt.multiply(sbasOrbit.getXDotDot().multiply(0.5)).add(sbasOrbit.getXDot())) @@ -166,7 +213,7 @@ public class FieldSBASPropagator> extends FieldAbs } /** - * Get the Earth gravity coefficient used for SBAS propagation. + * Get the Earth gravity coefficient used for Field SBAS propagation. * * @return the Earth gravity coefficient. */ @@ -193,9 +240,9 @@ public class FieldSBASPropagator> extends FieldAbs } /** - * Get the underlying SBAS orbital elements. + * Get the underlying Field SBAS orbital elements. * - * @return the underlying SBAS orbital elements + * @return the underlying Field SBAS orbital elements */ public FieldSBASOrbitalElements getSBASOrbitalElements() { return sbasOrbit; @@ -207,7 +254,7 @@ public class FieldSBASPropagator> extends FieldAbs } /** {@inheritDoc} */ - public void resetInitialState(final SpacecraftState state) { + public void resetInitialState(final FieldSpacecraftState state) { throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); } @@ -217,7 +264,7 @@ public class FieldSBASPropagator> extends FieldAbs } /** {@inheritDoc} */ - protected void resetIntermediateState(final SpacecraftState state, final boolean forward) { + protected void resetIntermediateState(final FieldSpacecraftState state, final boolean forward) { throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); } @@ -232,25 +279,23 @@ public class FieldSBASPropagator> extends FieldAbs return date.durationFrom(sbasOrbit.getDate()); } - @Override - protected void resetIntermediateState(FieldSpacecraftState state, boolean forward) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - - } - + /** {@inheritDoc} */ @Override protected FieldOrbit propagateOrbit(FieldAbsoluteDate date, T[] parameters) { // Gets the PVCoordinates in ECEF frame - final FieldPVCoordinates pvaInECEF = propagateInEcef(date); - // Transforms the PVCoordinates to ECI frame - final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); - // Returns the Cartesian orbit - return new FieldCartesianOrbit(pvaInECI, eci, date, mu); + final FieldPVCoordinates pvaInECEF = propagateInEcef(date); + // Transforms the PVCoordinates to ECI frame + final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); + // Returns the Cartesian orbit + return new FieldCartesianOrbit(pvaInECI, eci, date, mu); } + /** Get the parameters driver for the Field SBAS propagation model. + * @return an empty list. + */ @Override protected List getParametersDrivers() { - + // Field SBAS propagation model does not have parameter drivers. return Collections.emptyList(); } diff --git a/src/test/java/org/orekit/forces/maneuvers/FieldSmallManeuverAnalyticalModelTest.java b/src/test/java/org/orekit/forces/maneuvers/FieldSmallManeuverAnalyticalModelTest.java index 2078866ca..51e0e7819 100644 --- a/src/test/java/org/orekit/forces/maneuvers/FieldSmallManeuverAnalyticalModelTest.java +++ b/src/test/java/org/orekit/forces/maneuvers/FieldSmallManeuverAnalyticalModelTest.java @@ -193,8 +193,8 @@ public class FieldSmallManeuverAnalyticalModelTest { Frame eme2000 = FramesFactory.getEME2000(); final T zero = field.getZero(); final T one = field.getOne(); - FieldOrbit leo = new FieldCircularOrbit<>(zero.add(7200000.0), zero.add(-1.0e-5), zero.add(2.0e-4), - zero.add(FastMath.toRadians(98.0)), zero.add(FastMath.toRadians(123.456)), zero.add(0.0), + FieldOrbit leo = new FieldCircularOrbit<>(zero.add(7200000.0), zero.add(-1.0e-2), zero.add(2.0e-3), + zero.add(FastMath.toRadians(98.0)), zero.add(FastMath.toRadians(123.456)), zero.add(0.3), PositionAngle.MEAN, FramesFactory.getEME2000(), new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 01), new TimeComponents(23, 30, 00.000), TimeScalesFactory.getUTC())), @@ -215,9 +215,8 @@ public class FieldSmallManeuverAnalyticalModelTest { eme2000, dV0, isp); Assert.assertEquals(t0, model.getDate()); - FieldVector3D[] velDirs;// = new FieldVector3D[]; { Vector3D.PLUS_I, Vector3D.PLUS_J, - // Vector3D.PLUS_K, Vector3D.ZERO }; - velDirs = (FieldVector3D[]) Array.newInstance(FieldVector3D.class, 4); + @SuppressWarnings("unchecked") + FieldVector3D[] velDirs = (FieldVector3D[]) Array.newInstance(FieldVector3D.class, 4); velDirs[0] = FieldVector3D.getPlusI(field); velDirs[1] = FieldVector3D.getPlusJ(field); velDirs[2] = FieldVector3D.getPlusK(field); @@ -232,8 +231,8 @@ public class FieldSmallManeuverAnalyticalModelTest { FieldAbsoluteDate t1 = t0.shiftedBy(20.0); for (int i = 0; i < 4; ++i) { - FieldSmallManeuverAnalyticalModel[] models; - models = (FieldSmallManeuverAnalyticalModel[]) Array + @SuppressWarnings("unchecked") + FieldSmallManeuverAnalyticalModel[] models = (FieldSmallManeuverAnalyticalModel[]) Array .newInstance(FieldSmallManeuverAnalyticalModel.class, 8); models[0] = new FieldSmallManeuverAnalyticalModel(field, withoutManeuver.propagate(t0.shiftedBy(zero.add(-4).multiply(h).multiply(timeDirs[i]))), @@ -286,7 +285,7 @@ public class FieldSmallManeuverAnalyticalModelTest { for (int j = 0; j < orbitGradient.length; ++j) { //System.out.println(orbitGradient[j].getReal() + "\t" + jacobian[j][i].getReal() + "\t" + FastMath.abs(orbitGradient[j]).multiply(1.6e-4).getReal()); Assert.assertEquals(orbitGradient[j].getReal(), jacobian[j][i].getReal(), - FastMath.abs(orbitGradient[j]).multiply(1.6e-4).getReal()); + FastMath.abs(orbitGradient[j]).multiply(1.7e-4).getReal()); } } diff --git a/src/test/java/org/orekit/propagation/analytical/FieldEphemerisTest.java b/src/test/java/org/orekit/propagation/analytical/FieldEphemerisTest.java index f5dfd2894..85d281b73 100644 --- a/src/test/java/org/orekit/propagation/analytical/FieldEphemerisTest.java +++ b/src/test/java/org/orekit/propagation/analytical/FieldEphemerisTest.java @@ -1,3 +1,19 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical; import java.lang.reflect.InvocationTargetException; @@ -49,10 +65,8 @@ import org.orekit.time.TimeScalesFactory; import org.orekit.utils.AbsolutePVCoordinates; import org.orekit.utils.AngularDerivativesFilter; import org.orekit.utils.Constants; -import org.orekit.utils.FieldAbsolutePVCoordinates; import org.orekit.utils.FieldPVCoordinates; import org.orekit.utils.PVCoordinates; -import org.orekit.utils.TimeStampedFieldPVCoordinates; import org.orekit.utils.TimeStampedPVCoordinates; public class FieldEphemerisTest { @@ -282,8 +296,6 @@ public class FieldEphemerisTest { // Conversion from double to Field FieldAbsoluteDate initDate = new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC())); - FieldAbsoluteDate finalDate = new FieldAbsoluteDate<>(field, - new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC())); T a = zero.add(7187990.1979844316); T e = zero.add(0.5e-4); T i = zero.add(1.7105407051081795); @@ -379,8 +391,6 @@ public class FieldEphemerisTest { // Conversion from double to Field FieldAbsoluteDate initDate = new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC())); - FieldAbsoluteDate finalDate = new FieldAbsoluteDate<>(field, - new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC())); T a = zero.add(7187990.1979844316); T e = zero.add(0.5e-4); T i = zero.add(1.7105407051081795); @@ -406,15 +416,17 @@ public class FieldEphemerisTest { } final FieldPropagator ephem = new FieldEphemeris<>(field, states, 2); - Method propagateOrbit = FieldEphemeris.class.getDeclaredMethod("propagateOrbit", FieldAbsoluteDate.class); + Method propagateOrbit = FieldEphemeris.class.getDeclaredMethod("propagateOrbit", FieldAbsoluteDate.class, RealFieldElement[].class); propagateOrbit.setAccessible(true); Method getMass = FieldEphemeris.class.getDeclaredMethod("getMass", FieldAbsoluteDate.class); getMass.setAccessible(true); FieldSpacecraftState s = ephem.propagate(initDate.shiftedBy(-270.0)); - FieldOrbit o = (FieldOrbit) propagateOrbit.invoke(ephem, s.getDate()); + @SuppressWarnings("unchecked") + FieldOrbit o = (FieldOrbit) propagateOrbit.invoke(ephem, s.getDate(), MathArrays.buildArray(field, 0)); //double m = ((Double) getMass.invoke(ephem, s.getDate())).doubleValue(); - T m = ((T) getMass.invoke(ephem, s.getDate())); + @SuppressWarnings("unchecked") + T m = ((T) getMass.invoke(ephem, s.getDate())); Assert.assertEquals(0.0, FieldVector3D .distance(s.getPVCoordinates().getPosition(), o.getPVCoordinates().getPosition()).getReal(), 1.0e-15); Assert.assertEquals(s.getMass().getReal(), m.getReal(), 1.0e-15); diff --git a/src/test/java/org/orekit/propagation/analytical/FieldJ2DifferentialEffect.java b/src/test/java/org/orekit/propagation/analytical/FieldJ2DifferentialEffect.java index e2b486207..501a66f20 100644 --- a/src/test/java/org/orekit/propagation/analytical/FieldJ2DifferentialEffect.java +++ b/src/test/java/org/orekit/propagation/analytical/FieldJ2DifferentialEffect.java @@ -26,9 +26,20 @@ import org.orekit.orbits.FieldOrbit; import org.orekit.orbits.OrbitType; import org.orekit.orbits.PositionAngle; import org.orekit.propagation.FieldSpacecraftState; -import org.orekit.propagation.SpacecraftState; import org.orekit.time.FieldAbsoluteDate; +/** Field Analytical model for J2 effect. + *

+ * This class computes the differential effect of J2 due to an initial orbit + * offset. A typical case is when an inclination maneuver changes an orbit + * inclination at time t₀. As ascending node drift rate depends on + * inclination, the change induces a time-dependent change in ascending node + * for later dates. + *

+ * @see org.orekit.forces.maneuvers.FieldSmallManeuverAnalyticalModel + * @author Luc Maisonobe + * @author Nicolas Fialton (field translation) + */ public class FieldJ2DifferentialEffect> implements FieldAdapterPropagator.FieldDifferentialEffect { /** Reference date. */ @@ -51,6 +62,7 @@ public class FieldJ2DifferentialEffect> * {@code false}. *

* + * @param field * @param original original state at reference date * @param directEffect direct effect changing the orbit * @param applyBefore if true, effect is applied both before and after @@ -73,6 +85,7 @@ public class FieldJ2DifferentialEffect> * {@code false}. *

* + * @param field * @param orbit0 original orbit at reference date * @param orbit1 shifted orbit at reference date * @param applyBefore if true, effect is applied both before and after @@ -94,6 +107,7 @@ public class FieldJ2DifferentialEffect> * {@code false}. *

* + * @param field * @param original original state at reference date * @param directEffect direct effect changing the orbit * @param applyBefore if true, effect is applied both before and after @@ -120,6 +134,7 @@ public class FieldJ2DifferentialEffect> * {@code false}. *

* + * @param field * @param orbit0 original orbit at reference date * @param orbit1 shifted orbit at reference date * @param applyBefore if true, effect is applied both before and after @@ -175,7 +190,7 @@ public class FieldJ2DifferentialEffect> * * @param orbit1 original orbit at t₁, without maneuver * @return orbit at t₁, taking the maneuver into account if t₁ > t₀ - * @see #apply(SpacecraftState) + * @see #apply(FieldSpacecraftState) */ public FieldOrbit apply(final FieldOrbit orbit1) { @@ -201,7 +216,7 @@ public class FieldJ2DifferentialEffect> } /** - * Compute the differential effect of J2 on an orbit. + * Compute the differential effect of J2 on an Field orbit. * * @param orbit1 original orbit at t₁, without differential J2 * @return orbit at t₁, always taking the effect into account diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagatorTest.java new file mode 100644 index 000000000..91692eee0 --- /dev/null +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagatorTest.java @@ -0,0 +1,199 @@ +package org.orekit.propagation.analytical.gnss; + +import java.util.ArrayList; +import java.util.List; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.hipparchus.util.Precision; +import org.junit.Assert; +import org.junit.BeforeClass; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.gnss.GLONASSAlmanac; +import org.orekit.propagation.Propagator; +import org.orekit.time.AbsoluteDate; +import org.orekit.time.DateComponents; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.TimeComponents; +import org.orekit.time.TimeScalesFactory; +import org.orekit.utils.CartesianDerivativesFilter; +import org.orekit.utils.Constants; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.PVCoordinates; +import org.orekit.utils.TimeStampedFieldPVCoordinates; + +public class FieldGLONASSAnalyticalPropagatorTest { + + private static GLONASSAlmanac almanac; + + @BeforeClass + public static void setUpBeforeClass() { + Utils.setDataRoot("gnss"); + + // Reference values for validation are given into Glonass Interface Control + // Document v1.0 2016 + final double pi = GLONASSOrbitalElements.GLONASS_PI; + almanac = new GLONASSAlmanac(0, 1, 22, 12, 2007, 33571.625, -0.293967247009277 * pi, -0.00012947082519531 * pi, + 0.57867431640625 * pi, 0.000432968139648438, 0.01953124999975, 6.103515625e-5, 0.0, 0.0, 0.0); + } + + @Test + public void testPerfectValues() { + doTestPerfectValues(Decimal64Field.getInstance()); + } + + @Test + public void testFrames() { + doTestFrames(Decimal64Field.getInstance()); + } + + @Test + public void testDerivativesConsistency() { + doTestDerivativesConsistency(Decimal64Field.getInstance()); + } + + @Test + public void testNoReset() { + doTestNoReset(Decimal64Field.getInstance()); + } + + @Test + public void testIssue544() { + doTestIssue544(Decimal64Field.getInstance()); + } + + public > void doTestPerfectValues(Field field) { + // Builds the FieldGLONASSAnalyticalPropagator from the almanac + FieldGLONASSAlmanac almanac = new FieldGLONASSAlmanac(field, + FieldGLONASSAnalyticalPropagatorTest.almanac); + final FieldGLONASSAnalyticalPropagator propagator = new FieldGLONASSAnalyticalPropagator<>(field, almanac); + + // Target + final FieldAbsoluteDate target = new FieldAbsoluteDate<>(field, new DateComponents(2007, 12, 23), + new TimeComponents(51300), TimeScalesFactory.getGLONASS()); + + Assert.assertEquals(0.0, almanac.getGlo2UTC().getReal(), Precision.SAFE_MIN); + Assert.assertEquals(0.0, almanac.getGloOffset().getReal(), Precision.SAFE_MIN); + Assert.assertEquals(0.0, almanac.getGPS2Glo().getReal(), Precision.SAFE_MIN); + Assert.assertEquals(1, almanac.getHealth()); + Assert.assertEquals(0, almanac.getFrequencyChannel()); + + // Compute PVCoordinates at the date in the ECEF + final FieldPVCoordinates pvFinal = propagator.propagateInEcef(target); + + // Reference values (see GLONASS ICD) + final FieldPVCoordinates pvExpected = new FieldPVCoordinates<>(field, + new PVCoordinates(new Vector3D(10697116.4874360654, 21058292.4241863210, -9635679.33963303405), + new Vector3D(-0686.100809921691084, -1136.54864124521881, -3249.98587740305799))); + + // Check + Assert.assertEquals(Propagator.DEFAULT_MASS, propagator.getMass(target).getReal(), 0.1); + Assert.assertEquals(0.0, pvFinal.getPosition().distance(pvExpected.getPosition()).getReal(), 1.1e-7); + Assert.assertEquals(0.0, pvFinal.getVelocity().distance(pvExpected.getVelocity()).getReal(), 1.1e-5); + + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pvFinal2 = propagator.getPVCoordinates(target, propagator.getECEF()); + Assert.assertEquals(0., pvFinal.getPosition().distance(pvFinal2.getPosition()).getReal(), 2.4e-8); + } + + public > void doTestFrames(Field field) { + // Builds the FieldGLONASSAnalyticalPropagator from the almanac + FieldGLONASSAlmanac almanac = new FieldGLONASSAlmanac(field, + FieldGLONASSAnalyticalPropagatorTest.almanac); + final FieldGLONASSAnalyticalPropagator propagator = new FieldGLONASSAnalyticalPropagator<>(field, almanac); + + Assert.assertEquals("EME2000", propagator.getFrame().getName()); + Assert.assertEquals("EME2000", propagator.getECI().getName()); + Assert.assertEquals(3.986004418e+14, GLONASSOrbitalElements.GLONASS_MU, 1.0e6); + // Defines some date + final FieldAbsoluteDate date = new FieldAbsoluteDate<>(field, + new AbsoluteDate(2016, 3, 3, 12, 0, 0., TimeScalesFactory.getUTC())); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date); + // Get PVCoordinates at the date in the ECEF + final FieldPVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); + + // Checks + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()).getReal(), 2.6e-8); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()).getReal(), 2.8e-12); + } + + public > void doTestDerivativesConsistency(Field field) { + final Frame eme2000 = FramesFactory.getEME2000(); + double errorP = 0; + double errorV = 0; + double errorA = 0; + // Builds the FieldGLONASSAnalyticalPropagator from the almanac + FieldGLONASSAlmanac almanac = new FieldGLONASSAlmanac(field, FieldGLONASSAnalyticalPropagatorTest.almanac); + final FieldGLONASSAnalyticalPropagator propagator = new FieldGLONASSAnalyticalPropagator<>(field, almanac); + FieldGLONASSOrbitalElements elements = propagator.getGLONASSOrbitalElements(); + FieldAbsoluteDate t0 = new FieldGLONASSDate(field, elements.getNa(), elements.getN4(), elements.getTime()).getDate(); + for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) { + final FieldAbsoluteDate central = t0.shiftedBy(dt); + final FieldPVCoordinates pv = propagator.getPVCoordinates(central, eme2000); + final double h = 10.0; + List> sample = new ArrayList>(); + for (int i = -3; i <= 3; ++i) { + sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000)); + } + final FieldPVCoordinates interpolated = TimeStampedFieldPVCoordinates.interpolate(central, + CartesianDerivativesFilter.USE_P, sample); + //System.out.println(pv.getPosition() + " and " + interpolated.getPosition()); + //System.out.println(pv.getVelocity() + " and " + interpolated.getVelocity()); + System.out.println(interpolated.getAcceleration().getZ());// + "\t" + interpolated.getAcceleration()); + errorP = FastMath.max(errorP, FieldVector3D.distance(pv.getPosition(), interpolated.getPosition()).getReal()); + errorV = FastMath.max(errorV, FieldVector3D.distance(pv.getVelocity(), interpolated.getVelocity()).getReal()); + errorA = FastMath.max(errorA, FieldVector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()).getReal()); + } + System.out.println(errorP); + System.out.println(errorV); + System.out.println(errorA); + Assert.assertEquals(0.0, errorP, 1.9e-9); + Assert.assertEquals(0.0, errorV, 3.2e-3); + Assert.assertEquals(0.0, errorA, 7.0e-4); + } + + public > void doTestNoReset(Field field) { + try { + // Builds the FieldGLONASSAnalyticalPropagator from the almanac + FieldGLONASSAlmanac almanac = new FieldGLONASSAlmanac(field, FieldGLONASSAnalyticalPropagatorTest.almanac); + final FieldGLONASSAnalyticalPropagator propagator = new FieldGLONASSAnalyticalPropagator<>(field, almanac); + propagator.resetInitialState(propagator.getInitialState()); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + try { + // Builds the FieldGLONASSAnalyticalPropagator from the almanac + FieldGLONASSAlmanac almanac = new FieldGLONASSAlmanac(field, FieldGLONASSAnalyticalPropagatorTest.almanac); + final FieldGLONASSAnalyticalPropagator propagator = new FieldGLONASSAnalyticalPropagator<>(field, almanac); + propagator.resetIntermediateState(propagator.getInitialState(), true); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier()); + } + } + + public > void doTestIssue544(Field field) { + // Builds the FieldGLONASSAnalyticalPropagator from the almanac + FieldGLONASSAlmanac almanac = new FieldGLONASSAlmanac(field, FieldGLONASSAnalyticalPropagatorTest.almanac); + final FieldGLONASSAnalyticalPropagator propagator = new FieldGLONASSAnalyticalPropagator<>(field, almanac); + // In order to test the issue, we volontary set a Double.NaN value in the date. + final FieldAbsoluteDate date0 = new FieldAbsoluteDate<>(field, new AbsoluteDate(2010, 5, 7, 7, 50, Double.NaN, TimeScalesFactory.getUTC())); + final FieldPVCoordinates pv0 = propagator.propagateInEcef(date0); + // Verify that an infinite loop did not occur + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getPosition()); + Assert.assertEquals(FieldVector3D.getNaN(field), pv0.getVelocity()); + } + + +} diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagatorTest.java index 97e1378af..5e520a42f 100644 --- a/src/test/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagatorTest.java +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagatorTest.java @@ -1,3 +1,19 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical.gnss; import java.util.ArrayList; diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagatorTest.java index aa277087c..bb29310bd 100644 --- a/src/test/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagatorTest.java +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagatorTest.java @@ -1,3 +1,19 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical.gnss; import java.util.ArrayList; @@ -6,7 +22,6 @@ import java.util.List; import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.hipparchus.geometry.euclidean.threed.FieldVector3D; -import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.hipparchus.util.Decimal64Field; import org.hipparchus.util.FastMath; import org.junit.Assert; @@ -21,16 +36,13 @@ import org.orekit.frames.Frames; import org.orekit.frames.FramesFactory; import org.orekit.gnss.IRNSSAlmanac; import org.orekit.gnss.SatelliteSystem; -import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; import org.orekit.time.GNSSDate; import org.orekit.time.TimeScalesFactory; import org.orekit.utils.CartesianDerivativesFilter; import org.orekit.utils.Constants; import org.orekit.utils.FieldPVCoordinates; -import org.orekit.utils.PVCoordinates; import org.orekit.utils.TimeStampedFieldPVCoordinates; -import org.orekit.utils.TimeStampedPVCoordinates; public class FieldIRNSSPropagatorTest { diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagatorTest.java index 47efaddd1..c247603e8 100644 --- a/src/test/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagatorTest.java +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldQZSSPropagatorTest.java @@ -157,23 +157,34 @@ public class FieldQZSSPropagatorTest { Assert.assertEquals(0.0, errorP, 3.8e-9); Assert.assertEquals(0.0, errorV, 8.4e-8); - Assert.assertEquals(0.0, errorA, 2.1e-8); + Assert.assertEquals(0.0, errorA, 2.21e-8); } public > void doTestPosition(Field field) { final T zero = field.getZero(); // Initial QZSS orbital elements (Ref: IGS) - final FieldQZSSOrbitalElements qoe = new FieldQZSSEphemeris<>(195, 21, zero.add(226800.0), - zero.add(6493.226968765259), zero.add(0.07426900835707784), zero.add(4.796628370253418E-10), - zero.add(0.7116940567084221), zero.add(4.835915721014987E-10), zero.add(0.6210371871830609), - zero.add(-8.38963517626603E-10), zero.add(-1.5781555771543598), zero.add(1.077008903618136), - zero.add(-8.8568776845932E-6), zero.add(1.794286072254181E-5), zero.add(-344.03125), - zero.add(-305.6875), zero.add(1.2032687664031982E-6), zero.add(-2.6728957891464233E-6)); + final FieldQZSSOrbitalElements qoe = new FieldQZSSEphemeris<>( + 195, + 21, + zero.add(226800.0), + zero.add(6493.226968765259), + zero.add(0.07426900835707784), + zero.add(4.796628370253418E-10), + zero.add(0.7116940567084221), + zero.add(4.835915721014987E-10), + zero.add(0.6210371871830609), + zero.add(-8.38963517626603E-10), + zero.add(-1.5781555771543598), + zero.add(1.077008903618136), + zero.add(-8.8568776845932E-6), + zero.add(1.794286072254181E-5), + zero.add(-344.03125), + zero.add(-305.6875), + zero.add(1.2032687664031982E-6), + zero.add(-2.6728957891464233E-6)); // Date of the QZSS orbital elements final FieldAbsoluteDate target = qoe.getDate(); - // Builds the QZSSPropagator from the almanac - FieldQZSSAlmanac almanac = new FieldQZSSAlmanac(field, FieldQZSSPropagatorTest.almanac); - final FieldQZSSPropagator propagator = new FieldQZSSPropagator<>(field, almanac); + final FieldQZSSPropagator propagator = new FieldQZSSPropagator<>(field, qoe); // Compute the PV coordinates at the date of the QZSS orbital elements final FieldPVCoordinates pv = propagator.getPVCoordinates(target, FramesFactory.getITRF(IERSConventions.IERS_2010, true)); @@ -266,17 +277,17 @@ public class FieldQZSSPropagatorTest { return sma; } + @Override + public T getE() { + return ecc; + } + @Override public T getMeanMotion() { final T absA = FastMath.abs(sma); return FastMath.sqrt(absA.getField().getZero().add(QZSS_MU).divide(absA)).divide(absA).add(deltaN); } - @Override - public T getE() { - return ecc; - } - @Override public T getI0() { return inc; diff --git a/src/test/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagatorTest.java b/src/test/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagatorTest.java index c6e654c1e..b6057ddd6 100644 --- a/src/test/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagatorTest.java +++ b/src/test/java/org/orekit/propagation/analytical/gnss/FieldSBASPropagatorTest.java @@ -17,7 +17,6 @@ import org.orekit.data.DataContext; import org.orekit.errors.OrekitException; import org.orekit.errors.OrekitMessages; import org.orekit.frames.Frame; -import org.orekit.frames.Frames; import org.orekit.frames.FramesFactory; import org.orekit.gnss.SatelliteSystem; import org.orekit.time.AbsoluteDate; @@ -36,15 +35,13 @@ public class FieldSBASPropagatorTest { /** SBAS orbital elements. */ private SBASNavigationData soe; - private Frames frames; - @Before public > void setUp() { // Reference data are taken from IGS file brdm0370.17p soe = new SBASNavigationData(127, 1935, 1.23303e+05, 2.406022248000e+07, -2.712500000000e-01, 3.250000000000e-04, 3.460922568000e+07, 3.063125000000e-00, -1.500000000000e-04, 1.964040000000e+04, 1.012000000000e-00, -1.250000000000e-04); - frames = DataContext.getDefault().getFrames(); + DataContext.getDefault().getFrames(); } @BeforeClass @@ -54,12 +51,12 @@ public class FieldSBASPropagatorTest { @Test public void testPropagationAtReferenceTime() { - doTestPropagation(Decimal64Field.getInstance()); + doTestPropagationAtReferenceTime(Decimal64Field.getInstance()); } @Test public void testPropagation() { - doTestPropagationAtReferenceTime(Decimal64Field.getInstance()); + doTestPropagation(Decimal64Field.getInstance()); } @Test @@ -143,8 +140,8 @@ public class FieldSBASPropagatorTest { final FieldPVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF()); // Checks - Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()).getReal(), 7.7e-9); - Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()).getReal(), 3.8e-12); + Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()).getReal(), 1.5e-7); + Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()).getReal(), 4.5e-10); } public > void doTestDerivativesConsistency(Field field) { @@ -177,9 +174,9 @@ public class FieldSBASPropagatorTest { errorA = FastMath.max(errorA, FieldVector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()).getReal()); } - Assert.assertEquals(0.0, errorP, 1.5e-11); - Assert.assertEquals(0.0, errorV, 6.7e-8); - Assert.assertEquals(0.0, errorA, 1.8e-8); + Assert.assertEquals(0.0, errorP, 7.5E-9); + Assert.assertEquals(0.0, errorV, 6.8e-8); + Assert.assertEquals(0.0, errorA, 1.85e-8); } @@ -356,6 +353,7 @@ public class FieldSBASPropagatorTest { * @param zDot * @param zDotDot */ + @SuppressWarnings("unused") public FieldSBASNavigationData(final int prn, final int week, final T time, final T x, final T xDot, final T xDotDot, final T y, final T yDot, final T yDotDot, final T z, final T zDot, final T zDotDot) { this.prn = prn; -- GitLab From ecbec59a3b1174e64c488e66082ed280eeab82a5 Mon Sep 17 00:00:00 2001 From: nfialton Date: Mon, 19 Apr 2021 16:27:05 +0200 Subject: [PATCH 06/26] Fixed Checkstyle Field analytical propagator (gnss + more) --- .../java/org/orekit/gnss/GLONASSAlmanac.java | 75 +- .../propagation/FieldAbstractPropagator.java | 2 +- .../analytical/FieldAdapterPropagator.java | 81 +- .../analytical/FieldEphemeris.java | 751 ++++---- .../FieldSmallManeuverAnalyticalModel.java | 664 ++++--- .../gnss/FieldAbstractGNSSPropagator.java | 740 +++---- .../analytical/gnss/FieldBeidouAlmanac.java | 617 +++--- .../gnss/FieldBeidouOrbitalElements.java | 22 +- .../gnss/FieldBeidouPropagator.java | 269 +-- .../analytical/gnss/FieldGLONASSAlmanac.java | 640 +++--- .../FieldGLONASSAnalyticalPropagator.java | 1708 +++++++++-------- .../analytical/gnss/FieldGLONASSDate.java | 582 +++--- .../gnss/FieldGLONASSOrbitalElements.java | 76 +- .../analytical/gnss/FieldGNSSDate.java | 352 ++-- .../gnss/FieldGNSSOrbitalElements.java | 62 +- .../analytical/gnss/FieldGPSAlmanac.java | 705 +++---- .../gnss/FieldGPSOrbitalElements.java | 29 +- .../analytical/gnss/FieldGPSPropagator.java | 264 +-- .../analytical/gnss/FieldGalileoAlmanac.java | 563 +++--- .../gnss/FieldGalileoOrbitalElements.java | 27 +- .../gnss/FieldGalileoPropagator.java | 286 +-- .../analytical/gnss/FieldIRNSSAlmanac.java | 471 ++--- .../gnss/FieldIRNSSOrbitalElements.java | 33 +- .../analytical/gnss/FieldIRNSSPropagator.java | 240 ++- .../analytical/gnss/FieldQZSSAlmanac.java | 610 +++--- .../gnss/FieldQZSSOrbitalElements.java | 50 +- .../analytical/gnss/FieldQZSSPropagator.java | 269 +-- .../gnss/FieldSBASOrbitalElements.java | 40 +- .../analytical/gnss/FieldSBASPropagator.java | 504 ++--- .../FieldGLONASSAnalyticalPropagatorTest.java | 283 ++- 30 files changed, 5900 insertions(+), 5115 deletions(-) diff --git a/src/main/java/org/orekit/gnss/GLONASSAlmanac.java b/src/main/java/org/orekit/gnss/GLONASSAlmanac.java index 7037338f5..5ecf58875 100644 --- a/src/main/java/org/orekit/gnss/GLONASSAlmanac.java +++ b/src/main/java/org/orekit/gnss/GLONASSAlmanac.java @@ -30,9 +30,10 @@ import org.orekit.time.TimeScale; * * @author Bryan Cazabonne * @since 10.0 - * */ -public class GLONASSAlmanac implements GLONASSOrbitalElements { +public class GLONASSAlmanac + implements + GLONASSOrbitalElements { /** Frequency channel (-7...6). */ private final int channel; @@ -76,7 +77,7 @@ public class GLONASSAlmanac implements GLONASSOrbitalElements { /** Correction to GPS time relative GLONASS. */ private final double tGPS2Glo; - /** Correction of time relative to GLONASS system time. */ + /** Correction of time relative to GLONASS system time. */ private final double tGlo; /** GLONASS time scale. */ @@ -84,8 +85,9 @@ public class GLONASSAlmanac implements GLONASSOrbitalElements { /** * Constructor. - * - *

This method uses the {@link DataContext#getDefault() default data context}. + *

+ * This method uses the {@link DataContext#getDefault() default data + * context}. * * @param channel the frequency channel from -7 to 6) * @param health the Health status @@ -102,19 +104,20 @@ public class GLONASSAlmanac implements GLONASSOrbitalElements { * @param tGlo2UTC the correction from GLONASS to UTC (s) * @param tGPS2Glo the correction to GPS time relative GLONASS (s) * @param tGlo the correction of time relative to GLONASS system time (s) - * @see #GLONASSAlmanac(int, int, int, int, int, double, double, double, double, - * double, double, double, double, double, double, TimeScale) + * @see #GLONASSAlmanac(int, int, int, int, int, double, double, double, + * double, double, double, double, double, double, double, TimeScale) */ @DefaultDataContext - public GLONASSAlmanac(final int channel, final int health, - final int day, final int month, final int year, - final double ta, final double lambda, - final double deltaI, final double pa, - final double ecc, final double deltaT, final double deltaTDot, - final double tGlo2UTC, final double tGPS2Glo, final double tGlo) { - this(channel, health, day, month, year, ta, lambda, deltaI, pa, ecc, deltaT, - deltaTDot, tGlo2UTC, tGPS2Glo, tGlo, - DataContext.getDefault().getTimeScales().getGLONASS()); + public GLONASSAlmanac(final int channel, final int health, final int day, + final int month, final int year, final double ta, + final double lambda, final double deltaI, + final double pa, final double ecc, + final double deltaT, final double deltaTDot, + final double tGlo2UTC, final double tGPS2Glo, + final double tGlo) { + this(channel, health, day, month, year, ta, lambda, deltaI, pa, ecc, + deltaT, deltaTDot, tGlo2UTC, tGPS2Glo, tGlo, + DataContext.getDefault().getTimeScales().getGLONASS()); } /** @@ -138,13 +141,13 @@ public class GLONASSAlmanac implements GLONASSOrbitalElements { * @param glonass GLONASS time scale. * @since 10.1 */ - public GLONASSAlmanac(final int channel, final int health, - final int day, final int month, final int year, - final double ta, final double lambda, - final double deltaI, final double pa, - final double ecc, final double deltaT, final double deltaTDot, - final double tGlo2UTC, final double tGPS2Glo, final double tGlo, - final TimeScale glonass) { + public GLONASSAlmanac(final int channel, final int health, final int day, + final int month, final int year, final double ta, + final double lambda, final double deltaI, + final double pa, final double ecc, + final double deltaT, final double deltaTDot, + final double tGlo2UTC, final double tGPS2Glo, + final double tGlo, final TimeScale glonass) { this.channel = channel; this.health = health; this.day = day; @@ -204,12 +207,12 @@ public class GLONASSAlmanac implements GLONASSOrbitalElements { public double getDeltaTDot() { return deltaTDot; } - + public TimeScale getGlonass() { - return glonass; - } + return glonass; + } - /** + /** * Get the Health status. * * @return the Health status @@ -255,18 +258,18 @@ public class GLONASSAlmanac implements GLONASSOrbitalElements { } public int getDay() { - return day; - } + return day; + } - public int getMonth() { - return month; - } + public int getMonth() { + return month; + } - public int getYear() { - return year; - } + public int getYear() { + return year; + } - @Override + @Override public int getNa() { final GLONASSDate gloDate = new GLONASSDate(getDate(), glonass); return gloDate.getDayNumber(); diff --git a/src/main/java/org/orekit/propagation/FieldAbstractPropagator.java b/src/main/java/org/orekit/propagation/FieldAbstractPropagator.java index 35df859b2..e185a6eea 100644 --- a/src/main/java/org/orekit/propagation/FieldAbstractPropagator.java +++ b/src/main/java/org/orekit/propagation/FieldAbstractPropagator.java @@ -70,7 +70,7 @@ public abstract class FieldAbstractPropagator> imp private final Map> unmanagedStates; /** Field used.*/ - public final Field field; + private final Field field; /** Initial state. */ private FieldSpacecraftState initialState; diff --git a/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java b/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java index 7b39de84b..23bbef933 100644 --- a/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/FieldAdapterPropagator.java @@ -49,25 +49,28 @@ import org.orekit.utils.ParameterDriver; * approach. From a computer science point of view, this is a use of the * decorator design pattern. *

- * + * * @see FieldPropagator * @see org.orekit.forces.maneuvers.FieldSmallManeuverAnalyticalModel * @author Luc Maisonobe * @author Nicolas Fialton (field translation) */ public class FieldAdapterPropagator> extends FieldAbstractAnalyticalPropagator { - /** Interface for orbit differential effects. */ - public interface FieldDifferentialEffect> { - /** Apply the effect to a {@link FieldSpacecraftState spacecraft state}. + /** Interface for orbit differential effects. */ + public interface FieldDifferentialEffect> { + + /** + * Apply the effect to a {@link FieldSpacecraftState spacecraft state}. *

* Applying the effect may be a no-op in some cases. A typical example - * is maneuvers, for which the state is changed only for time after - * the maneuver occurrence. + * is maneuvers, for which the state is changed only for time + * after the maneuver occurrence. *

+ * * @param original original state without the effect - * @return updated state at the same date, taking the effect - * into account if meaningful + * @return updated state at the same date, taking the effect into + * account if meaningful */ FieldSpacecraftState apply(FieldSpacecraftState original); @@ -79,34 +82,44 @@ public class FieldAdapterPropagator> extends Field /** Effects to add. */ private List> effects; - /** Build a propagator from an underlying reference propagator. - *

The reference propagator can be almost anything, numerical, - * analytical, and even an ephemeris. It may already take some maneuvers - * into account.

+ /** + * Build a propagator from an underlying reference propagator. + *

+ * The reference propagator can be almost anything, numerical, analytical, + * and even an ephemeris. It may already take some maneuvers into account. + *

+ * * @param field * @param reference reference propagator */ - public FieldAdapterPropagator(Field field, final FieldPropagator reference) { + public FieldAdapterPropagator(final Field field, + final FieldPropagator reference) { super(field, reference.getAttitudeProvider()); this.reference = reference; this.effects = new ArrayList>(); } - /** Add a differential effect. + /** + * Add a differential effect. + * * @param effect differential effect */ public void addEffect(final FieldDifferentialEffect effect) { effects.add(effect); } - /** Get the reference propagator. + /** + * Get the reference propagator. + * * @return reference propagator */ public FieldPropagator getPropagator() { return reference; } - /** Get the differential effects. + /** + * Get the differential effects. + * * @return differential effects models, as an unmodifiable list */ public List> getEffects() { @@ -125,9 +138,11 @@ public class FieldAdapterPropagator> extends Field } /** {@inheritDoc} */ - protected void resetIntermediateState(final FieldSpacecraftState state, final boolean forward) { + protected void resetIntermediateState(final FieldSpacecraftState state, + final boolean forward) { if (reference instanceof FieldAbstractAnalyticalPropagator) { - ((FieldAbstractAnalyticalPropagator) reference).resetIntermediateState(state, forward); + ((FieldAbstractAnalyticalPropagator) reference) + .resetIntermediateState(state, forward); } else { throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); } @@ -135,7 +150,8 @@ public class FieldAdapterPropagator> extends Field /** {@inheritDoc} */ @Override - protected FieldSpacecraftState basicPropagate(final FieldAbsoluteDate date) { + protected FieldSpacecraftState + basicPropagate(final FieldAbsoluteDate date) { // compute reference state FieldSpacecraftState state = reference.propagate(date); @@ -149,7 +165,8 @@ public class FieldAdapterPropagator> extends Field // forward additional states from the reference propagator for (final Map.Entry entry : before.entrySet()) { if (!state.hasAdditionalState(entry.getKey())) { - state = state.addAdditionalState(entry.getKey(), entry.getValue()); + state = + state.addAdditionalState(entry.getKey(), entry.getValue()); } } @@ -162,24 +179,26 @@ public class FieldAdapterPropagator> extends Field return basicPropagate(date).getOrbit(); } - /** {@inheritDoc}*/ + /** {@inheritDoc} */ protected T getMass(final FieldAbsoluteDate date) { return basicPropagate(date).getMass(); } - @Override - protected FieldOrbit propagateOrbit(FieldAbsoluteDate date, T[] parameters) { - // TODO Auto-generated method stub - return null; - } + @Override + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, + final T[] parameters) { + return basicPropagate(date).getOrbit(); + } - /** Get the parameters driver for the Field SBAS propagation model. + /** + * Get the parameters driver for the Field SBAS propagation model. + * * @return an empty list. */ - @Override - protected List getParametersDrivers() { - // The Field Adapter propagation model does not have parameter drivers. + @Override + protected List getParametersDrivers() { + // The Field Adapter propagation model does not have parameter drivers. return Collections.emptyList(); - } + } } diff --git a/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java b/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java index 656dc9704..195aeac3b 100644 --- a/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java +++ b/src/main/java/org/orekit/propagation/analytical/FieldEphemeris.java @@ -57,363 +57,402 @@ import org.orekit.utils.TimeStampedFieldPVCoordinates; * @author Nicolas Fialton (field translation) */ -public class FieldEphemeris> extends FieldAbstractAnalyticalPropagator - implements FieldBoundedPropagator { - - /** - * Default extrapolation time threshold: 1ms. - * - * @since 9.0 - **/ - public static final double DEFAULT_EXTRAPOLATION_THRESHOLD_SEC = 1e-3; - - /** First date in range. */ - private final FieldAbsoluteDate minDate; - - /** Last date in range. */ - private final FieldAbsoluteDate maxDate; - - /** The extrapolation threshold beyond which the propagation will fail. **/ - private final T extrapolationThreshold; - - /** Reference frame. */ - private final Frame frame; - - /** Names of the additional states. */ - private final String[] additional; - - /** Local PV Provider used for computing attitude. **/ - private LocalPVProvider pvProvider; - - /** Thread-safe cache. */ - private final transient ImmutableTimeStampedCache cache; - - /** - * Constructor with tabulated states. - *

- * This constructor allows extrapolating outside of the states time span by up - * to the 1ms {@link #DEFAULT_EXTRAPOLATION_THRESHOLD_SEC default extrapolation - * threshold}. - *

- * - *

- * This constructor uses the {@link DataContext#getDefault() default data - * context}. - * - * @param field - * @param states tabulates states - * @param interpolationPoints number of points to use in interpolation - * @exception MathIllegalArgumentException if the number of states is smaller - * than the number of points to use in - * interpolation - * @see #Ephemeris(List, int, T) - * @see #Ephemeris(List, int, T, AttitudeProvider) - */ - @DefaultDataContext - public FieldEphemeris(Field field, final List states, final int interpolationPoints) - throws MathIllegalArgumentException { - this(field, states, interpolationPoints, field.getZero().add(DEFAULT_EXTRAPOLATION_THRESHOLD_SEC)); - } - - /** - * Constructor with tabulated states. - * - *

- * This constructor uses the {@link DataContext#getDefault() default data - * context}. - * - * @param field - * @param states tabulates states - * @param interpolationPoints number of points to use in interpolation - * @param extrapolationThreshold the largest time difference in seconds between - * the start or stop boundary of the ephemeris - * bounds to be doing extrapolation - * @exception MathIllegalArgumentException if the number of states is smaller - * than the number of points to use in - * interpolation - * @see #FieldEphemeris(field, List, int, T, AttitudeProvider) - */ - @DefaultDataContext - public FieldEphemeris(Field field, final List states, final int interpolationPoints, - final T extrapolationThreshold) throws MathIllegalArgumentException { - this(field, states, interpolationPoints, extrapolationThreshold, - Propagator.getDefaultLaw(DataContext.getDefault().getFrames())); - } - - /** - * Constructor with tabulated states. - * - * @param field - * @param states tabulates states - * @param interpolationPoints number of points to use in interpolation - * @param extrapolationThreshold the largest time difference in seconds between - * the start or stop boundary of the ephemeris - * bounds to be doing extrapolation - * @param attitudeProvider attitude law to use. - * @exception MathIllegalArgumentException if the number of states is smaller - * than the number of points to use in - * interpolation - */ - public FieldEphemeris(Field field, final List states, final int interpolationPoints, - final T extrapolationThreshold, final AttitudeProvider attitudeProvider) - throws MathIllegalArgumentException { - - super(field, attitudeProvider); - - if (states.size() < interpolationPoints) { - throw new MathIllegalArgumentException(LocalizedCoreFormats.INSUFFICIENT_DIMENSION, states.size(), - interpolationPoints); - } - - final SpacecraftState s0 = states.get(0); - minDate = new FieldAbsoluteDate<>(field, s0.getDate()); - maxDate = new FieldAbsoluteDate<>(field, states.get(states.size() - 1).getDate()); - frame = s0.getFrame(); - - final Set names0 = s0.getAdditionalStates().keySet(); - additional = names0.toArray(new String[names0.size()]); - - // check all states handle the same additional states - for (final SpacecraftState state : states) { - s0.ensureCompatibleAdditionalStates(state); - } - - pvProvider = new LocalPVProvider<>(states, interpolationPoints, extrapolationThreshold); - - // user needs to explicitly set attitude provider if they want to use one - setAttitudeProvider(null); - - // set up cache - cache = new ImmutableTimeStampedCache(interpolationPoints, states); - - this.extrapolationThreshold = extrapolationThreshold; - } - - /** - * Get the first date of the range. - * - * @return the first date of the range - */ - public FieldAbsoluteDate getMinDate() { - return minDate; - } - - /** - * Get the last date of the range. - * - * @return the last date of the range - */ - public FieldAbsoluteDate getMaxDate() { - return maxDate; - } - - /** - * Get the maximum timespan outside of the stored ephemeris that is allowed for - * extrapolation. - * - * @return the extrapolation threshold in seconds - */ - public T getExtrapolationThreshold() { - return extrapolationThreshold; - } - - /** {@inheritDoc} */ - @Override - public Frame getFrame() { - return frame; - } - - /** {@inheritDoc} */ - @Override - public FieldSpacecraftState basicPropagate(final FieldAbsoluteDate date) { - final FieldSpacecraftState evaluatedState; - - final FieldAbsoluteDate central; - if (date.compareTo(minDate) < 0 - && FastMath.abs(date.durationFrom(minDate)).getReal() <= extrapolationThreshold.getReal()) { - // avoid TimeStampedCacheException as we are still within the tolerance before - // minDate - central = minDate; - } else if (date.compareTo(maxDate) > 0 - && FastMath.abs(date.durationFrom(maxDate)).getReal() <= extrapolationThreshold.getReal()) { - // avoid TimeStampedCacheException as we are still within the tolerance after - // maxDate - central = maxDate; - } else { - central = date; - } - final List> neighbors = cache.getNeighbors(central.toAbsoluteDate()) - .map(state -> new FieldSpacecraftState<>(date.getField(), state)) - .collect(Collectors.toList()); - evaluatedState = neighbors.get(0).interpolate(date, neighbors); - - final AttitudeProvider attitudeProvider = getAttitudeProvider(); - - if (attitudeProvider == null) { - return evaluatedState; - } else { - pvProvider.setCurrentState(evaluatedState); - final FieldAttitude calculatedAttitude = attitudeProvider.getAttitude(pvProvider, date, - evaluatedState.getFrame()); - - // Verify if orbit is defined - if (evaluatedState.isOrbitDefined()) { - return new FieldSpacecraftState(evaluatedState.getOrbit(), calculatedAttitude, - evaluatedState.getMass(), evaluatedState.getAdditionalStates()); - } else { - return new FieldSpacecraftState(evaluatedState.getAbsPVA(), calculatedAttitude, - evaluatedState.getMass(), evaluatedState.getAdditionalStates()); - } - - } - } +public class FieldEphemeris> + extends + FieldAbstractAnalyticalPropagator + implements + FieldBoundedPropagator { + + /** + * Default extrapolation time threshold: 1ms. + * + * @since 9.0 + **/ + public static final double DEFAULT_EXTRAPOLATION_THRESHOLD_SEC = 1e-3; + + /** First date in range. */ + private final FieldAbsoluteDate minDate; + + /** Last date in range. */ + private final FieldAbsoluteDate maxDate; + + /** The extrapolation threshold beyond which the propagation will fail. **/ + private final T extrapolationThreshold; + + /** Reference frame. */ + private final Frame frame; + + /** Names of the additional states. */ + private final String[] additional; + + /** Local PV Provider used for computing attitude. **/ + private LocalPVProvider pvProvider; + + /** Thread-safe cache. */ + private final transient ImmutableTimeStampedCache cache; + + /** + * Constructor with tabulated states. + *

+ * This constructor allows extrapolating outside of the states time span by + * up to the 1ms {@link #DEFAULT_EXTRAPOLATION_THRESHOLD_SEC default + * extrapolation threshold}. + *

+ *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. + * + * @param field + * @param states tabulates states + * @param interpolationPoints number of points to use in interpolation + * @exception MathIllegalArgumentException if the number of states is + * smaller than the number of points to use in interpolation + * @see #Ephemeris(List, int, T) + * @see #Ephemeris(List, int, T, AttitudeProvider) + */ + @DefaultDataContext + public FieldEphemeris(final Field field, final List states, + final int interpolationPoints) + throws MathIllegalArgumentException { + this(field, states, interpolationPoints, + field.getZero().add(DEFAULT_EXTRAPOLATION_THRESHOLD_SEC)); + } + + /** + * Constructor with tabulated states. + *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. + * + * @param field + * @param states tabulates states + * @param interpolationPoints number of points to use in interpolation + * @param extrapolationThreshold the largest time difference in seconds + * between the start or stop boundary of the ephemeris bounds to be + * doing extrapolation + * @exception MathIllegalArgumentException if the number of states is + * smaller than the number of points to use in interpolation + * @see #FieldEphemeris(field, List, int, T, AttitudeProvider) + */ + @DefaultDataContext + public FieldEphemeris(final Field field, final List states, + final int interpolationPoints, + final T extrapolationThreshold) + throws MathIllegalArgumentException { + this(field, states, interpolationPoints, extrapolationThreshold, + Propagator.getDefaultLaw(DataContext.getDefault().getFrames())); + } + + /** + * Constructor with tabulated states. + * + * @param field + * @param states tabulates states + * @param interpolationPoints number of points to use in interpolation + * @param extrapolationThreshold the largest time difference in seconds + * between the start or stop boundary of the ephemeris bounds to be + * doing extrapolation + * @param attitudeProvider attitude law to use. + * @exception MathIllegalArgumentException if the number of states is + * smaller than the number of points to use in interpolation + */ + public FieldEphemeris(final Field field, final List states, + final int interpolationPoints, + final T extrapolationThreshold, + final AttitudeProvider attitudeProvider) + throws MathIllegalArgumentException { + + super(field, attitudeProvider); + + if (states.size() < interpolationPoints) { + throw new MathIllegalArgumentException(LocalizedCoreFormats.INSUFFICIENT_DIMENSION, + states.size(), + interpolationPoints); + } + + final SpacecraftState s0 = states.get(0); + minDate = new FieldAbsoluteDate<>(field, s0.getDate()); + maxDate = + new FieldAbsoluteDate<>(field, + states.get(states.size() - 1).getDate()); + frame = s0.getFrame(); + + final Set names0 = s0.getAdditionalStates().keySet(); + additional = names0.toArray(new String[names0.size()]); + + // check all states handle the same additional states + for (final SpacecraftState state : states) { + s0.ensureCompatibleAdditionalStates(state); + } + + pvProvider = + new LocalPVProvider<>(states, interpolationPoints, + extrapolationThreshold); + + // user needs to explicitly set attitude provider if they want to use + // one + setAttitudeProvider(null); + + // set up cache + cache = + new ImmutableTimeStampedCache(interpolationPoints, + states); + + this.extrapolationThreshold = extrapolationThreshold; + } + + /** + * Get the first date of the range. + * + * @return the first date of the range + */ + public FieldAbsoluteDate getMinDate() { + return minDate; + } + + /** + * Get the last date of the range. + * + * @return the last date of the range + */ + public FieldAbsoluteDate getMaxDate() { + return maxDate; + } + + /** + * Get the maximum timespan outside of the stored ephemeris that is allowed + * for extrapolation. + * + * @return the extrapolation threshold in seconds + */ + public T getExtrapolationThreshold() { + return extrapolationThreshold; + } /** {@inheritDoc} */ - @Override - protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, final T[] parameters) { - return basicPropagate(date).getOrbit(); - } - - /** {@inheritDoc} */ - @Override - protected T getMass(final FieldAbsoluteDate date) { - return basicPropagate(date).getMass(); - } - - /** {@inheritDoc} */ - @Override - public TimeStampedFieldPVCoordinates getPVCoordinates(final FieldAbsoluteDate date, final Frame f) { - return propagate(date).getPVCoordinates(f); - } - - /** {@inheritDoc} */ - @Override - protected List getParametersDrivers() { - return Collections.emptyList(); - } - - /** - * Try (and fail) to reset the initial state. - *

- * This method always throws an exception, as ephemerides cannot be reset. - *

- * - * @param state new initial state to consider - */ - public void resetInitialState(final FieldSpacecraftState state) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - /** {@inheritDoc} */ - protected void resetIntermediateState(final FieldSpacecraftState state, final boolean forward) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - /** {@inheritDoc} */ - public FieldSpacecraftState getInitialState() { - return basicPropagate(getMinDate()); - } - - /** {@inheritDoc} */ - @Override - public boolean isAdditionalStateManaged(final String name) { - - // the additional state may be managed by a specific provider in the base class - if (super.isAdditionalStateManaged(name)) { - return true; - } - - // the additional state may be managed in the states sample - for (final String a : additional) { - if (a.equals(name)) { - return true; - } - } - - return false; - - } - - /** {@inheritDoc} */ - @Override - public String[] getManagedAdditionalStates() { - final String[] upperManaged = super.getManagedAdditionalStates(); - final String[] managed = new String[upperManaged.length + additional.length]; - System.arraycopy(upperManaged, 0, managed, 0, upperManaged.length); - System.arraycopy(additional, 0, managed, upperManaged.length, additional.length); - return managed; - } - - /** Internal PVCoordinatesProvider for attitude computation. */ - private static class LocalPVProvider> - implements FieldPVCoordinatesProvider, Serializable { - - /** Serializable UID. */ - private static final long serialVersionUID = 20160115L; - - /** Current state. */ - private FieldSpacecraftState currentState; - - /** List of spacecraft states. */ - private List states; - - /** Interpolation points number. */ - private int interpolationPoints; - - /** Extrapolation threshold. */ - private T extrapolationThreshold; - - /** - * Constructor. - * - * @param states list of spacecraft states - * @param interpolationPoints interpolation points number - * @param extrapolationThreshold extrapolation threshold value - */ - LocalPVProvider(final List states, final int interpolationPoints, - final T extrapolationThreshold) { - - this.states = states; - this.interpolationPoints = interpolationPoints; - this.extrapolationThreshold = extrapolationThreshold; - } - - /** - * Get the current state. - * - * @return current state - */ - public FieldSpacecraftState getCurrentState() { - return currentState; - } - - /** - * Set the current state. - * - * @param state state to set - */ - public void setCurrentState(final FieldSpacecraftState state) { - this.currentState = state; - } - - /** {@inheritDoc} */ - public TimeStampedFieldPVCoordinates getPVCoordinates(final FieldAbsoluteDate date, final Frame f) { - final T dt = getCurrentState().getDate().durationFrom(date); - final double closeEnoughTimeInSec = 1e-9; - - if (FastMath.abs(dt).getReal() > closeEnoughTimeInSec) { - - // used in case of attitude transition, the attitude computed is not at the - // current date. - final FieldEphemeris ephemeris = new FieldEphemeris<>(date.getField(), states, interpolationPoints, - extrapolationThreshold, null); - return ephemeris.getPVCoordinates(date, f); - } - - return currentState.getPVCoordinates(f); - - } - - } + @Override + public Frame getFrame() { + return frame; + } + + /** {@inheritDoc} */ + @Override + public FieldSpacecraftState + basicPropagate(final FieldAbsoluteDate date) { + final FieldSpacecraftState evaluatedState; + + final FieldAbsoluteDate central; + if (date.compareTo(minDate) < 0 && + FastMath.abs(date.durationFrom(minDate)) + .getReal() <= extrapolationThreshold.getReal()) { + // avoid TimeStampedCacheException as we are still within the + // tolerance before + // minDate + central = minDate; + } else if (date.compareTo(maxDate) > 0 && + FastMath.abs(date.durationFrom(maxDate)) + .getReal() <= extrapolationThreshold.getReal()) { + // avoid TimeStampedCacheException as we are still within the + // tolerance after + // maxDate + central = maxDate; + } else { + central = date; + } + final List> neighbors = + cache.getNeighbors(central.toAbsoluteDate()) + .map(state -> new FieldSpacecraftState<>(date.getField(), + state)) + .collect(Collectors.toList()); + evaluatedState = neighbors.get(0).interpolate(date, neighbors); + + final AttitudeProvider attitudeProvider = getAttitudeProvider(); + + if (attitudeProvider == null) { + return evaluatedState; + } else { + pvProvider.setCurrentState(evaluatedState); + final FieldAttitude calculatedAttitude = + attitudeProvider.getAttitude(pvProvider, date, + evaluatedState.getFrame()); + + // Verify if orbit is defined + if (evaluatedState.isOrbitDefined()) { + return new FieldSpacecraftState(evaluatedState.getOrbit(), + calculatedAttitude, + evaluatedState.getMass(), + evaluatedState + .getAdditionalStates()); + } else { + return new FieldSpacecraftState(evaluatedState.getAbsPVA(), + calculatedAttitude, + evaluatedState.getMass(), + evaluatedState + .getAdditionalStates()); + } + + } + } + + /** {@inheritDoc} */ + @Override + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, + final T[] parameters) { + return basicPropagate(date).getOrbit(); + } + + /** {@inheritDoc} */ + @Override + protected T getMass(final FieldAbsoluteDate date) { + return basicPropagate(date).getMass(); + } + + /** {@inheritDoc} */ + @Override + public TimeStampedFieldPVCoordinates + getPVCoordinates(final FieldAbsoluteDate date, final Frame f) { + return propagate(date).getPVCoordinates(f); + } + + /** {@inheritDoc} */ + @Override + protected List getParametersDrivers() { + return Collections.emptyList(); + } + + /** + * Try (and fail) to reset the initial state. + *

+ * This method always throws an exception, as ephemerides cannot be reset. + *

+ * + * @param state new initial state to consider + */ + public void resetInitialState(final FieldSpacecraftState state) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected void resetIntermediateState(final FieldSpacecraftState state, + final boolean forward) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + public FieldSpacecraftState getInitialState() { + return basicPropagate(getMinDate()); + } + + /** {@inheritDoc} */ + @Override + public boolean isAdditionalStateManaged(final String name) { + + // the additional state may be managed by a specific provider in the + // base class + if (super.isAdditionalStateManaged(name)) { + return true; + } + + // the additional state may be managed in the states sample + for (final String a : additional) { + if (a.equals(name)) { + return true; + } + } + + return false; + + } + + /** {@inheritDoc} */ + @Override + public String[] getManagedAdditionalStates() { + final String[] upperManaged = super.getManagedAdditionalStates(); + final String[] managed = + new String[upperManaged.length + additional.length]; + System.arraycopy(upperManaged, 0, managed, 0, upperManaged.length); + System.arraycopy(additional, 0, managed, upperManaged.length, + additional.length); + return managed; + } + + /** Internal PVCoordinatesProvider for attitude computation. */ + private static class LocalPVProvider> + implements + FieldPVCoordinatesProvider, + Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 20160115L; + + /** Current state. */ + private FieldSpacecraftState currentState; + + /** List of spacecraft states. */ + private List states; + + /** Interpolation points number. */ + private int interpolationPoints; + + /** Extrapolation threshold. */ + private T extrapolationThreshold; + + /** + * Constructor. + * + * @param states list of spacecraft states + * @param interpolationPoints interpolation points number + * @param extrapolationThreshold extrapolation threshold value + */ + LocalPVProvider(final List states, + final int interpolationPoints, + final T extrapolationThreshold) { + + this.states = states; + this.interpolationPoints = interpolationPoints; + this.extrapolationThreshold = extrapolationThreshold; + } + + /** + * Get the current state. + * + * @return current state + */ + public FieldSpacecraftState getCurrentState() { + return currentState; + } + + /** + * Set the current state. + * + * @param state state to set + */ + public void setCurrentState(final FieldSpacecraftState state) { + this.currentState = state; + } + + /** {@inheritDoc} */ + public TimeStampedFieldPVCoordinates + getPVCoordinates(final FieldAbsoluteDate date, final Frame f) { + final T dt = getCurrentState().getDate().durationFrom(date); + final double closeEnoughTimeInSec = 1e-9; + + if (FastMath.abs(dt).getReal() > closeEnoughTimeInSec) { + + // used in case of attitude transition, the attitude computed is + // not at the + // current date. + final FieldEphemeris ephemeris = + new FieldEphemeris<>(date.getField(), states, + interpolationPoints, + extrapolationThreshold, null); + return ephemeris.getPVCoordinates(date, f); + } + + return currentState.getPVCoordinates(f); + + } + + } } diff --git a/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java b/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java index cf77417cc..f73835617 100644 --- a/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java +++ b/src/main/java/org/orekit/propagation/analytical/FieldSmallManeuverAnalyticalModel.java @@ -51,7 +51,6 @@ import org.orekit.utils.Constants; * with respect to Keplerian or equinoctial elements at date t₀ allows to * propagate the change in orbital elements to final date t₁. * - * *

* The second Jacobian, J1/0, is computed using a simple Keplerian * model, i.e. it is the identity except for the mean motion row which also @@ -72,316 +71,365 @@ import org.orekit.utils.Constants; * {@link org.orekit.propagation.analytical.J2DifferentialEffect * J2DifferentialEffect} must be used together with an instance of this class. *

- * + * * @author Luc Maisonobe * @author Nicolas Fialton (field translation) */ public class FieldSmallManeuverAnalyticalModel> - implements FieldAdapterPropagator.FieldDifferentialEffect { - /** State at maneuver date (before maneuver occurrence). */ - private final FieldSpacecraftState state0; - - /** Inertial velocity increment. */ - private final FieldVector3D inertialDV; - - /** Mass change ratio. */ - private final T massRatio; - - /** Type of orbit used for internal Jacobians. */ - private final OrbitType type; - - /** Initial Keplerian (or equinoctial) Jacobian with respect to maneuver. */ - private final T[][] j0; - - /** - * Time derivative of the initial Keplerian (or equinoctial) Jacobian with - * respect to maneuver. - */ - private T[][] j0Dot; - - /** Mean anomaly change factor. */ - private final T ksi; - - /** - * Build a maneuver defined in spacecraft frame. - * - * @param state0 state at maneuver date, before the maneuver is - * performed - * @param dV velocity increment in spacecraft frame - * @param isp engine specific impulse (s) - */ - public FieldSmallManeuverAnalyticalModel(Field field, final FieldSpacecraftState state0, - final FieldVector3D dV, final T isp) { - this(field, state0, state0.getFrame(), state0.getAttitude().getRotation().applyInverseTo(dV), isp); - } - - /** - * Build a maneuver defined in user-specified frame. - * - * @param state0 state at maneuver date, before the maneuver is - * performed - * @param frame frame in which velocity increment is defined - * @param dV velocity increment in specified frame - * @param isp engine specific impulse (s) - */ - public FieldSmallManeuverAnalyticalModel(Field field, final FieldSpacecraftState state0, final Frame frame, - final FieldVector3D dV, final T isp) { - final T zero = field.getZero(); - this.state0 = state0; - this.massRatio = FastMath.exp(zero.subtract(dV.getNorm()).divide(isp.multiply(Constants.G0_STANDARD_GRAVITY))); - - // use equinoctial orbit type if possible, Keplerian if nearly hyperbolic orbits - type = (state0.getE().getReal() < 0.9) ? OrbitType.EQUINOCTIAL : OrbitType.KEPLERIAN; - - // compute initial Jacobian - final T[][] fullJacobian = MathArrays.buildArray(field, 6, 6); - j0 = MathArrays.buildArray(field, 6, 3); - final FieldOrbit orbit0 = type.convertType(state0.getOrbit()); - orbit0.getJacobianWrtCartesian(PositionAngle.MEAN, fullJacobian); - for (int i = 0; i < j0.length; ++i) { - System.arraycopy(fullJacobian[i], 3, j0[i], 0, 3); - } - - // use lazy evaluation for j0Dot, as it is used only when Jacobians are - // evaluated - j0Dot = null; - - // compute maneuver effect on Keplerian (or equinoctial) elements - inertialDV = frame.getTransformTo(state0.getFrame(), state0.getDate()).transformVector(dV); - - // compute mean anomaly change: dM(t1) = dM(t0) + ksi * da * (t1 - t0) - final T mu = state0.getMu(); - final T a = state0.getA(); - ksi = zero.add(-1.5).multiply(FastMath.sqrt(zero.add(mu).divide(a)).divide(a.multiply(a))); - - } - - /** - * Get the date of the maneuver. - * - * @return date of the maneuver - */ - public FieldAbsoluteDate getDate() { - return state0.getDate(); - } - - /** - * Get the inertial velocity increment of the maneuver. - * - * @return velocity increment in a state-dependent inertial frame - * @see #getInertialFrame() - */ - public FieldVector3D getInertialDV() { - return inertialDV; - } - - /** - * Get the inertial frame in which the velocity increment is defined. - * - * @return inertial frame in which the velocity increment is defined - * @see #getInertialDV() - */ - public Frame getInertialFrame() { - return state0.getFrame(); - } - - /** - * Compute the effect of the maneuver on an orbit. - * - * @param orbit1 original orbit at t₁, without maneuver - * @return orbit at t₁, taking the maneuver into account if t₁ > t₀ - * @see #apply(SpacecraftState) - * @see #getJacobian(Orbit, PositionAngle, double[][]) - */ - public FieldOrbit apply(final FieldOrbit orbit1) { - - if (orbit1.getDate().compareTo(state0.getDate()) <= 0) { - // the maneuver has not occurred yet, don't change anything - return orbit1; - } - - return orbit1.getType().convertType(updateOrbit(orbit1)); - - } - - /** - * Compute the effect of the maneuver on a spacecraft state. - * - * @param state1 original spacecraft state at t₁, without maneuver - * @return spacecraft state at t₁, taking the maneuver into account if t₁ > - * t₀ - * @see #apply(Orbit) - * @see #getJacobian(Orbit, PositionAngle, double[][]) - */ - public FieldSpacecraftState apply(final FieldSpacecraftState state1) { - - if (state1.getDate().compareTo(state0.getDate()) <= 0) { - // the maneuver has not occurred yet, don't change anything - return state1; - } - - return new FieldSpacecraftState<>(state1.getOrbit().getType().convertType(updateOrbit(state1.getOrbit())), - state1.getAttitude(), updateMass(state1.getMass())); - - } - - /** - * Compute the effect of the maneuver on an orbit. - * - * @param orbit1 original orbit at t₁, without maneuver - * @return orbit at t₁, always taking the maneuver into account, always in the - * internal type - */ - private FieldOrbit updateOrbit(final FieldOrbit orbit1) { - - // compute maneuver effect - final T dt = orbit1.getDate().durationFrom(state0.getDate()); - final T x = inertialDV.getX(); - final T y = inertialDV.getY(); - final T z = inertialDV.getZ(); - final Field field = z.getField(); - final T[] delta = MathArrays.buildArray(field, 6); - for (int i = 0; i < delta.length; ++i) { - delta[i] = j0[i][0].multiply(x).add(j0[i][1].multiply(y).add(j0[i][2].multiply(z))); - } - delta[5] = delta[5].add(ksi.multiply(delta[0]).multiply(dt)); - - // convert current orbital state to Keplerian or equinoctial elements - final T[] parameters = MathArrays.buildArray(field, 6); - type.mapOrbitToArray(type.convertType(orbit1), PositionAngle.MEAN, parameters, null); - for (int i = 0; i < delta.length; ++i) { - parameters[i] = parameters[i].add(delta[i]); - } - - // build updated orbit as Keplerian or equinoctial elements - return type.mapArrayToOrbit(parameters, null, PositionAngle.MEAN, orbit1.getDate(), orbit1.getMu(), - orbit1.getFrame()); - - } - - /** - * Compute the Jacobian of the orbit with respect to maneuver parameters. - *

- * The Jacobian matrix is a 6x4 matrix. Element jacobian[i][j] corresponds to - * the partial derivative of orbital parameter i with respect to maneuver - * parameter j. The rows order is the same order as used in - * {@link Orbit#getJacobianWrtCartesian(PositionAngle, double[][]) - * Orbit.getJacobianWrtCartesian} method. Columns (0, 1, 2) correspond to the - * velocity increment coordinates (ΔVx, ΔVy, - * ΔVz) in the inertial frame returned by - * {@link #getInertialFrame()}, and column 3 corresponds to the maneuver date - * t₀. - *

- * - * @param orbit1 original orbit at t₁, without maneuver - * @param positionAngle type of the position angle to use - * @param jacobian placeholder 6x4 (or larger) matrix to be filled with the - * Jacobian, if matrix is larger than 6x4, only the 6x4 - * upper left corner will be modified - * @see #apply(Orbit) - */ - public void getJacobian(final FieldOrbit orbit1, final PositionAngle positionAngle, final T[][] jacobian) { - - final T dt = orbit1.getDate().durationFrom(state0.getDate()); - if (dt.getReal() < 0) { - // the maneuver has not occurred yet, Jacobian is null - for (int i = 0; i < 6; ++i) { - Arrays.fill(jacobian[i], 0, 4, dt.getField().getZero()); - } - return; - } - - // derivatives of Keplerian/equinoctial elements with respect to velocity - // increment - final T x = inertialDV.getX(); - final T y = inertialDV.getY(); - final T z = inertialDV.getZ(); - for (int i = 0; i < 6; ++i) { - System.arraycopy(j0[i], 0, jacobian[i], 0, 3); - } - for (int j = 0; j < 3; ++j) { - jacobian[5][j] = jacobian[5][j].add(ksi.multiply(dt).multiply(j0[0][j])); - } - - // derivatives of Keplerian/equinoctial elements with respect to date - evaluateJ0Dot(); - for (int i = 0; i < 6; ++i) { - jacobian[i][3] = j0Dot[i][0].multiply(x).add(j0Dot[i][1].multiply(y)).add(j0Dot[i][2].multiply(z)); - } - final T da = j0[0][0].multiply(x).add(j0[0][1].multiply(y)).add(j0[0][2].multiply(z)); - jacobian[5][3] = jacobian[5][3].add(ksi.multiply(jacobian[0][3].multiply(dt).subtract(da))); - - if (orbit1.getType() != type || positionAngle != PositionAngle.MEAN) { - - // convert to derivatives of Cartesian parameters - final Field field = x.getField(); - final T[][] j2 = MathArrays.buildArray(field, 6, 6); - final T[][] pvJacobian = MathArrays.buildArray(field, 6, 4); - final FieldOrbit updated = updateOrbit(orbit1); - updated.getJacobianWrtParameters(PositionAngle.MEAN, j2); - for (int i = 0; i < 6; ++i) { - for (int j = 0; j < 4; ++j) { - pvJacobian[i][j] = j2[i][0].multiply(jacobian[0][j]).add(j2[i][1].multiply(jacobian[1][j])) - .add(j2[i][2].multiply(jacobian[2][j])).add(j2[i][3].multiply(jacobian[3][j])) - .add(j2[i][4].multiply(jacobian[4][j])).add(j2[i][5].multiply(jacobian[5][j])); - } - } - - // convert to derivatives of specified parameters - final T[][] j3 = MathArrays.buildArray(field, 6, 6); - orbit1.getType().convertType(updated).getJacobianWrtCartesian(positionAngle, j3); - for (int j = 0; j < 4; ++j) { - for (int i = 0; i < 6; ++i) { - jacobian[i][j] = j3[i][0].multiply(pvJacobian[0][j]).add(j3[i][1].multiply(pvJacobian[1][j])) - .add(j3[i][2].multiply(pvJacobian[2][j])).add(j3[i][3].multiply(pvJacobian[3][j])) - .add(j3[i][4].multiply(pvJacobian[4][j])).add(j3[i][5].multiply(pvJacobian[5][j])); - } - } - - } - - } - - /** - * Lazy evaluation of the initial Jacobian time derivative. - */ - private void evaluateJ0Dot() { - - if (j0Dot == null) { - final Field field = massRatio.getField(); - final T zero = field.getZero(); - j0Dot = MathArrays.buildArray(field, 6, 3); - final T dt = zero.add(1.0e-5).divide(state0.getOrbit().getKeplerianMeanMotion()); - final FieldOrbit orbit = type.convertType(state0.getOrbit()); - - // compute shifted Jacobians - final T[][] j0m1 = MathArrays.buildArray(field, 6, 6); - orbit.shiftedBy(dt.multiply(-1)).getJacobianWrtCartesian(PositionAngle.MEAN, j0m1); - final T[][] j0p1 = MathArrays.buildArray(field, 6, 6); - orbit.shiftedBy(dt.multiply(+1)).getJacobianWrtCartesian(PositionAngle.MEAN, j0p1); - - // evaluate derivative by finite differences - for (int i = 0; i < j0Dot.length; ++i) { - final T[] m1Row = j0m1[i]; - final T[] p1Row = j0p1[i]; - final T[] j0DotRow = j0Dot[i]; - for (int j = 0; j < 3; ++j) { - j0DotRow[j] = (p1Row[j + 3].subtract(m1Row[j + 3])).divide(dt.multiply(2)); - } - } - - } - - } - - /** - * Update a spacecraft mass due to maneuver. - * - * @param mass masse before maneuver - * @return mass after maneuver - */ - public T updateMass(final T mass) { - return massRatio.multiply(mass); - } + implements + FieldAdapterPropagator.FieldDifferentialEffect { + + /** State at maneuver date (before maneuver occurrence). */ + private final FieldSpacecraftState state0; + + /** Inertial velocity increment. */ + private final FieldVector3D inertialDV; + + /** Mass change ratio. */ + private final T massRatio; + + /** Type of orbit used for internal Jacobians. */ + private final OrbitType type; + + /** Initial Keplerian (or equinoctial) Jacobian with respect to maneuver. */ + private final T[][] j0; + + /** + * Time derivative of the initial Keplerian (or equinoctial) Jacobian with + * respect to maneuver. + */ + private T[][] j0Dot; + + /** Mean anomaly change factor. */ + private final T ksi; + + /** + * Build a maneuver defined in spacecraft frame. + * + * @param field + * @param state0 state at maneuver date, before the maneuver is performed + * @param dV velocity increment in spacecraft frame + * @param isp engine specific impulse (s) + */ + public FieldSmallManeuverAnalyticalModel(final Field field, + final FieldSpacecraftState state0, + final FieldVector3D dV, + final T isp) { + this(field, state0, state0.getFrame(), + state0.getAttitude().getRotation().applyInverseTo(dV), isp); + } + + /** + * Build a maneuver defined in user-specified frame. + * + * @param field + * @param state0 state at maneuver date, before the maneuver is + * performed + * @param frame frame in which velocity increment is defined + * @param dV velocity increment in specified frame + * @param isp engine specific impulse (s) + */ + public FieldSmallManeuverAnalyticalModel(final Field field, + final FieldSpacecraftState state0, + final Frame frame, + final FieldVector3D dV, + final T isp) { + final T zero = field.getZero(); + this.state0 = state0; + this.massRatio = + FastMath.exp(zero.subtract(dV.getNorm()) + .divide(isp.multiply(Constants.G0_STANDARD_GRAVITY))); + + // use equinoctial orbit type if possible, Keplerian if nearly + // hyperbolic orbits + type = + (state0.getE().getReal() < 0.9) ? + OrbitType.EQUINOCTIAL : + OrbitType.KEPLERIAN; + + // compute initial Jacobian + final T[][] fullJacobian = MathArrays.buildArray(field, 6, 6); + j0 = MathArrays.buildArray(field, 6, 3); + final FieldOrbit orbit0 = type.convertType(state0.getOrbit()); + orbit0.getJacobianWrtCartesian(PositionAngle.MEAN, fullJacobian); + for (int i = 0; i < j0.length; ++i) { + System.arraycopy(fullJacobian[i], 3, j0[i], 0, 3); + } + + // use lazy evaluation for j0Dot, as it is used only when Jacobians are + // evaluated + j0Dot = null; + + // compute maneuver effect on Keplerian (or equinoctial) elements + inertialDV = + frame.getTransformTo(state0.getFrame(), state0.getDate()) + .transformVector(dV); + + // compute mean anomaly change: dM(t1) = dM(t0) + ksi * da * (t1 - t0) + final T mu = state0.getMu(); + final T a = state0.getA(); + ksi = + zero.add(-1.5).multiply(FastMath.sqrt(zero.add(mu).divide(a)) + .divide(a.multiply(a))); + + } + + /** + * Get the date of the maneuver. + * + * @return date of the maneuver + */ + public FieldAbsoluteDate getDate() { + return state0.getDate(); + } + + /** + * Get the inertial velocity increment of the maneuver. + * + * @return velocity increment in a state-dependent inertial frame + * @see #getInertialFrame() + */ + public FieldVector3D getInertialDV() { + return inertialDV; + } + + /** + * Get the inertial frame in which the velocity increment is defined. + * + * @return inertial frame in which the velocity increment is defined + * @see #getInertialDV() + */ + public Frame getInertialFrame() { + return state0.getFrame(); + } + + /** + * Compute the effect of the maneuver on an orbit. + * + * @param orbit1 original orbit at t₁, without maneuver + * @return orbit at t₁, taking the maneuver into account if t₁ > t₀ + * @see #apply(SpacecraftState) + * @see #getJacobian(Orbit, PositionAngle, double[][]) + */ + public FieldOrbit apply(final FieldOrbit orbit1) { + + if (orbit1.getDate().compareTo(state0.getDate()) <= 0) { + // the maneuver has not occurred yet, don't change anything + return orbit1; + } + + return orbit1.getType().convertType(updateOrbit(orbit1)); + + } + + /** + * Compute the effect of the maneuver on a spacecraft state. + * + * @param state1 original spacecraft state at t₁, without maneuver + * @return spacecraft state at t₁, taking the maneuver into account if t₁ + * > t₀ + * @see #apply(Orbit) + * @see #getJacobian(Orbit, PositionAngle, double[][]) + */ + public FieldSpacecraftState apply(final FieldSpacecraftState state1) { + + if (state1.getDate().compareTo(state0.getDate()) <= 0) { + // the maneuver has not occurred yet, don't change anything + return state1; + } + + return new FieldSpacecraftState<>(state1.getOrbit().getType() + .convertType(updateOrbit(state1.getOrbit())), state1.getAttitude(), + updateMass(state1.getMass())); + + } + + /** + * Compute the effect of the maneuver on an orbit. + * + * @param orbit1 original orbit at t₁, without maneuver + * @return orbit at t₁, always taking the maneuver into account, always in + * the internal type + */ + private FieldOrbit updateOrbit(final FieldOrbit orbit1) { + + // compute maneuver effect + final T dt = orbit1.getDate().durationFrom(state0.getDate()); + final T x = inertialDV.getX(); + final T y = inertialDV.getY(); + final T z = inertialDV.getZ(); + final Field field = z.getField(); + final T[] delta = MathArrays.buildArray(field, 6); + for (int i = 0; i < delta.length; ++i) { + delta[i] = + j0[i][0].multiply(x) + .add(j0[i][1].multiply(y).add(j0[i][2].multiply(z))); + } + delta[5] = delta[5].add(ksi.multiply(delta[0]).multiply(dt)); + + // convert current orbital state to Keplerian or equinoctial elements + final T[] parameters = MathArrays.buildArray(field, 6); + type.mapOrbitToArray(type.convertType(orbit1), PositionAngle.MEAN, + parameters, null); + for (int i = 0; i < delta.length; ++i) { + parameters[i] = parameters[i].add(delta[i]); + } + + // build updated orbit as Keplerian or equinoctial elements + return type.mapArrayToOrbit(parameters, null, PositionAngle.MEAN, + orbit1.getDate(), orbit1.getMu(), + orbit1.getFrame()); + + } + + /** + * Compute the Jacobian of the orbit with respect to maneuver parameters. + *

+ * The Jacobian matrix is a 6x4 matrix. Element jacobian[i][j] corresponds + * to the partial derivative of orbital parameter i with respect to maneuver + * parameter j. The rows order is the same order as used in + * {@link Orbit#getJacobianWrtCartesian(PositionAngle, double[][]) + * Orbit.getJacobianWrtCartesian} method. Columns (0, 1, 2) correspond to + * the velocity increment coordinates (ΔVx, ΔVy, + * ΔVz) in the inertial frame returned by + * {@link #getInertialFrame()}, and column 3 corresponds to the maneuver + * date t₀. + *

+ * + * @param orbit1 original orbit at t₁, without maneuver + * @param positionAngle type of the position angle to use + * @param jacobian placeholder 6x4 (or larger) matrix to be filled with the + * Jacobian, if matrix is larger than 6x4, only the 6x4 upper left + * corner will be modified + * @see #apply(Orbit) + */ + public void getJacobian(final FieldOrbit orbit1, + final PositionAngle positionAngle, + final T[][] jacobian) { + + final T dt = orbit1.getDate().durationFrom(state0.getDate()); + if (dt.getReal() < 0) { + // the maneuver has not occurred yet, Jacobian is null + for (int i = 0; i < 6; ++i) { + Arrays.fill(jacobian[i], 0, 4, dt.getField().getZero()); + } + return; + } + + // derivatives of Keplerian/equinoctial elements with respect to + // velocity + // increment + final T x = inertialDV.getX(); + final T y = inertialDV.getY(); + final T z = inertialDV.getZ(); + for (int i = 0; i < 6; ++i) { + System.arraycopy(j0[i], 0, jacobian[i], 0, 3); + } + for (int j = 0; j < 3; ++j) { + jacobian[5][j] = + jacobian[5][j].add(ksi.multiply(dt).multiply(j0[0][j])); + } + + // derivatives of Keplerian/equinoctial elements with respect to date + evaluateJ0Dot(); + for (int i = 0; i < 6; ++i) { + jacobian[i][3] = + j0Dot[i][0].multiply(x).add(j0Dot[i][1].multiply(y)) + .add(j0Dot[i][2].multiply(z)); + } + final T da = + j0[0][0].multiply(x).add(j0[0][1].multiply(y)) + .add(j0[0][2].multiply(z)); + jacobian[5][3] = + jacobian[5][3] + .add(ksi.multiply(jacobian[0][3].multiply(dt).subtract(da))); + + if (orbit1.getType() != type || positionAngle != PositionAngle.MEAN) { + + // convert to derivatives of Cartesian parameters + final Field field = x.getField(); + final T[][] j2 = MathArrays.buildArray(field, 6, 6); + final T[][] pvJacobian = MathArrays.buildArray(field, 6, 4); + final FieldOrbit updated = updateOrbit(orbit1); + updated.getJacobianWrtParameters(PositionAngle.MEAN, j2); + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 4; ++j) { + pvJacobian[i][j] = + j2[i][0].multiply(jacobian[0][j]) + .add(j2[i][1].multiply(jacobian[1][j])) + .add(j2[i][2].multiply(jacobian[2][j])) + .add(j2[i][3].multiply(jacobian[3][j])) + .add(j2[i][4].multiply(jacobian[4][j])) + .add(j2[i][5].multiply(jacobian[5][j])); + } + } + + // convert to derivatives of specified parameters + final T[][] j3 = MathArrays.buildArray(field, 6, 6); + orbit1.getType().convertType(updated) + .getJacobianWrtCartesian(positionAngle, j3); + for (int j = 0; j < 4; ++j) { + for (int i = 0; i < 6; ++i) { + jacobian[i][j] = + j3[i][0].multiply(pvJacobian[0][j]) + .add(j3[i][1].multiply(pvJacobian[1][j])) + .add(j3[i][2].multiply(pvJacobian[2][j])) + .add(j3[i][3].multiply(pvJacobian[3][j])) + .add(j3[i][4].multiply(pvJacobian[4][j])) + .add(j3[i][5].multiply(pvJacobian[5][j])); + } + } + + } + + } + + /** + * Lazy evaluation of the initial Jacobian time derivative. + */ + private void evaluateJ0Dot() { + + if (j0Dot == null) { + final Field field = massRatio.getField(); + final T zero = field.getZero(); + j0Dot = MathArrays.buildArray(field, 6, 3); + final T dt = + zero.add(1.0e-5) + .divide(state0.getOrbit().getKeplerianMeanMotion()); + final FieldOrbit orbit = type.convertType(state0.getOrbit()); + + // compute shifted Jacobians + final T[][] j0m1 = MathArrays.buildArray(field, 6, 6); + orbit.shiftedBy(dt.multiply(-1)) + .getJacobianWrtCartesian(PositionAngle.MEAN, j0m1); + final T[][] j0p1 = MathArrays.buildArray(field, 6, 6); + orbit.shiftedBy(dt.multiply(+1)) + .getJacobianWrtCartesian(PositionAngle.MEAN, j0p1); + + // evaluate derivative by finite differences + for (int i = 0; i < j0Dot.length; ++i) { + final T[] m1Row = j0m1[i]; + final T[] p1Row = j0p1[i]; + final T[] j0DotRow = j0Dot[i]; + for (int j = 0; j < 3; ++j) { + j0DotRow[j] = + (p1Row[j + 3].subtract(m1Row[j + 3])) + .divide(dt.multiply(2)); + } + } + + } + + } + + /** + * Update a spacecraft mass due to maneuver. + * + * @param mass masse before maneuver + * @return mass after maneuver + */ + public T updateMass(final T mass) { + return massRatio.multiply(mass); + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java index ed0e442d3..c0785c3c7 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldAbstractGNSSPropagator.java @@ -42,348 +42,412 @@ import org.orekit.utils.FieldPVCoordinates; * {@link FieldAbstractAnalyticalPropagator} methods for specific GNSS * propagators. *

- * + * * @author Pascal Parraud * @author Nicolas Fialton (field translation) */ public abstract class FieldAbstractGNSSPropagator> - extends FieldAbstractAnalyticalPropagator { - - // Data used to solve Kepler's equation - /** First coefficient to compute Kepler equation solver starter. */ - private static final double A; - - /** Second coefficient to compute Kepler equation solver starter. */ - private static final double B; - - static { - final double k1 = 3 * FastMath.PI + 2; - final double k2 = FastMath.PI - 1; - final double k3 = 6 * FastMath.PI - 1; - A = 3 * k2 * k2 / k1; - B = k3 * k3 / (6 * k1); - } - - /** The GNSS orbital elements used. */ - private final FieldGNSSOrbitalElements gnssOrbit; - - /** Mean angular velocity of the Earth. */ - private final double av; - - /** Duration of the GNSS cycle in seconds. */ - private final double cycleDuration; - - /** The spacecraft mass (kg). */ - private final T mass; - - /** The Earth gravity coefficient used for GNSS propagation. */ - private final T mu; - - /** The ECI frame used for GNSS propagation. */ - private final Frame eci; - - /** The ECEF frame used for GNSS propagation. */ - private final Frame ecef; - - /** - * Build a new instance. - * - * @param field - * @param gnssOrbit the common Field GNSS orbital elements to be used by the Field - * Abstract GNSS propagator - * @param attitudeProvider provider for attitude computation - * @param eci the ECI frame used for GNSS propagation - * @param ecef the ECEF frame used for GNSS propagation - * @param mass the spacecraft mass (kg) - * @param av mean angular velocity of the Earth (rad/s) - * @param cycleDuration duration of the GNSS cycle in seconds - * @param mu the Earth gravity coefficient used for GNSS - * propagation - */ - - protected FieldAbstractGNSSPropagator(final Field field, final FieldGNSSOrbitalElements gnssOrbit, - final AttitudeProvider attitudeProvider, final Frame eci, final Frame ecef, final double mass, - final double av, final double cycleDuration, final double mu) { - super(field, attitudeProvider); - this.gnssOrbit = gnssOrbit; - this.av = av; - this.cycleDuration = cycleDuration; - this.mass = field.getZero().add(mass); - this.mu = field.getZero().add(mu); - // Sets the Earth Centered Inertial frame - this.eci = eci; - // Sets the Earth Centered Earth Fixed frame - this.ecef = ecef; - // Sets the start date as the date of the orbital elements - setStartDate(gnssOrbit.getDate()); - } - - /** - * Get the duration from GNSS Reference epoch. - *

- * This takes the GNSS week roll-over into account. - *

- * - * @param date the considered date - * @return the duration from GNSS orbit Reference epoch (s) - */ - private T getTk(final FieldAbsoluteDate date) { - // Time from ephemeris reference epoch - T tk = date.durationFrom(gnssOrbit.getDate()); - // Adjusts the time to take roll over week into account - while (tk.getReal() > 0.5 * cycleDuration) { - tk = tk.subtract(cycleDuration); - } - while (tk.getReal() < -0.5 * cycleDuration) { - tk = tk.add(cycleDuration); - } - // Returns the time from ephemeris reference epoch - return tk; - } - - /** - * Gets the FieldPVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}. - * - *

- * The algorithm uses automatic differentiation to compute velocity and - * acceleration. - *

- * - * @param date the computation date - * @return the GNSS SV FieldPVCoordinates in {@link #getECEF() ECEF frame} - */ - public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { - // Field - final Field field = date.getField(); - // Duration from GNSS ephemeris Reference date - final FieldUnivariateDerivative2 tk = new FieldUnivariateDerivative2<>(getTk(date), field.getOne(), - field.getZero()); - // Mean anomaly - final FieldUnivariateDerivative2 mk = tk.multiply(gnssOrbit.getMeanMotion()).add(gnssOrbit.getM0()); - // Eccentric Anomaly - final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk); - // True Anomaly - final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek); - // Argument of Latitude - final FieldUnivariateDerivative2 phik = vk.add(gnssOrbit.getPa()); - final FieldUnivariateDerivative2 twoPhik = phik.multiply(2); - final FieldUnivariateDerivative2 c2phi = twoPhik.cos(); - final FieldUnivariateDerivative2 s2phi = twoPhik.sin(); - // Argument of Latitude Correction - final FieldUnivariateDerivative2 dphik = c2phi.multiply(gnssOrbit.getCuc()) - .add(s2phi.multiply(gnssOrbit.getCus())); - // Radius Correction - final FieldUnivariateDerivative2 drk = c2phi.multiply(gnssOrbit.getCrc()) - .add(s2phi.multiply(gnssOrbit.getCrs())); - // Inclination Correction - final FieldUnivariateDerivative2 dik = c2phi.multiply(gnssOrbit.getCic()) - .add(s2phi.multiply(gnssOrbit.getCis())); - // Corrected Argument of Latitude - final FieldUnivariateDerivative2 uk = phik.add(dphik); - // Corrected Radius - final FieldUnivariateDerivative2 rk = ek.cos().multiply((gnssOrbit.getE()).negate()).add(1) - .multiply(gnssOrbit.getSma()).add(drk); - // Corrected Inclination - final FieldUnivariateDerivative2 ik = tk.multiply(gnssOrbit.getIDot()).add(gnssOrbit.getI0()).add(dik); - final FieldUnivariateDerivative2 cik = ik.cos(); - // Positions in orbital plane - final FieldUnivariateDerivative2 xk = uk.cos().multiply(rk); - final FieldUnivariateDerivative2 yk = uk.sin().multiply(rk); - // Corrected longitude of ascending node - final FieldUnivariateDerivative2 omk = tk.multiply((gnssOrbit.getOmegaDot()).subtract(av)) - .add(gnssOrbit.getOmega0().subtract(gnssOrbit.getTime().multiply(av))); - final FieldUnivariateDerivative2 comk = omk.cos(); - final FieldUnivariateDerivative2 somk = omk.sin(); - // returns the Earth-fixed coordinates - final FieldVector3D> positionwithDerivatives = new FieldVector3D<>( - xk.multiply(comk).subtract(yk.multiply(somk).multiply(cik)), - xk.multiply(somk).add(yk.multiply(comk).multiply(cik)), yk.multiply(ik.sin())); - return new FieldPVCoordinates( - new FieldVector3D(positionwithDerivatives.getX().getValue(), - positionwithDerivatives.getY().getValue(), positionwithDerivatives.getZ().getValue()), - new FieldVector3D(positionwithDerivatives.getX().getFirstDerivative(), - positionwithDerivatives.getY().getFirstDerivative(), - positionwithDerivatives.getZ().getFirstDerivative()), - new FieldVector3D(positionwithDerivatives.getX().getSecondDerivative(), - positionwithDerivatives.getY().getSecondDerivative(), - positionwithDerivatives.getZ().getSecondDerivative())); - } - - /** - * Gets eccentric anomaly from mean anomaly. - *

- * The algorithm used to solve the Kepler equation has been published in: - * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. Gooding, - * Celestial Mechanics 38 (1986) 307-334 - *

- *

- * It has been copied from the OREKIT library (KeplerianOrbit class). - *

- * - * @param mk the mean anomaly (rad) - * @return the eccentric anomaly (rad) - */ - private FieldUnivariateDerivative2 getEccentricAnomaly(final FieldUnivariateDerivative2 mk) { - - final T zero = mk.getValue().getField().getZero(); - // reduce M to [-PI PI] interval - final FieldUnivariateDerivative2 reducedM = new FieldUnivariateDerivative2( - MathUtils.normalizeAngle(mk.getValue(), zero), mk.getFirstDerivative(), mk.getSecondDerivative()); - - // compute start value according to A. W. Odell and R. H. Gooding S12 starter - FieldUnivariateDerivative2 ek; - if (FastMath.abs(reducedM.getValue().getReal()) < 1.0 / 6.0) { - if (FastMath.abs(reducedM.getValue().getReal()) < Precision.SAFE_MIN) { - // this is an Orekit change to the S12 starter. - // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN - // appearing later in - // the computation. As in this case E and M are almost equal, we initialize ek - // with reducedM - ek = reducedM; - } else { - // this is the standard S12 starter - ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(gnssOrbit.getE())); - } - } else { - if (reducedM.getValue().getReal() < 0) { - final FieldUnivariateDerivative2 w = reducedM.add(FastMath.PI); - ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM) - .multiply(gnssOrbit.getE())); - } else { - final FieldUnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI); - ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM) - .multiply(gnssOrbit.getE())); - } - } - - final T e1 = gnssOrbit.getE().negate().add(1.0); - final boolean noCancellationRisk = (e1.getReal() - + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; - - // perform two iterations, each consisting of one Halley step and one - // Newton-Raphson step - for (int j = 0; j < 2; ++j) { - final FieldUnivariateDerivative2 f; - FieldUnivariateDerivative2 fd; - final FieldUnivariateDerivative2 fdd = ek.sin().multiply(gnssOrbit.getE()); - final FieldUnivariateDerivative2 fddd = ek.cos().multiply(gnssOrbit.getE()); - if (noCancellationRisk) { - f = ek.subtract(fdd).subtract(reducedM); - fd = fddd.subtract(1).negate(); - } else { - f = eMeSinE(ek).subtract(reducedM); - final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); - fd = s.multiply(s).multiply(gnssOrbit.getE().multiply(2.0)).add(e1); - } - final FieldUnivariateDerivative2 dee = f.multiply(fd) - .divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd))); - - // update eccentric anomaly, using expressions that limit underflow problems - final FieldUnivariateDerivative2 w = fd - .add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3)))); - fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); - ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd)); - } - - // expand the result back to original range - ek = ek.add((mk.getValue()).subtract(reducedM.getValue())); - - // Returns the eccentric anomaly - return ek; - } - - /** - * Accurate computation of E - e sin(E). - * - * @param E eccentric anomaly - * @return E - e sin(E) - */ - private FieldUnivariateDerivative2 eMeSinE(final FieldUnivariateDerivative2 E) { - FieldUnivariateDerivative2 x = E.sin().multiply(gnssOrbit.getE().negate().add(1)); - final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); - FieldUnivariateDerivative2 term = E; - FieldUnivariateDerivative2 d = E.getField().getZero(); - // the inequality test below IS intentional and should NOT be replaced by a - // check with a small tolerance - for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); !(x.getValue()).equals(x0.getValue());) { - d = d.add(2); - term = term.multiply(mE2.divide(d.multiply(d.add(1)))); - x0 = x; - x = x.subtract(term); - } - return x; - } - - /** - * Gets true anomaly from eccentric anomaly. - * - * @param ek the eccentric anomaly (rad) - * @return the true anomaly (rad) - */ - private FieldUnivariateDerivative2 getTrueAnomaly(final FieldUnivariateDerivative2 ek) { - final FieldUnivariateDerivative2 svk = ek.sin() - .multiply(FastMath.sqrt((gnssOrbit.getE().multiply(gnssOrbit.getE())).negate().add(1.0))); - final FieldUnivariateDerivative2 cvk = ek.cos().subtract(gnssOrbit.getE()); - return svk.atan2(cvk); - } - - /** {@inheritDoc} */ - protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, final T[] parameters) { - // Gets the PVCoordinates in ECEF frame - final FieldPVCoordinates pvaInECEF = propagateInEcef(date); - // Transforms the PVCoordinates to ECI frame - final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); - // Returns the Cartesian orbit - return new FieldCartesianOrbit(pvaInECI, eci, date, mu); - } - - /** - * Get the Earth gravity coefficient used for GNSS propagation. - * - * @return the Earth gravity coefficient. - */ - public T getMU() { - return mu; - } - - /** {@inheritDoc} */ - public Frame getFrame() { - return eci; - } - - /** {@inheritDoc} */ - protected T getMass(final FieldAbsoluteDate date) { - return mass; - } - - /** {@inheritDoc} */ - public void resetInitialState(final FieldSpacecraftState state) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - /** {@inheritDoc} */ - protected void resetIntermediateState(final FieldSpacecraftState state, final boolean forward) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - /** - * Gets the Earth Centered Inertial frame used to propagate the orbit. - * - * @return the ECI frame - */ - public Frame getECI() { - return eci; - } - - /** - * Gets the Earth Centered Earth Fixed frame used to propagate GNSS orbits - * according to the Interface Control Document. - * - * @return the ECEF frame - */ - public Frame getECEF() { - return ecef; - } + extends FieldAbstractAnalyticalPropagator { + + // Data used to solve Kepler's equation + /** First coefficient to compute Kepler equation solver starter. */ + private static final double A; + + /** Second coefficient to compute Kepler equation solver starter. */ + private static final double B; + + static { + final double k1 = 3 * FastMath.PI + 2; + final double k2 = FastMath.PI - 1; + final double k3 = 6 * FastMath.PI - 1; + A = 3 * k2 * k2 / k1; + B = k3 * k3 / (6 * k1); + } + + /** The GNSS orbital elements used. */ + private final FieldGNSSOrbitalElements gnssOrbit; + + /** Mean angular velocity of the Earth. */ + private final double av; + + /** Duration of the GNSS cycle in seconds. */ + private final double cycleDuration; + + /** The spacecraft mass (kg). */ + private final T mass; + + /** The Earth gravity coefficient used for GNSS propagation. */ + private final T mu; + + /** The ECI frame used for GNSS propagation. */ + private final Frame eci; + + /** The ECEF frame used for GNSS propagation. */ + private final Frame ecef; + + /** + * Build a new instance. + * + * @param field + * @param gnssOrbit the common Field GNSS orbital elements to be used by the + * Field Abstract GNSS propagator + * @param attitudeProvider provider for attitude computation + * @param eci the ECI frame used for GNSS propagation + * @param ecef the ECEF frame used for GNSS propagation + * @param mass the spacecraft mass (kg) + * @param av mean angular velocity of the Earth (rad/s) + * @param cycleDuration duration of the GNSS cycle in seconds + * @param mu the Earth gravity coefficient used for GNSS propagation + */ + + protected FieldAbstractGNSSPropagator(final Field field, + final FieldGNSSOrbitalElements gnssOrbit, + final AttitudeProvider attitudeProvider, + final Frame eci, final Frame ecef, + final double mass, final double av, + final double cycleDuration, + final double mu) { + super(field, attitudeProvider); + this.gnssOrbit = gnssOrbit; + this.av = av; + this.cycleDuration = cycleDuration; + this.mass = field.getZero().add(mass); + this.mu = field.getZero().add(mu); + // Sets the Earth Centered Inertial frame + this.eci = eci; + // Sets the Earth Centered Earth Fixed frame + this.ecef = ecef; + // Sets the start date as the date of the orbital elements + setStartDate(gnssOrbit.getDate()); + } + + /** + * Get the duration from GNSS Reference epoch. + *

+ * This takes the GNSS week roll-over into account. + *

+ * + * @param date the considered date + * @return the duration from GNSS orbit Reference epoch (s) + */ + private T getTk(final FieldAbsoluteDate date) { + // Time from ephemeris reference epoch + T tk = date.durationFrom(gnssOrbit.getDate()); + // Adjusts the time to take roll over week into account + while (tk.getReal() > 0.5 * cycleDuration) { + tk = tk.subtract(cycleDuration); + } + while (tk.getReal() < -0.5 * cycleDuration) { + tk = tk.add(cycleDuration); + } + // Returns the time from ephemeris reference epoch + return tk; + } + + /** + * Gets the FieldPVCoordinates of the GNSS SV in {@link #getECEF() ECEF + * frame}. + *

+ * The algorithm uses automatic differentiation to compute velocity and + * acceleration. + *

+ * + * @param date the computation date + * @return the GNSS SV FieldPVCoordinates in {@link #getECEF() ECEF frame} + */ + public FieldPVCoordinates + propagateInEcef(final FieldAbsoluteDate date) { + // Field + final Field field = date.getField(); + // Duration from GNSS ephemeris Reference date + final FieldUnivariateDerivative2 tk = + new FieldUnivariateDerivative2<>(getTk(date), field.getOne(), + field.getZero()); + // Mean anomaly + final FieldUnivariateDerivative2 mk = + tk.multiply(gnssOrbit.getMeanMotion()).add(gnssOrbit.getM0()); + // Eccentric Anomaly + final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk); + // True Anomaly + final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek); + // Argument of Latitude + final FieldUnivariateDerivative2 phik = vk.add(gnssOrbit.getPa()); + final FieldUnivariateDerivative2 twoPhik = phik.multiply(2); + final FieldUnivariateDerivative2 c2phi = twoPhik.cos(); + final FieldUnivariateDerivative2 s2phi = twoPhik.sin(); + // Argument of Latitude Correction + final FieldUnivariateDerivative2 dphik = + c2phi.multiply(gnssOrbit.getCuc()) + .add(s2phi.multiply(gnssOrbit.getCus())); + // Radius Correction + final FieldUnivariateDerivative2 drk = + c2phi.multiply(gnssOrbit.getCrc()) + .add(s2phi.multiply(gnssOrbit.getCrs())); + // Inclination Correction + final FieldUnivariateDerivative2 dik = + c2phi.multiply(gnssOrbit.getCic()) + .add(s2phi.multiply(gnssOrbit.getCis())); + // Corrected Argument of Latitude + final FieldUnivariateDerivative2 uk = phik.add(dphik); + // Corrected Radius + final FieldUnivariateDerivative2 rk = + ek.cos().multiply((gnssOrbit.getE()).negate()).add(1) + .multiply(gnssOrbit.getSma()).add(drk); + // Corrected Inclination + final FieldUnivariateDerivative2 ik = + tk.multiply(gnssOrbit.getIDot()).add(gnssOrbit.getI0()).add(dik); + final FieldUnivariateDerivative2 cik = ik.cos(); + // Positions in orbital plane + final FieldUnivariateDerivative2 xk = uk.cos().multiply(rk); + final FieldUnivariateDerivative2 yk = uk.sin().multiply(rk); + // Corrected longitude of ascending node + final FieldUnivariateDerivative2 omk = + tk.multiply((gnssOrbit.getOmegaDot()).subtract(av)).add(gnssOrbit + .getOmega0().subtract(gnssOrbit.getTime().multiply(av))); + final FieldUnivariateDerivative2 comk = omk.cos(); + final FieldUnivariateDerivative2 somk = omk.sin(); + // returns the Earth-fixed coordinates + final FieldVector3D> positionwithDerivatives = + new FieldVector3D<>(xk.multiply(comk) + .subtract(yk.multiply(somk).multiply(cik)), + xk.multiply(somk) + .add(yk.multiply(comk).multiply(cik)), + yk.multiply(ik.sin())); + return new FieldPVCoordinates(new FieldVector3D(positionwithDerivatives + .getX().getValue(), positionwithDerivatives.getY().getValue(), + positionwithDerivatives + .getZ() + .getValue()), + new FieldVector3D(positionwithDerivatives + .getX().getFirstDerivative(), + positionwithDerivatives + .getY() + .getFirstDerivative(), + positionwithDerivatives + .getZ() + .getFirstDerivative()), + new FieldVector3D(positionwithDerivatives + .getX().getSecondDerivative(), + positionwithDerivatives + .getY() + .getSecondDerivative(), + positionwithDerivatives + .getZ() + .getSecondDerivative())); + } + + /** + * Gets eccentric anomaly from mean anomaly. + *

+ * The algorithm used to solve the Kepler equation has been published in: + * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. + * Gooding, Celestial Mechanics 38 (1986) 307-334 + *

+ *

+ * It has been copied from the OREKIT library (KeplerianOrbit class). + *

+ * + * @param mk the mean anomaly (rad) + * @return the eccentric anomaly (rad) + */ + private FieldUnivariateDerivative2 + getEccentricAnomaly(final FieldUnivariateDerivative2 mk) { + + final T zero = mk.getValue().getField().getZero(); + // reduce M to [-PI PI] interval + final FieldUnivariateDerivative2 reducedM = + new FieldUnivariateDerivative2(MathUtils + .normalizeAngle(mk.getValue(), zero), mk.getFirstDerivative(), + mk.getSecondDerivative()); + + // compute start value according to A. W. Odell and R. H. Gooding S12 + // starter + FieldUnivariateDerivative2 ek; + if (FastMath.abs(reducedM.getValue().getReal()) < 1.0 / 6.0) { + if (FastMath + .abs(reducedM.getValue().getReal()) < Precision.SAFE_MIN) { + // this is an Orekit change to the S12 starter. + // If reducedM is 0.0, the derivative of cbrt is infinite which + // induces NaN + // appearing later in + // the computation. As in this case E and M are almost equal, we + // initialize ek + // with reducedM + ek = reducedM; + } else { + // this is the standard S12 starter + ek = + reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM) + .multiply(gnssOrbit.getE())); + } + } else { + if (reducedM.getValue().getReal() < 0) { + final FieldUnivariateDerivative2 w = + reducedM.add(FastMath.PI); + ek = + reducedM.add(w.multiply(-A).divide(w.subtract(B)) + .subtract(FastMath.PI).subtract(reducedM) + .multiply(gnssOrbit.getE())); + } else { + final FieldUnivariateDerivative2 minusW = + reducedM.subtract(FastMath.PI); + ek = + reducedM.add(minusW.multiply(A).divide(minusW.add(B)) + .add(FastMath.PI).subtract(reducedM) + .multiply(gnssOrbit.getE())); + } + } + + final T e1 = gnssOrbit.getE().negate().add(1.0); + final boolean noCancellationRisk = + (e1.getReal() + + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; + + // perform two iterations, each consisting of one Halley step and one + // Newton-Raphson step + for (int j = 0; j < 2; ++j) { + final FieldUnivariateDerivative2 f; + FieldUnivariateDerivative2 fd; + final FieldUnivariateDerivative2 fdd = + ek.sin().multiply(gnssOrbit.getE()); + final FieldUnivariateDerivative2 fddd = + ek.cos().multiply(gnssOrbit.getE()); + if (noCancellationRisk) { + f = ek.subtract(fdd).subtract(reducedM); + fd = fddd.subtract(1).negate(); + } else { + f = eMeSinE(ek).subtract(reducedM); + final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); + fd = + s.multiply(s).multiply(gnssOrbit.getE().multiply(2.0)) + .add(e1); + } + final FieldUnivariateDerivative2 dee = + f.multiply(fd).divide(f.multiply(0.5).multiply(fdd) + .subtract(fd.multiply(fd))); + + // update eccentric anomaly, using expressions that limit underflow + // problems + final FieldUnivariateDerivative2 w = + fd.add(dee.multiply(0.5) + .multiply(fdd.add(dee.multiply(fdd).divide(3)))); + fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); + ek = + ek.subtract(f.subtract(dee.multiply(fd.subtract(w))) + .divide(fd)); + } + + // expand the result back to original range + ek = ek.add((mk.getValue()).subtract(reducedM.getValue())); + + // Returns the eccentric anomaly + return ek; + } + + /** + * Accurate computation of E - e sin(E). + * + * @param E eccentric anomaly + * @return E - e sin(E) + */ + private FieldUnivariateDerivative2 + eMeSinE(final FieldUnivariateDerivative2 E) { + FieldUnivariateDerivative2 x = + E.sin().multiply(gnssOrbit.getE().negate().add(1)); + final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); + FieldUnivariateDerivative2 term = E; + FieldUnivariateDerivative2 d = E.getField().getZero(); + // the inequality test below IS intentional and should NOT be replaced + // by a + // check with a small tolerance + for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); + !(x.getValue()).equals(x0.getValue());) { + d = d.add(2); + term = term.multiply(mE2.divide(d.multiply(d.add(1)))); + x0 = x; + x = x.subtract(term); + } + return x; + } + + /** + * Gets true anomaly from eccentric anomaly. + * + * @param ek the eccentric anomaly (rad) + * @return the true anomaly (rad) + */ + private FieldUnivariateDerivative2 + getTrueAnomaly(final FieldUnivariateDerivative2 ek) { + final FieldUnivariateDerivative2 svk = + ek.sin() + .multiply(FastMath + .sqrt((gnssOrbit.getE().multiply(gnssOrbit.getE())).negate() + .add(1.0))); + final FieldUnivariateDerivative2 cvk = + ek.cos().subtract(gnssOrbit.getE()); + return svk.atan2(cvk); + } + + /** {@inheritDoc} */ + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, + final T[] parameters) { + // Gets the PVCoordinates in ECEF frame + final FieldPVCoordinates pvaInECEF = propagateInEcef(date); + // Transforms the PVCoordinates to ECI frame + final FieldPVCoordinates pvaInECI = + ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); + // Returns the Cartesian orbit + return new FieldCartesianOrbit(pvaInECI, eci, date, mu); + } + + /** + * Get the Earth gravity coefficient used for GNSS propagation. + * + * @return the Earth gravity coefficient. + */ + public T getMU() { + return mu; + } + + /** {@inheritDoc} */ + public Frame getFrame() { + return eci; + } + + /** {@inheritDoc} */ + protected T getMass(final FieldAbsoluteDate date) { + return mass; + } + + /** {@inheritDoc} */ + public void resetInitialState(final FieldSpacecraftState state) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected void resetIntermediateState(final FieldSpacecraftState state, + final boolean forward) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** + * Gets the Earth Centered Inertial frame used to propagate the orbit. + * + * @return the ECI frame + */ + public Frame getECI() { + return eci; + } + + /** + * Gets the Earth Centered Earth Fixed frame used to propagate GNSS orbits + * according to the Interface Control Document. + * + * @return the ECEF frame + */ + public Frame getECEF() { + return ecef; + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java index 962e587fb..d4f2af92d 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouAlmanac.java @@ -23,7 +23,6 @@ import org.orekit.annotation.DefaultDataContext; import org.orekit.data.DataContext; import org.orekit.gnss.BeidouAlmanac; import org.orekit.gnss.SatelliteSystem; -import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; /** @@ -31,315 +30,317 @@ import org.orekit.time.FieldAbsoluteDate; * * @see "BeiDou Navigation Satellite System, Signal In Space, Interface Control * Document, Version 2.1, Table 5-12" - * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) - * */ -public class FieldBeidouAlmanac> implements FieldBeidouOrbitalElements { - - private final T zero; - /** PRN number. */ - private final int prn; - - /** Health status. */ - private final int health; - - /** BeiDou week. */ - private final int week; - - /** Time of applicability. */ - private final T toa; - - /** Semi-major axis. */ - private final T sma; - - /** Eccentricity. */ - private final T ecc; - - /** Inclination. */ - private final T inc; - - /** Longitude of Orbital Plane. */ - private final T om0; - - /** Rate of Right Ascension. */ - private final T dom; - - /** Argument of perigee. */ - private final T aop; - - /** Mean anomaly. */ - private final T anom; - - /** Zeroth order clock correction. */ - private final T af0; - - /** First order clock correction. */ - private final T af1; - - /** Date of validity. */ - private final FieldAbsoluteDate date; - - /** - * Build a new almanac. - * - *

- * This method uses the {@link DataContext#getDefault() default data context}. - * - * @param prn the PRN number - * @param week the BeiDou week - * @param toa the Almanac Time of Applicability (s) - * @param sqa the Square Root of Semi-Major Axis (m^1/2) - * @param ecc the eccentricity - * @param inc0 the orbit reference inclination 0.0 for GEO satellites and 0.30 - * * BEIDOU_PI for MEO/IGSO satellites (rad) - * @param dinc the correction of orbit reference inclination at reference time - * (rad) - * @param om0 the geographic longitude of the orbital plane at the weekly - * epoch (rad) - * @param dom the Rate of Right Ascension (rad/s) - * @param aop the Argument of Perigee (rad) - * @param anom the Mean Anomaly (rad) - * @param af0 the Zeroth Order Clock Correction (s) - * @param af1 the First Order Clock Correction (s/s) - * @param health the Health status - * @see #BeidouAlmanac(int, int, T, T, T, T, T, T, T, T, T, T, T, int, - * AbsoluteDate) - */ - @DefaultDataContext - public FieldBeidouAlmanac(final int prn, final int week, final T toa, final T sqa, final T ecc, final T inc0, - final T dinc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, - final int health) { - this(prn, week, toa, sqa, ecc, inc0, dinc, om0, dom, aop, anom, af0, af1, health, - new FieldGNSSDate(week, toa.multiply(1000), SatelliteSystem.BEIDOU, DataContext.getDefault().getTimeScales()) - .getDate()); - } - - /** - * Build a new almanac. - * - * @param prn the PRN number - * @param week the BeiDou week - * @param toa the Almanac Time of Applicability (s) - * @param sqa the Square Root of Semi-Major Axis (m^1/2) - * @param ecc the eccentricity - * @param inc0 the orbit reference inclination 0.0 for GEO satellites and 0.30 - * * BEIDOU_PI for MEO/IGSO satellites (rad) - * @param dinc the correction of orbit reference inclination at reference time - * (rad) - * @param om0 the geographic longitude of the orbital plane at the weekly - * epoch (rad) - * @param dom the Rate of Right Ascension (rad/s) - * @param aop the Argument of Perigee (rad) - * @param anom the Mean Anomaly (rad) - * @param af0 the Zeroth Order Clock Correction (s) - * @param af1 the First Order Clock Correction (s/s) - * @param health the Health status - * @param date that corresponds to {@code week} and {@code toa}. - * @since 10.1 - */ - public FieldBeidouAlmanac(final int prn, final int week, final T toa, final T sqa, final T ecc, final T inc0, - final T dinc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, - final int health, final FieldAbsoluteDate date) { - this.zero = toa.getField().getZero(); - this.prn = prn; - this.week = week; - this.toa = toa; - this.sma = sqa.multiply(sqa); - this.ecc = ecc; - this.inc = inc0.add(dinc); - this.om0 = om0; - this.dom = dom; - this.aop = aop; - this.anom = anom; - this.af0 = af0; - this.af1 = af1; - this.health = health; - this.date = date; - } - - /** - * Constructor - * - * This constructor converts a BeidouAlmanac into a FieldBeidouAlmanac - * - * @param field - * @param almanac a BeidouAlmanac - */ - public FieldBeidouAlmanac(Field field, BeidouAlmanac almanac) { - this.zero = field.getZero(); - this.prn = almanac.getPRN(); - this.week = almanac.getWeek(); - this.toa = zero.add(almanac.getTime()); - this.sma = zero.add(almanac.getSma()); - this.ecc = zero.add(almanac.getE()); - this.inc = zero.add(almanac.getI0()); - this.om0 = zero.add(almanac.getOmega0()); - this.dom = zero.add(almanac.getOmegaDot()); - this.aop = zero.add(almanac.getPa()); - this.anom = zero.add(almanac.getM0()); - this.af0 = zero.add(almanac.getAf0()); - this.af1 = zero.add(almanac.getAf1()); - this.health = almanac.getHealth(); - this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); - } - - @Override - public FieldAbsoluteDate getDate() { - return date; - } - - @Override - public int getPRN() { - return prn; - } - - @Override - public int getWeek() { - return week; - } - - @Override - public T getTime() { - return toa; - } - - @Override - public T getSma() { - return sma; - } - - @Override - public T getMeanMotion() { - final T absA = FastMath.abs(sma); - return FastMath.sqrt(date.getDate().getField().getZero().add(BEIDOU_MU).divide(absA)).divide(absA); - } - - @Override - public T getE() { - return ecc; - } - - @Override - public T getI0() { - return inc; - } - - @Override - public T getIDot() { - return date.getDate().getField().getZero(); - } - - @Override - public T getOmega0() { - return om0; - } - - @Override - public T getOmegaDot() { - return dom; - } - - @Override - public T getPa() { - return aop; - } - - @Override - public T getM0() { - return anom; - } - - @Override - public T getCuc() { - return date.getDate().getField().getZero(); - } - - @Override - public T getCus() { - return date.getDate().getField().getZero(); - } - - @Override - public T getCrc() { - return date.getDate().getField().getZero(); - } - - @Override - public T getCrs() { - return date.getDate().getField().getZero(); - } - - @Override - public T getCic() { - return date.getDate().getField().getZero(); - } - - @Override - public T getCis() { - return date.getDate().getField().getZero(); - } - - @Override - public T getAf0() { - return af0; - } - - @Override - public T getAf1() { - return af1; - } - - /** - * Gets the Health status. - * - * @return the Health status - */ - public int getHealth() { - return health; - } - - @Override - public T getAf2() { - // TODO Auto-generated method stub - return null; - } - - @Override - public T getToc() { - // TODO Auto-generated method stub - return null; - } - - @Override - public int getAODC() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public int getAODE() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public int getIOD() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public T getTGD1() { - // TODO Auto-generated method stub - return null; - } - - @Override - public T getTGD2() { - // TODO Auto-generated method stub - return null; - } +public class FieldBeidouAlmanac> + implements FieldBeidouOrbitalElements { + + /** Field Zero. */ + private final T zero; + + /** PRN number. */ + private final int prn; + + /** Health status. */ + private final int health; + + /** BeiDou week. */ + private final int week; + + /** Time of applicability. */ + private final T toa; + + /** Semi-major axis. */ + private final T sma; + + /** Eccentricity. */ + private final T ecc; + + /** Inclination. */ + private final T inc; + + /** Longitude of Orbital Plane. */ + private final T om0; + + /** Rate of Right Ascension. */ + private final T dom; + + /** Argument of perigee. */ + private final T aop; + + /** Mean anomaly. */ + private final T anom; + + /** Zeroth order clock correction. */ + private final T af0; + + /** First order clock correction. */ + private final T af1; + + /** Date of validity. */ + private final FieldAbsoluteDate date; + + /** + * Build a new almanac. + *

+ * This method uses the {@link DataContext#getDefault() default data + * context}. + * + * @param prn the PRN number + * @param week the BeiDou week + * @param toa the Almanac Time of Applicability (s) + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc0 the orbit reference inclination 0.0 for GEO satellites and + * 0.30 * BEIDOU_PI for MEO/IGSO satellites (rad) + * @param dinc the correction of orbit reference inclination at reference + * time (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @see #BeidouAlmanac(int, int, T, T, T, T, T, T, T, T, T, T, T, int, + * AbsoluteDate) + */ + @DefaultDataContext + public FieldBeidouAlmanac(final int prn, final int week, final T toa, + final T sqa, final T ecc, final T inc0, + final T dinc, final T om0, final T dom, + final T aop, final T anom, final T af0, + final T af1, final int health) { + this(prn, week, toa, sqa, ecc, inc0, dinc, om0, dom, aop, anom, af0, + af1, health, + new FieldGNSSDate(week, toa.multiply(1000), + SatelliteSystem.BEIDOU, + DataContext.getDefault().getTimeScales()) + .getDate()); + } + + /** + * Build a new almanac. + * + * @param prn the PRN number + * @param week the BeiDou week + * @param toa the Almanac Time of Applicability (s) + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc0 the orbit reference inclination 0.0 for GEO satellites and + * 0.30 * BEIDOU_PI for MEO/IGSO satellites (rad) + * @param dinc the correction of orbit reference inclination at reference + * time (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @param date that corresponds to {@code week} and {@code toa}. + * @since 10.1 + */ + public FieldBeidouAlmanac(final int prn, final int week, final T toa, + final T sqa, final T ecc, final T inc0, + final T dinc, final T om0, final T dom, + final T aop, final T anom, final T af0, + final T af1, final int health, + final FieldAbsoluteDate date) { + this.zero = toa.getField().getZero(); + this.prn = prn; + this.week = week; + this.toa = toa; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.inc = inc0.add(dinc); + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.af0 = af0; + this.af1 = af1; + this.health = health; + this.date = date; + } + + /** + * Constructor This constructor converts a BeidouAlmanac into a + * FieldBeidouAlmanac. + * + * @param field + * @param almanac a BeidouAlmanac + */ + public FieldBeidouAlmanac(final Field field, final BeidouAlmanac almanac) { + this.zero = field.getZero(); + this.prn = almanac.getPRN(); + this.week = almanac.getWeek(); + this.toa = zero.add(almanac.getTime()); + this.sma = zero.add(almanac.getSma()); + this.ecc = zero.add(almanac.getE()); + this.inc = zero.add(almanac.getI0()); + this.om0 = zero.add(almanac.getOmega0()); + this.dom = zero.add(almanac.getOmegaDot()); + this.aop = zero.add(almanac.getPa()); + this.anom = zero.add(almanac.getM0()); + this.af0 = zero.add(almanac.getAf0()); + this.af1 = zero.add(almanac.getAf1()); + this.health = almanac.getHealth(); + this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toa; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(date.getDate().getField().getZero().add(BEIDOU_MU) + .divide(absA)).divide(absA); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return date.getDate().getField().getZero(); + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCus() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCrc() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCrs() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCic() { + return date.getDate().getField().getZero(); + } + + @Override + public T getCis() { + return date.getDate().getField().getZero(); + } + + @Override + public T getAf0() { + return af0; + } + + @Override + public T getAf1() { + return af1; + } + + /** + * Gets the Health status. + * + * @return the Health status + */ + public int getHealth() { + return health; + } + + @Override + public T getAf2() { + return zero; + } + + @Override + public T getToc() { + return zero; + } + + @Override + public int getAODC() { + return 0; + } + + @Override + public int getAODE() { + return 0; + } + + @Override + public int getIOD() { + return 0; + } + + @Override + public T getTGD1() { + return zero; + } + + @Override + public T getTGD2() { + return zero; + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouOrbitalElements.java index abd14faed..d0a1b2021 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouOrbitalElements.java @@ -18,15 +18,21 @@ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; -/** This interface provides the minimal set of orbital elements needed by the {@link BeidouPropagator}. -* -* @see Beidou Interface Control Document -* @author Bryan Cazabonne -* @author Nicolas Fialton (field translation) -*/ -public interface FieldBeidouOrbitalElements> extends FieldGNSSOrbitalElements { +/** + * This interface provides the minimal set of orbital elements needed by the + * {@link BeidouPropagator}. + * + * @see Beidou + * Interface Control Document + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + */ +public interface FieldBeidouOrbitalElements> + extends + FieldGNSSOrbitalElements { - // Constants + // Constants /** Earth's universal gravitational parameter for Beidou user in m³/s². */ double BEIDOU_MU = 3.986004418e+14; diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java index d869a2a21..b42e8a405 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldBeidouPropagator.java @@ -37,142 +37,163 @@ import org.orekit.utils.ParameterDriver; * @see Beidou * Interface Control Document - * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) */ -public class FieldBeidouPropagator> extends FieldAbstractGNSSPropagator { +public class FieldBeidouPropagator> + extends + FieldAbstractGNSSPropagator { - // Constants - /** Value of the earth's rotation rate in rad/s. */ - private static final double BEIDOU_AV = 7.2921150e-5; + // Constants + /** Value of the earth's rotation rate in rad/s. */ + private static final double BEIDOU_AV = 7.2921150e-5; - /** Duration of the Beidou cycle in seconds. */ - private static final double BEIDOU_CYCLE_DURATION = FieldBeidouOrbitalElements.BEIDOU_WEEK_IN_SECONDS - * FieldBeidouOrbitalElements.BEIDOU_WEEK_NB; + /** Duration of the Beidou cycle in seconds. */ + private static final double BEIDOU_CYCLE_DURATION = + FieldBeidouOrbitalElements.BEIDOU_WEEK_IN_SECONDS * + FieldBeidouOrbitalElements.BEIDOU_WEEK_NB; - // Fields - /** The Beidou orbital elements used. */ - private final FieldBeidouOrbitalElements bdsOrbit; + // Fields + /** The Beidou orbital elements used. */ + private final FieldBeidouOrbitalElements bdsOrbit; - /** - * Default constructor. - * - *

The Field Beidou orbital elements is the only requested parameter to build a FieldBeidouPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, - final Frames frames)}

- * - * @param field - * @param bdsOrbit the Field Beidou orbital elements to be used by the Field Beidou propagator. - * @see #attitudeProvider(AttitudeProvider provider) - * @see #mass(double mass) - * @see #eci(Frame inertial) - * @see #ecef(Frame bodyFixed) - */ - @DefaultDataContext - public FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit) { - this(field, bdsOrbit, DataContext.getDefault().getFrames()); - } + /** + * Default constructor. + *

+ * The Field Beidou orbital elements is the only requested parameter to + * build a FieldBeidouPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. Another data context can be set using + * {@code FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, final Frames frames)} + *

+ * + * @param field + * @param bdsOrbit the Field Beidou orbital elements to be used by the Field Beidou propagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) + */ + @DefaultDataContext + public FieldBeidouPropagator(final Field field, + final FieldBeidouOrbitalElements bdsOrbit) { + this(field, bdsOrbit, DataContext.getDefault().getFrames()); + } - /** - * Constructor. - * - *

The Field Beidou orbital elements is the only requested parameter to build a FieldBeidouPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, - final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)}

- * - * @param field - * @param bdsOrbit the Field Beidou orbital elements to be used by the Field Beidou propagator. - * @param frames set of frames to use building the propagator. - * @see #attitudeProvider(AttitudeProvider provider) - * @see #mass(double mass) - * @see #eci(Frame inertial) - * @see #ecef(Frame bodyFixed) - */ - public FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, - final Frames frames) { - this(field, bdsOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), - frames.getITRF(IERSConventions.IERS_2010, true)); - } + /** + * Constructor. + *

+ * The Field Beidou orbital elements is the only requested parameter to + * build a FieldBeidouPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. Another data context can be set using + * {@code FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)} + *

+ * + * @param field + * @param bdsOrbit the Field Beidou orbital elements to be used by the Field + * Beidou propagator. + * @param frames set of frames to use building the propagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) + */ + public FieldBeidouPropagator(final Field field, + final FieldBeidouOrbitalElements bdsOrbit, + final Frames frames) { + this(field, bdsOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, + frames.getEME2000(), + frames.getITRF(IERSConventions.IERS_2010, true)); + } - /** - * Constructor. - * - *

The Field Beidou orbital elements is the only requested parameter to build a FieldBeidouPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - * @param field - * @param bdsOrbit the Field Beidou orbital elements to be used by the Field Beidou propagator. - * @param attitudeProvider - * @param mass - * @param eci - * @param ecef - */ - public FieldBeidouPropagator(final Field field, final FieldBeidouOrbitalElements bdsOrbit, - final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { - super(field, bdsOrbit, attitudeProvider, eci, ecef, mass, BEIDOU_AV, BEIDOU_CYCLE_DURATION, - FieldBeidouOrbitalElements.BEIDOU_MU); - // Stores the Beidou orbital elements - this.bdsOrbit = bdsOrbit; - } + /** + * Constructor. + *

+ * The Field Beidou orbital elements is the only requested parameter to + * build a FieldBeidouPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + * @param field + * @param bdsOrbit the Field Beidou orbital elements to be used by the Field + * Beidou propagator. + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldBeidouPropagator(final Field field, + final FieldBeidouOrbitalElements bdsOrbit, + final AttitudeProvider attitudeProvider, + final double mass, final Frame eci, + final Frame ecef) { + super(field, bdsOrbit, attitudeProvider, eci, ecef, mass, BEIDOU_AV, + BEIDOU_CYCLE_DURATION, FieldBeidouOrbitalElements.BEIDOU_MU); + // Stores the Beidou orbital elements + this.bdsOrbit = bdsOrbit; + } - /** - * Get the underlying Field Beidou orbital elements. - * - * @return the underlying Field Beidou orbital elements - */ - public FieldBeidouOrbitalElements getFieldBeidouOrbitalElements() { - return bdsOrbit; - } + /** + * Get the underlying Field Beidou orbital elements. + * + * @return the underlying Field Beidou orbital elements + */ + public FieldBeidouOrbitalElements getFieldBeidouOrbitalElements() { + return bdsOrbit; + } - /** Get the parameters driver for the Field Beidou propagation model. + /** + * Get the parameters driver for the Field Beidou propagation model. + * * @return an empty list. */ - @Override - protected List getParametersDrivers() { - // Field Beidou propagation model does not have parameter drivers. - return Collections.emptyList(); - } + @Override + protected List getParametersDrivers() { + // Field Beidou propagation model does not have parameter drivers. + return Collections.emptyList(); + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAlmanac.java index c79cfa866..753d478ca 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAlmanac.java @@ -31,321 +31,331 @@ import org.orekit.time.TimeScale; * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) - * */ -public class FieldGLONASSAlmanac> implements FieldGLONASSOrbitalElements { - private final T zero; - /** Frequency channel (-7...6). */ - private final int channel; - - /** Health status. */ - private final int health; - - /** Day of Almanac. */ - private final int day; - - /** Month of Almanac. */ - private final int month; - - /** Year of Almanac. */ - private final int year; - - /** Reference time of the almanac. */ - private final T ta; - - /** Greenwich longitude of ascending node of orbit. */ - private final T lambda; - - /** Correction to the mean value of inclination. */ - private final T deltaI; - - /** Argument of perigee. */ - private final T pa; - - /** Eccentricity. */ - private final T ecc; - - /** Correction to the mean value of Draconian period. */ - private final T deltaT; - - /** Rate of change of orbital period. */ - private final T deltaTDot; - - /** Correction from GLONASS to UTC. */ - private final T tGlo2UTC; - - /** Correction to GPS time relative GLONASS. */ - private final T tGPS2Glo; - - /** Correction of time relative to GLONASS system time. */ - private final T tGlo; - - /** GLONASS time scale. */ - private final TimeScale glonass; - - - /** - * Constructor. - * - *

- * This method uses the {@link DataContext#getDefault() default data context}. - * - * @param channel the frequency channel from -7 to 6) - * @param health the Health status - * @param day the day of Almanac - * @param month the month of Almanac - * @param year the year of Almanac - * @param ta the reference time of the almanac (s) - * @param lambda the Greenwich longitude of ascending node of orbit (rad) - * @param deltaI the correction to the mean value of inclination (rad) - * @param pa the argument of perigee (rad) - * @param ecc the eccentricity - * @param deltaT the correction to the mean value of Draconian period (s) - * @param deltaTDot the rate of change of orbital period - * @param tGlo2UTC the correction from GLONASS to UTC (s) - * @param tGPS2Glo the correction to GPS time relative GLONASS (s) - * @param tGlo the correction of time relative to GLONASS system time (s) - * @see #GLONASSAlmanac(int, int, int, int, int, T, T, T, T, T, T, T, T, T, T, - * TimeScale) - */ - @DefaultDataContext - public FieldGLONASSAlmanac(final int channel, final int health, final int day, final int month, final int year, - final T ta, final T lambda, final T deltaI, final T pa, final T ecc, final T deltaT, final T deltaTDot, - final T tGlo2UTC, final T tGPS2Glo, final T tGlo) { - this(channel, health, day, month, year, ta, lambda, deltaI, pa, ecc, deltaT, deltaTDot, tGlo2UTC, tGPS2Glo, - tGlo, DataContext.getDefault().getTimeScales().getGLONASS()); - } - - /** - * Constructor. - * - * @param channel the frequency channel from -7 to 6) - * @param health the Health status - * @param day the day of Almanac - * @param month the month of Almanac - * @param year the year of Almanac - * @param ta the reference time of the almanac (s) - * @param lambda the Greenwich longitude of ascending node of orbit (rad) - * @param deltaI the correction to the mean value of inclination (rad) - * @param pa the argument of perigee (rad) - * @param ecc the eccentricity - * @param deltaT the correction to the mean value of Draconian period (s) - * @param deltaTDot the rate of change of orbital period - * @param tGlo2UTC the correction from GLONASS to UTC (s) - * @param tGPS2Glo the correction to GPS time relative GLONASS (s) - * @param tGlo the correction of time relative to GLONASS system time (s) - * @param glonass GLONASS time scale. - * @since 10.1 - */ - public FieldGLONASSAlmanac(final int channel, final int health, final int day, final int month, final int year, - final T ta, final T lambda, final T deltaI, final T pa, final T ecc, final T deltaT, final T deltaTDot, - final T tGlo2UTC, final T tGPS2Glo, final T tGlo, final TimeScale glonass) { - this.zero = ta.getField().getZero(); - this.channel = channel; - this.health = health; - this.day = day; - this.month = month; - this.year = year; - this.ta = ta; - this.lambda = lambda; - this.deltaI = deltaI; - this.pa = pa; - this.ecc = ecc; - this.deltaT = deltaT; - this.deltaTDot = deltaTDot; - this.tGlo2UTC = tGlo2UTC; - this.tGPS2Glo = tGPS2Glo; - this.tGlo = tGlo; - this.glonass = glonass; - } - - /** - * Constructor - * - * This constructor converts a GLONASSAlmanac into a FieldGLONASSAlmanac - * - * @param field - * @param almanac - */ - public FieldGLONASSAlmanac(Field field, GLONASSAlmanac almanac) { - this.zero = field.getZero(); - this.channel = almanac.getFrequencyChannel(); - this.health = almanac.getHealth(); - this.day = almanac.getDay(); - this.month = almanac.getMonth(); - this.year = almanac.getYear(); - this.ta = zero.add(almanac.getTime()); - this.lambda = zero.add(almanac.getLambda()); - this.deltaI = zero.add(almanac.getDeltaI()); - this.pa = zero.add(almanac.getPa()); - this.ecc = zero.add(almanac.getE()); - this.deltaT = zero.add(almanac.getDeltaT()); - this.deltaTDot = zero.add(almanac.getDeltaTDot()); - this.tGlo2UTC = zero.add(almanac.getGlo2UTC()); - this.tGPS2Glo = zero.add(almanac.getGPS2Glo()); - this.tGlo = zero.add(almanac.getGloOffset()); - this.glonass = almanac.getGlonass(); - } - - - @Override - public FieldAbsoluteDate getDate() { - final DateComponents date = new DateComponents(year, month, day); - final TimeComponents time = new TimeComponents(ta.getReal()); - return new FieldAbsoluteDate<>(ta.getField(), date, time, glonass); - } - - @Override - public T getTime() { - return ta; - } - - @Override - public T getLambda() { - return lambda; - } - - @Override - public T getE() { - return ecc; - } - - @Override - public T getPa() { - return pa; - } - - @Override - public T getDeltaI() { - return deltaI; - } - - @Override - public T getDeltaT() { - return deltaT; - } - - @Override - public T getDeltaTDot() { - return deltaTDot; - } - - /** - * Get the Health status. - * - * @return the Health status - */ - public int getHealth() { - return health; - } - - /** - * Get the frequency channel. - * - * @return the frequency channel - */ - public int getFrequencyChannel() { - return channel; - } - - /** - * Get the correction from GLONASS to UTC. - * - * @return the correction from GLONASS to UTC (s) - */ - public T getGlo2UTC() { - return tGlo2UTC; - } - - /** - * Get the correction to GPS time relative GLONASS. - * - * @return the to GPS time relative GLONASS (s) - */ - public T getGPS2Glo() { - return tGPS2Glo; - } - - /** - * Get the correction of time relative to GLONASS system time. - * - * @return the correction of time relative to GLONASS system time (s) - */ - public T getGloOffset() { - return tGlo; - } - - @Override - public int getNa() { - final FieldGLONASSDate gloDate = new FieldGLONASSDate<>(zero.getField(), getDate(), glonass); - return gloDate.getDayNumber(); - } - - @Override - public int getN4() { - final FieldGLONASSDate gloDate = new FieldGLONASSDate<>(zero.getField(), getDate(), glonass); - return gloDate.getIntervalNumber(); - } - - - @Override - public T getGammaN() { - return zero; - } - - @Override - public T getTN() { - return zero; - } - - @Override - public T getXDot() { - return zero; - } - - @Override - public T getX() { - return zero; - } - - @Override - public T getXDotDot() { - return zero; - } - - @Override - public T getYDot() { - return zero; - } - - @Override - public T getY() { - return zero; - } - - @Override - public T getYDotDot() { - return zero; - } - - @Override - public T getZDot() { - return zero; - } - - @Override - public T getZ() { - return zero; - } - - @Override - public T getZDotDot() { - return zero; - } - - @Override - public int getIOD() { - return 0; - } +public class FieldGLONASSAlmanac> + implements + FieldGLONASSOrbitalElements { + + /** Field Zero. */ + private final T zero; + + /** Frequency channel (-7...6). */ + private final int channel; + + /** Health status. */ + private final int health; + + /** Day of Almanac. */ + private final int day; + + /** Month of Almanac. */ + private final int month; + + /** Year of Almanac. */ + private final int year; + + /** Reference time of the almanac. */ + private final T ta; + + /** Greenwich longitude of ascending node of orbit. */ + private final T lambda; + + /** Correction to the mean value of inclination. */ + private final T deltaI; + + /** Argument of perigee. */ + private final T pa; + + /** Eccentricity. */ + private final T ecc; + + /** Correction to the mean value of Draconian period. */ + private final T deltaT; + + /** Rate of change of orbital period. */ + private final T deltaTDot; + + /** Correction from GLONASS to UTC. */ + private final T tGlo2UTC; + + /** Correction to GPS time relative GLONASS. */ + private final T tGPS2Glo; + + /** Correction of time relative to GLONASS system time. */ + private final T tGlo; + + /** GLONASS time scale. */ + private final TimeScale glonass; + + /** + * Constructor. + *

+ * This method uses the {@link DataContext#getDefault() default data + * context}. + * + * @param channel the frequency channel from -7 to 6) + * @param health the Health status + * @param day the day of Almanac + * @param month the month of Almanac + * @param year the year of Almanac + * @param ta the reference time of the almanac (s) + * @param lambda the Greenwich longitude of ascending node of orbit (rad) + * @param deltaI the correction to the mean value of inclination (rad) + * @param pa the argument of perigee (rad) + * @param ecc the eccentricity + * @param deltaT the correction to the mean value of Draconian period (s) + * @param deltaTDot the rate of change of orbital period + * @param tGlo2UTC the correction from GLONASS to UTC (s) + * @param tGPS2Glo the correction to GPS time relative GLONASS (s) + * @param tGlo the correction of time relative to GLONASS system time (s) + * @see #GLONASSAlmanac(int, int, int, int, int, T, T, T, T, T, T, T, T, T, + * T, TimeScale) + */ + @DefaultDataContext + public FieldGLONASSAlmanac(final int channel, final int health, + final int day, final int month, final int year, + final T ta, final T lambda, final T deltaI, + final T pa, final T ecc, final T deltaT, + final T deltaTDot, final T tGlo2UTC, + final T tGPS2Glo, final T tGlo) { + this(channel, health, day, month, year, ta, lambda, deltaI, pa, ecc, + deltaT, deltaTDot, tGlo2UTC, tGPS2Glo, tGlo, + DataContext.getDefault().getTimeScales().getGLONASS()); + } + + /** + * Constructor. + * + * @param channel the frequency channel from -7 to 6) + * @param health the Health status + * @param day the day of Almanac + * @param month the month of Almanac + * @param year the year of Almanac + * @param ta the reference time of the almanac (s) + * @param lambda the Greenwich longitude of ascending node of orbit (rad) + * @param deltaI the correction to the mean value of inclination (rad) + * @param pa the argument of perigee (rad) + * @param ecc the eccentricity + * @param deltaT the correction to the mean value of Draconian period (s) + * @param deltaTDot the rate of change of orbital period + * @param tGlo2UTC the correction from GLONASS to UTC (s) + * @param tGPS2Glo the correction to GPS time relative GLONASS (s) + * @param tGlo the correction of time relative to GLONASS system time (s) + * @param glonass GLONASS time scale. + * @since 10.1 + */ + public FieldGLONASSAlmanac(final int channel, final int health, + final int day, final int month, final int year, + final T ta, final T lambda, final T deltaI, + final T pa, final T ecc, final T deltaT, + final T deltaTDot, final T tGlo2UTC, + final T tGPS2Glo, final T tGlo, + final TimeScale glonass) { + this.zero = ta.getField().getZero(); + this.channel = channel; + this.health = health; + this.day = day; + this.month = month; + this.year = year; + this.ta = ta; + this.lambda = lambda; + this.deltaI = deltaI; + this.pa = pa; + this.ecc = ecc; + this.deltaT = deltaT; + this.deltaTDot = deltaTDot; + this.tGlo2UTC = tGlo2UTC; + this.tGPS2Glo = tGPS2Glo; + this.tGlo = tGlo; + this.glonass = glonass; + } + + /** + * Constructor This constructor converts a GLONASSAlmanac into a + * FieldGLONASSAlmanac. + * + * @param field + * @param almanac + */ + public FieldGLONASSAlmanac(final Field field, final GLONASSAlmanac almanac) { + this.zero = field.getZero(); + this.channel = almanac.getFrequencyChannel(); + this.health = almanac.getHealth(); + this.day = almanac.getDay(); + this.month = almanac.getMonth(); + this.year = almanac.getYear(); + this.ta = zero.add(almanac.getTime()); + this.lambda = zero.add(almanac.getLambda()); + this.deltaI = zero.add(almanac.getDeltaI()); + this.pa = zero.add(almanac.getPa()); + this.ecc = zero.add(almanac.getE()); + this.deltaT = zero.add(almanac.getDeltaT()); + this.deltaTDot = zero.add(almanac.getDeltaTDot()); + this.tGlo2UTC = zero.add(almanac.getGlo2UTC()); + this.tGPS2Glo = zero.add(almanac.getGPS2Glo()); + this.tGlo = zero.add(almanac.getGloOffset()); + this.glonass = almanac.getGlonass(); + } + + @Override + public FieldAbsoluteDate getDate() { + final DateComponents date = new DateComponents(year, month, day); + final TimeComponents time = new TimeComponents(ta.getReal()); + return new FieldAbsoluteDate<>(ta.getField(), date, time, glonass); + } + + @Override + public T getTime() { + return ta; + } + + @Override + public T getLambda() { + return lambda; + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getPa() { + return pa; + } + + @Override + public T getDeltaI() { + return deltaI; + } + + @Override + public T getDeltaT() { + return deltaT; + } + + @Override + public T getDeltaTDot() { + return deltaTDot; + } + + /** + * Get the Health status. + * + * @return the Health status + */ + public int getHealth() { + return health; + } + + /** + * Get the frequency channel. + * + * @return the frequency channel + */ + public int getFrequencyChannel() { + return channel; + } + + /** + * Get the correction from GLONASS to UTC. + * + * @return the correction from GLONASS to UTC (s) + */ + public T getGlo2UTC() { + return tGlo2UTC; + } + + /** + * Get the correction to GPS time relative GLONASS. + * + * @return the to GPS time relative GLONASS (s) + */ + public T getGPS2Glo() { + return tGPS2Glo; + } + + /** + * Get the correction of time relative to GLONASS system time. + * + * @return the correction of time relative to GLONASS system time (s) + */ + public T getGloOffset() { + return tGlo; + } + + @Override + public int getNa() { + final FieldGLONASSDate gloDate = + new FieldGLONASSDate<>(zero.getField(), getDate(), glonass); + return gloDate.getDayNumber(); + } + + @Override + public int getN4() { + final FieldGLONASSDate gloDate = + new FieldGLONASSDate<>(zero.getField(), getDate(), glonass); + return gloDate.getIntervalNumber(); + } + + @Override + public T getGammaN() { + return zero; + } + + @Override + public T getTN() { + return zero; + } + + @Override + public T getXDot() { + return zero; + } + + @Override + public T getX() { + return zero; + } + + @Override + public T getXDotDot() { + return zero; + } + + @Override + public T getYDot() { + return zero; + } + + @Override + public T getY() { + return zero; + } + + @Override + public T getYDotDot() { + return zero; + } + + @Override + public T getZDot() { + return zero; + } + + @Override + public T getZ() { + return zero; + } + + @Override + public T getZDotDot() { + return zero; + } + + @Override + public int getIOD() { + return 0; + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java index 26042decf..13f8ebe39 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSAnalyticalPropagator.java @@ -52,776 +52,954 @@ import org.orekit.utils.ParameterDriver; * @see * GLONASS Interface Control Document - * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) */ public class FieldGLONASSAnalyticalPropagator> -extends FieldAbstractAnalyticalPropagator { - - // Constants - /** Constant 7.0 / 3.0. */ - private static final double SEVEN_THIRD = 7.0 / 3.0; - - /** Constant 7.0 / 6.0. */ - private static final double SEVEN_SIXTH = 7.0 / 6.0; - - /** Constant 7.0 / 24.0. */ - private static final double SEVEN_24TH = 7.0 / 24.0; - - /** Constant 49.0 / 72.0. */ - private static final double FN_72TH = 49.0 / 72.0; - - /** Value of the earth's rotation rate in rad/s. */ - private static final double GLONASS_AV = 7.2921150e-5; - - /** Mean value of inclination for Glonass orbit is equal to 63°. */ - private static final double GLONASS_MEAN_INCLINATION = 64.8; - - /** - * Mean value of Draconian period for Glonass orbit is equal to 40544s : 11 - * hours 15 minutes 44 seconds. - */ - private static final double GLONASS_MEAN_DRACONIAN_PERIOD = 40544; - - /** Second degree zonal coefficient of normal potential. */ - private static final double GLONASS_J20 = 1.08262575e-3; - - /** Equatorial radius of Earth (m). */ - private static final double GLONASS_EARTH_EQUATORIAL_RADIUS = 6378136; - - // Data used to solve Kepler's equation - /** First coefficient to compute Kepler equation solver starter. */ - private static final double A; - - /** Second coefficient to compute Kepler equation solver starter. */ - private static final double B; - - static { - final double k1 = 3 * FastMath.PI + 2; - final double k2 = FastMath.PI - 1; - final double k3 = 6 * FastMath.PI - 1; - A = 3 * k2 * k2 / k1; - B = k3 * k3 / (6 * k1); - } - - // Fields - /** The GLONASS orbital elements used. */ - private final FieldGLONASSOrbitalElements glonassOrbit; - - /** The spacecraft mass (kg). */ - private final T mass; - - /** The ECI frame used for GLONASS propagation. */ - private final Frame eci; - - /** The ECEF frame used for GLONASS propagation. */ - private final Frame ecef; - - /** Data context for propagation. */ - private final DataContext dataContext; - - /** - * Default constructor. - * - *

The Field GLONASS orbital elements is the only requested parameter to build a FieldGLONASSPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The data context is by default to the - * {@link DataContext#getDefault() default data context}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, final DataContext dataContext)}

- * - * @param field - * @param glonassOrbElt the Field GLONASS orbital elements to be used by the Field GLONASS propagator. - */ - @DefaultDataContext - public FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt) { - this(field, glonassOrbElt, DataContext.getDefault()); - } - - /** - * Constructor. - * - *

The Field GLONASS orbital elements is the only requested parameter to build a FieldGLONASSPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The data context is by default to the - * {@link DataContext#getDefault() default data context}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, - final DataContext dataContext, final AttitudeProvider attitudeProvider, T mass, final Frame eci, final Frame ecef)}

- * - * @param field - * @param glonassOrbElt the Field GLONASS orbital elements to be used by the Field GLONASS propagator. - * @param dataContext the data - */ - public FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, final DataContext dataContext) { - this(field, glonassOrbElt, dataContext, Propagator.getDefaultLaw(dataContext.getFrames()), field.getZero().add(DEFAULT_MASS),dataContext.getFrames().getEME2000(), - dataContext.getFrames().getITRF(IERSConventions.IERS_2010, true)); - } - - /** - * Constructor. - * - * *

The Field GLONASS orbital elements is the only requested parameter to build a FieldGLONASSPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The data context is by default to the - * {@link DataContext#getDefault() default data context}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - * @param field - * @param glonassOrbElt - * @param dataContext - * @param attitudeProvider - * @param mass - * @param eci - * @param ecef - */ - public FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, - final DataContext dataContext, final AttitudeProvider attitudeProvider, T mass, final Frame eci, final Frame ecef) { - - super(field, attitudeProvider); - this.dataContext = dataContext; - // Stores the GLONASS orbital elements - this.glonassOrbit = glonassOrbElt; - // Sets the start date as the date of the orbital elements - setStartDate(glonassOrbit.getDate()); - // Sets the mass - this.mass = mass; - // Sets the Earth Centered Inertial frame - this.eci = eci; - // Sets the Earth Centered Earth Fixed frame - this.ecef = ecef; - } - - /** - * Gets the FieldPVCoordinates of the GLONASS SV in {@link #getECEF() ECEF frame}. - * - *

- * The algorithm is defined at Appendix M.1 from GLONASS Interface Control - * Document, with automatic differentiation added to compute velocity and - * acceleration. - *

- * - * @param date the computation date - * @return the GLONASS SV FieldPVCoordinates in {@link #getECEF() ECEF frame} - */ - public FieldPVCoordinates propagateInEcef(final FieldAbsoluteDate date) { - - // Interval of prediction dTpr - final FieldUnivariateDerivative2 dTpr = getdTpr(date); - - // Zero - final FieldUnivariateDerivative2 zero = dTpr.getField().getZero(); - - // The number of whole orbits "w" on a prediction interval - final FieldUnivariateDerivative2 w = FastMath.floor(dTpr.divide(glonassOrbit.getDeltaT().add(GLONASS_MEAN_DRACONIAN_PERIOD))); - - // Current inclination - final FieldUnivariateDerivative2 i = zero.add(zero.add(GLONASS_MEAN_INCLINATION / 180 * GLONASSOrbitalElements.GLONASS_PI).add(glonassOrbit.getDeltaI())); - - // Eccentricity - final FieldUnivariateDerivative2 e = zero.add(glonassOrbit.getE()); - - // Mean draconique period in orbite w+1 and mean motion - final FieldUnivariateDerivative2 tDR = w.multiply(2.0).add(1.0).multiply(glonassOrbit.getDeltaTDot()) - .add(glonassOrbit.getDeltaT()) - .add(GLONASS_MEAN_DRACONIAN_PERIOD); - final FieldUnivariateDerivative2 n = tDR.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI).reciprocal(); - - // Semi-major axis : computed by successive approximation - final FieldUnivariateDerivative2 sma = computeSma(tDR, i, e); - - // (ae / p)^2 term - final FieldUnivariateDerivative2 p = sma.multiply(e.multiply(e).negate().add(1.0)); - final FieldUnivariateDerivative2 aeop = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); - final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); - - // Current longitude of the ascending node - final FieldUnivariateDerivative2 lambda = computeLambda(dTpr, n, aeop2, i); - - // Current argument of perigee - final FieldUnivariateDerivative2 pa = computePA(dTpr, n, aeop2, i); - - // Mean longitude at the instant the spacecraft passes the current ascending - // node - final FieldUnivariateDerivative2 tanPAo2 = FastMath.tan(pa.divide(2.0)); - final FieldUnivariateDerivative2 coef = tanPAo2 - .multiply(FastMath.sqrt(e.negate().add(1.0).divide(e.add(1.0)))); - final FieldUnivariateDerivative2 e0 = FastMath.atan(coef).multiply(2.0).negate(); - final FieldUnivariateDerivative2 m1 = pa.add(e0).subtract(FastMath.sin(e0).multiply(e)); - - // Current mean longitude - final FieldUnivariateDerivative2 correction = dTpr - .subtract(w.multiply(zero.add(GLONASS_MEAN_DRACONIAN_PERIOD).add(glonassOrbit.getDeltaT()))) - .subtract(w.multiply(w).multiply(glonassOrbit.getDeltaTDot())); - final FieldUnivariateDerivative2 m = m1.add(n.multiply(correction)); - - // Take into consideration the periodic perturbations - final FieldSinCos> scPa = FastMath.sinCos(pa); - final FieldUnivariateDerivative2 h = e.multiply(scPa.sin()); - final FieldUnivariateDerivative2 l = e.multiply(scPa.cos()); - // δa1 - final FieldUnivariateDerivative2[] d1 = getParameterDifferentials(sma, i, h, l, m1); - // δa2 - final FieldUnivariateDerivative2[] d2 = getParameterDifferentials(sma, i, h, l, m); - // Apply corrections - final FieldUnivariateDerivative2 smaCorr = sma.add(d2[0]).subtract(d1[0]); - final FieldUnivariateDerivative2 hCorr = h.add(d2[1]).subtract(d1[1]); - final FieldUnivariateDerivative2 lCorr = l.add(d2[2]).subtract(d1[2]); - final FieldUnivariateDerivative2 lambdaCorr = lambda.add(d2[3]).subtract(d1[3]); - final FieldUnivariateDerivative2 iCorr = i.add(d2[4]).subtract(d1[4]); - final FieldUnivariateDerivative2 mCorr = m.add(d2[5]).subtract(d1[5]); - final FieldUnivariateDerivative2 eCorr = FastMath.sqrt(hCorr.multiply(hCorr).add(lCorr.multiply(lCorr))); - final FieldUnivariateDerivative2 paCorr; - if (eCorr.getValue().getReal() == 0.) { - paCorr = zero; - } else { - if (lCorr.getValue() == eCorr.getValue()) { - paCorr = zero.add(0.5 * FieldGLONASSOrbitalElements.GLONASS_PI); - } else if (lCorr.getValue().getReal() == -eCorr.getValue().getReal()) { - paCorr = zero.add(-0.5 * FieldGLONASSOrbitalElements.GLONASS_PI); - } else { - paCorr = FastMath.atan2(hCorr, lCorr); - } - } - - // Eccentric Anomaly - final FieldUnivariateDerivative2 mk = mCorr.subtract(paCorr); - final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk, eCorr); - - // True Anomaly - final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek, eCorr); - - // Argument of Latitude - final FieldUnivariateDerivative2 phik = vk.add(paCorr); - - // Corrected Radius - final FieldUnivariateDerivative2 pCorr = smaCorr.multiply(eCorr.multiply(eCorr).negate().add(1.0)); - final FieldUnivariateDerivative2 rk = pCorr.divide(eCorr.multiply(FastMath.cos(vk)).add(1.0)); - - // Positions in orbital plane - final FieldSinCos> scPhik = FastMath.sinCos(phik); - final FieldUnivariateDerivative2 xk = scPhik.cos().multiply(rk); - final FieldUnivariateDerivative2 yk = scPhik.sin().multiply(rk); - - // Coordinates of position - final FieldSinCos> scL = FastMath.sinCos(lambdaCorr); - final FieldSinCos> scI = FastMath.sinCos(iCorr); - final FieldVector3D> positionwithDerivatives = - new FieldVector3D<>(xk.multiply(scL.cos()).subtract(yk.multiply(scL.sin()).multiply(scI.cos())), - xk.multiply(scL.sin()).add(yk.multiply(scL.cos()).multiply(scI.cos())), - yk.multiply(scI.sin())); - - return new FieldPVCoordinates( - new FieldVector3D(positionwithDerivatives.getX().getValue(), - positionwithDerivatives.getY().getValue(), - positionwithDerivatives.getZ().getValue()), - new FieldVector3D(positionwithDerivatives.getX().getFirstDerivative(), - positionwithDerivatives.getY().getFirstDerivative(), - positionwithDerivatives.getZ().getFirstDerivative()), - new FieldVector3D(positionwithDerivatives.getX().getSecondDerivative(), - positionwithDerivatives.getY().getSecondDerivative(), - positionwithDerivatives.getZ().getSecondDerivative())); - } - - /** - * Gets eccentric anomaly from mean anomaly. - *

- * The algorithm used to solve the Kepler equation has been published in: - * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. Gooding, - * Celestial Mechanics 38 (1986) 307-334 - *

- *

- * It has been copied from the OREKIT library (KeplerianOrbit class). - *

- * - * @param mk the mean anomaly (rad) - * @param e the eccentricity - * @return the eccentric anomaly (rad) - */ - private FieldUnivariateDerivative2 getEccentricAnomaly(final FieldUnivariateDerivative2 mk, - final FieldUnivariateDerivative2 e) { - - // reduce M to [-PI PI] interval - final T zero = mk.getValue().getField().getZero(); - final FieldUnivariateDerivative2 reducedM = new FieldUnivariateDerivative2( - MathUtils.normalizeAngle(mk.getValue(), zero), mk.getFirstDerivative(), mk.getSecondDerivative()); - - // compute start value according to A. W. Odell and R. H. Gooding S12 starter - FieldUnivariateDerivative2 ek; - if (FastMath.abs(reducedM.getValue()).getReal() < 1.0 / 6.0) { - if (FastMath.abs(reducedM.getValue()).getReal() < Precision.SAFE_MIN) { - // this is an Orekit change to the S12 starter. - // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN - // appearing later in - // the computation. As in this case E and M are almost equal, we initialize ek - // with reducedM - ek = reducedM; - } else { - // this is the standard S12 starter - ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(e)); - } - } else { - if (reducedM.getValue().getReal() < 0) { - final FieldUnivariateDerivative2 w = reducedM.add(FastMath.PI); - ek = reducedM - .add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(e)); - } else { - final FieldUnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI); - ek = reducedM - .add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(e)); - } - } - - final FieldUnivariateDerivative2 e1 = e.negate().add(1.0); - final boolean noCancellationRisk = (e1.getReal() - + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; - - // perform two iterations, each consisting of one Halley step and one - // Newton-Raphson step - for (int j = 0; j < 2; ++j) { - final FieldUnivariateDerivative2 f; - FieldUnivariateDerivative2 fd; - final FieldUnivariateDerivative2 fdd = ek.sin().multiply(e); - final FieldUnivariateDerivative2 fddd = ek.cos().multiply(e); - if (noCancellationRisk) { - f = ek.subtract(fdd).subtract(reducedM); - fd = fddd.subtract(1).negate(); - } else { - f = eMeSinE(ek, e).subtract(reducedM); - final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); - fd = s.multiply(s).multiply(e.multiply(2.0)).add(e1); - } - final FieldUnivariateDerivative2 dee = f.multiply(fd) - .divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd))); - - // update eccentric anomaly, using expressions that limit underflow problems - final FieldUnivariateDerivative2 w = fd - .add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3)))); - fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); - ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd)); - } - - // expand the result back to original range - ek = ek.add(mk.getValue().subtract(reducedM.getValue())); - - // Returns the eccentric anomaly - return ek; - } - - /** - * Accurate computation of E - e sin(E). - * - * @param E eccentric anomaly - * @param ecc the eccentricity - * @return E - e sin(E) - */ - private FieldUnivariateDerivative2 eMeSinE(final FieldUnivariateDerivative2 E, - final FieldUnivariateDerivative2 ecc) { - FieldUnivariateDerivative2 x = E.sin().multiply(ecc.negate().add(1.0)); - final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); - FieldUnivariateDerivative2 term = E; - FieldUnivariateDerivative2 d = E.getField().getZero(); - // the inequality test below IS intentional and should NOT be replaced by a - // check with a small tolerance - for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); !(x.getValue()).equals(x0.getValue());) { - d = d.add(2); - term = term.multiply(mE2.divide(d.multiply(d.add(1)))); - x0 = x; - x = x.subtract(term); - } - return x; - } - - /** - * Gets true anomaly from eccentric anomaly. - * - * @param ek the eccentric anomaly (rad) - * @param ecc the eccentricity - * @return the true anomaly (rad) - */ - private FieldUnivariateDerivative2 getTrueAnomaly(final FieldUnivariateDerivative2 ek, - final FieldUnivariateDerivative2 ecc) { - final FieldUnivariateDerivative2 svk = ek.sin().multiply(FastMath.sqrt(ecc.multiply(ecc).negate().add(1.0))); - final FieldUnivariateDerivative2 cvk = ek.cos().subtract(ecc); - return svk.atan2(cvk); - } - - /** - * Get the interval of prediction. - * - * @param date the considered date - * @return the duration from GLONASS orbit Reference epoch (s) - */ - private FieldUnivariateDerivative2 getdTpr(final FieldAbsoluteDate date) { - final TimeScale glonass = dataContext.getTimeScales().getGLONASS(); - final FieldGLONASSDate tEnd = new FieldGLONASSDate(date.getField(), date, glonass); - final FieldGLONASSDate tSta = new FieldGLONASSDate(date.getField(), glonassOrbit.getDate(), glonass); - final int n = tEnd.getDayNumber(); - final int na = tSta.getDayNumber(); - final int deltaN; - if (na == 27) { - deltaN = n - na - FastMath.round((float) (n - na) / 1460) * 1460; - } else { - deltaN = n - na - FastMath.round((float) (n - na) / 1461) * 1461; - } - - final T zero = date.getField().getZero(); - final FieldUnivariateDerivative2 ti = new FieldUnivariateDerivative2(zero.add(tEnd.getSecInDay()), - zero.add(1.0), zero); - - return ti.subtract(glonassOrbit.getTime()).add(86400 * deltaN); - } - - /** - * Computes the semi-major axis of orbit using technique of successive - * approximations. - * - * @param tDR mean draconique period (s) - * @param i current inclination (rad) - * @param e eccentricity - * @return the semi-major axis (m). - */ - private FieldUnivariateDerivative2 computeSma(final FieldUnivariateDerivative2 tDR, - final FieldUnivariateDerivative2 i, final FieldUnivariateDerivative2 e) { - - // Zero - final FieldUnivariateDerivative2 zero = tDR.getField().getZero(); - - // If one of the input parameter is equal to Double.NaN, an infinite loop can - // occur. - // In that case, we do not compute the value of the semi major axis. - // We decided to return a Double.NaN value instead. - if (Double.isNaN(tDR.getValue().getReal()) || Double.isNaN(i.getValue().getReal()) - || Double.isNaN(e.getValue().getReal())) { - return zero.add(Double.NaN); - } - - // Common parameters - final FieldUnivariateDerivative2 sinI = FastMath.sin(i); - final FieldUnivariateDerivative2 sin2I = sinI.multiply(sinI); - final FieldUnivariateDerivative2 ome2 = e.multiply(e).negate().add(1.0); - final FieldUnivariateDerivative2 ome2Pow3o2 = FastMath.sqrt(ome2).multiply(ome2); - final FieldUnivariateDerivative2 pa = zero.add(glonassOrbit.getPa()); - final FieldUnivariateDerivative2 cosPA = FastMath.cos(pa); - final FieldUnivariateDerivative2 opecosPA = e.multiply(cosPA).add(1.0); - final FieldUnivariateDerivative2 opecosPAPow2 = opecosPA.multiply(opecosPA); - final FieldUnivariateDerivative2 opecosPAPow3 = opecosPAPow2.multiply(opecosPA); - - // Initial approximation - FieldUnivariateDerivative2 tOCK = tDR; - - // Successive approximations - // The process of approximation ends when fulfilling the following condition: - // |a(n+1) - a(n)| < 1cm - FieldUnivariateDerivative2 an = zero; - FieldUnivariateDerivative2 anp1 = zero; - boolean isLastStep = false; - while (!isLastStep) { - - // a(n+1) computation - final FieldUnivariateDerivative2 tOCKo2p = tOCK.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI); - final FieldUnivariateDerivative2 tOCKo2pPow2 = tOCKo2p.multiply(tOCKo2p); - anp1 = FastMath.cbrt(tOCKo2pPow2.multiply(GLONASSOrbitalElements.GLONASS_MU)); - - // p(n+1) computation - final FieldUnivariateDerivative2 p = anp1.multiply(ome2); - - // Tock(n+1) computation - final FieldUnivariateDerivative2 aeop = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); - final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); - final FieldUnivariateDerivative2 term1 = aeop2.multiply(GLONASS_J20).multiply(1.5); - final FieldUnivariateDerivative2 term2 = sin2I.multiply(2.5).negate().add(2.0); - final FieldUnivariateDerivative2 term3 = ome2Pow3o2.divide(opecosPAPow2); - final FieldUnivariateDerivative2 term4 = opecosPAPow3.divide(ome2); - tOCK = tDR.divide(term1.multiply(term2.multiply(term3).add(term4)).negate().add(1.0)); - - // Check convergence - if (FastMath.abs(anp1.subtract(an).getReal()) <= 0.01) { - isLastStep = true; - } - - an = anp1; - } - - return an; - - } - - /** - * Computes the current longitude of the ascending node. - * - * @param dTpr interval of prediction (s) - * @param n mean motion (rad/s) - * @param aeop2 square of the ratio between the radius of the ellipsoid and p, - * with p = sma * (1 - ecc²) - * @param i inclination (rad) - * @return the current longitude of the ascending node (rad) - */ - private FieldUnivariateDerivative2 computeLambda(final FieldUnivariateDerivative2 dTpr, - final FieldUnivariateDerivative2 n, final FieldUnivariateDerivative2 aeop2, - final FieldUnivariateDerivative2 i) { - final FieldUnivariateDerivative2 cosI = FastMath.cos(i); - final FieldUnivariateDerivative2 precession = aeop2.multiply(n).multiply(cosI).multiply(1.5 * GLONASS_J20); - return dTpr.multiply(precession.add(GLONASS_AV)).negate().add(glonassOrbit.getLambda()); - } - - /** - * Computes the current argument of perigee. - * - * @param dTpr interval of prediction (s) - * @param n mean motion (rad/s) - * @param aeop2 square of the ratio between the radius of the ellipsoid and p, - * with p = sma * (1 - ecc²) - * @param i inclination (rad) - * @return the current argument of perigee (rad) - */ - private FieldUnivariateDerivative2 computePA(final FieldUnivariateDerivative2 dTpr, - final FieldUnivariateDerivative2 n, final FieldUnivariateDerivative2 aeop2, - final FieldUnivariateDerivative2 i) { - final FieldUnivariateDerivative2 cosI = FastMath.cos(i); - final FieldUnivariateDerivative2 cos2I = cosI.multiply(cosI); - final FieldUnivariateDerivative2 precession = aeop2.multiply(n) - .multiply(cos2I.multiply(5.0).negate().add(1.0)).multiply(0.75 * GLONASS_J20); - return dTpr.multiply(precession).negate().add(glonassOrbit.getPa()); - } - - /** - * Computes the differentials δai. - *

- * The value of i depends of the type of longitude (i = 2 for the current mean - * longitude; i = 1 for the mean longitude at the instant the spacecraft passes - * the current ascending node) - *

- * - * @param a semi-major axis (m) - * @param i inclination (rad) - * @param h x component of the eccentricity (rad) - * @param l y component of the eccentricity (rad) - * @param m longitude (current or at the ascending node instant) - * @return the differentials of the orbital parameters - */ - private FieldUnivariateDerivative2[] getParameterDifferentials(final FieldUnivariateDerivative2 a, - final FieldUnivariateDerivative2 i, final FieldUnivariateDerivative2 h, - final FieldUnivariateDerivative2 l, final FieldUnivariateDerivative2 m) { - - // B constant - final FieldUnivariateDerivative2 aeoa = a.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); - final FieldUnivariateDerivative2 aeoa2 = aeoa.multiply(aeoa); - final FieldUnivariateDerivative2 b = aeoa2.multiply(1.5 * GLONASS_J20); - - // Commons Parameters - final FieldSinCos> scI = FastMath.sinCos(i); - final FieldSinCos> scLk = FastMath.sinCos(m); - final FieldSinCos> sc2Lk = FieldSinCos.sum(scLk, scLk); - final FieldSinCos> sc3Lk = FieldSinCos.sum(scLk, sc2Lk); - final FieldSinCos> sc4Lk = FieldSinCos.sum(sc2Lk, sc2Lk); - final FieldUnivariateDerivative2 cosI = scI.cos(); - final FieldUnivariateDerivative2 sinI = scI.sin(); - final FieldUnivariateDerivative2 cosI2 = cosI.multiply(cosI); - final FieldUnivariateDerivative2 sinI2 = sinI.multiply(sinI); - final FieldUnivariateDerivative2 cosLk = scLk.cos(); - final FieldUnivariateDerivative2 sinLk = scLk.sin(); - final FieldUnivariateDerivative2 cos2Lk = sc2Lk.cos(); - final FieldUnivariateDerivative2 sin2Lk = sc2Lk.sin(); - final FieldUnivariateDerivative2 cos3Lk = sc3Lk.cos(); - final FieldUnivariateDerivative2 sin3Lk = sc3Lk.sin(); - final FieldUnivariateDerivative2 cos4Lk = sc4Lk.cos(); - final FieldUnivariateDerivative2 sin4Lk = sc4Lk.sin(); - - // h*cos(nLk), l*cos(nLk), h*sin(nLk) and l*sin(nLk) - // n = 1 - final FieldUnivariateDerivative2 hCosLk = h.multiply(cosLk); - final FieldUnivariateDerivative2 hSinLk = h.multiply(sinLk); - final FieldUnivariateDerivative2 lCosLk = l.multiply(cosLk); - final FieldUnivariateDerivative2 lSinLk = l.multiply(sinLk); - // n = 2 - final FieldUnivariateDerivative2 hCos2Lk = h.multiply(cos2Lk); - final FieldUnivariateDerivative2 hSin2Lk = h.multiply(sin2Lk); - final FieldUnivariateDerivative2 lCos2Lk = l.multiply(cos2Lk); - final FieldUnivariateDerivative2 lSin2Lk = l.multiply(sin2Lk); - // n = 3 - final FieldUnivariateDerivative2 hCos3Lk = h.multiply(cos3Lk); - final FieldUnivariateDerivative2 hSin3Lk = h.multiply(sin3Lk); - final FieldUnivariateDerivative2 lCos3Lk = l.multiply(cos3Lk); - final FieldUnivariateDerivative2 lSin3Lk = l.multiply(sin3Lk); - // n = 4 - final FieldUnivariateDerivative2 hCos4Lk = h.multiply(cos4Lk); - final FieldUnivariateDerivative2 hSin4Lk = h.multiply(sin4Lk); - final FieldUnivariateDerivative2 lCos4Lk = l.multiply(cos4Lk); - final FieldUnivariateDerivative2 lSin4Lk = l.multiply(sin4Lk); - - // 1 - (3 / 2)*sin²i - final FieldUnivariateDerivative2 om3o2xSinI2 = sinI2.multiply(1.5).negate().add(1.0); - - // Compute Differentials - // δa - final FieldUnivariateDerivative2 dakT1 = b.multiply(2.0).multiply(om3o2xSinI2).multiply(lCosLk.add(hSinLk)); - final FieldUnivariateDerivative2 dakT2 = b.multiply(sinI2).multiply(hSinLk.multiply(0.5) - .subtract(lCosLk.multiply(0.5)).add(cos2Lk).add(lCos3Lk.multiply(3.5)).add(hSin3Lk.multiply(3.5))); - final FieldUnivariateDerivative2 dak = dakT1.add(dakT2); - - // δh - final FieldUnivariateDerivative2 dhkT1 = b.multiply(om3o2xSinI2) - .multiply(sinLk.add(lSin2Lk.multiply(1.5)).subtract(hCos2Lk.multiply(1.5))); - final FieldUnivariateDerivative2 dhkT2 = b.multiply(sinI2).multiply(0.25) - .multiply(sinLk.subtract(sin3Lk.multiply(SEVEN_THIRD)).add(lSin2Lk.multiply(5.0)) - .subtract(lSin4Lk.multiply(8.5)).add(hCos4Lk.multiply(8.5)).add(hCos2Lk)); - final FieldUnivariateDerivative2 dhkT3 = lSin2Lk.multiply(cosI2).multiply(b).multiply(0.5).negate(); - final FieldUnivariateDerivative2 dhk = dhkT1.subtract(dhkT2).add(dhkT3); - - // δl - final FieldUnivariateDerivative2 dlkT1 = b.multiply(om3o2xSinI2) - .multiply(cosLk.add(lCos2Lk.multiply(1.5)).add(hSin2Lk.multiply(1.5))); - final FieldUnivariateDerivative2 dlkT2 = b.multiply(sinI2).multiply(0.25) - .multiply(cosLk.negate().subtract(cos3Lk.multiply(SEVEN_THIRD)).subtract(hSin2Lk.multiply(5.0)) - .subtract(lCos4Lk.multiply(8.5)).subtract(hSin4Lk.multiply(8.5)).add(lCos2Lk)); - final FieldUnivariateDerivative2 dlkT3 = hSin2Lk.multiply(cosI2).multiply(b).multiply(0.5); - final FieldUnivariateDerivative2 dlk = dlkT1.subtract(dlkT2).add(dlkT3); - - // δλ - final FieldUnivariateDerivative2 dokT1 = b.negate().multiply(cosI); - final FieldUnivariateDerivative2 dokT2 = lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)) - .subtract(sin2Lk.multiply(0.5)).subtract(lSin3Lk.multiply(SEVEN_SIXTH)) - .add(hCos3Lk.multiply(SEVEN_SIXTH)); - final FieldUnivariateDerivative2 dok = dokT1.multiply(dokT2); - - // δi - final FieldUnivariateDerivative2 dik = b.multiply(sinI).multiply(cosI).multiply(0.5).multiply(lCosLk.negate() - .add(hSinLk).add(cos2Lk).add(lCos3Lk.multiply(SEVEN_THIRD)).add(hSin3Lk.multiply(SEVEN_THIRD))); - - // δL - final FieldUnivariateDerivative2 dLkT1 = b.multiply(2.0).multiply(om3o2xSinI2) - .multiply(lSinLk.multiply(1.75).subtract(hCosLk.multiply(1.75))); - final FieldUnivariateDerivative2 dLkT2 = b.multiply(sinI2).multiply(3.0) - .multiply(hCosLk.multiply(SEVEN_24TH).negate().subtract(lSinLk.multiply(SEVEN_24TH)) - .subtract(hCos3Lk.multiply(FN_72TH)).add(lSin3Lk.multiply(FN_72TH)).add(sin2Lk.multiply(0.25))); - final FieldUnivariateDerivative2 dLkT3 = b.multiply(cosI2) - .multiply(lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)).subtract(sin2Lk.multiply(0.5)) - .subtract(lSin3Lk.multiply(SEVEN_SIXTH)).add(hCos3Lk.multiply(SEVEN_SIXTH))); - final FieldUnivariateDerivative2 dLk = dLkT1.add(dLkT2).add(dLkT3); - - // Final array - final FieldUnivariateDerivative2[] differentials = MathArrays.buildArray(a.getField(), 6); - differentials[0] = dak.multiply(a); - differentials[1] = dhk; - differentials[2] = dlk; - differentials[3] = dok; - differentials[4] = dik; - differentials[5] = dLk; - - return differentials; - } - - /** {@inheritDoc} */ - protected T getMass(final FieldAbsoluteDate date) { - return mass; - } - - /** - * Get the Earth gravity coefficient used for GLONASS propagation. - * - * @return the Earth gravity coefficient. - */ - public T getMU() { - return field.getZero().add(GLONASSOrbitalElements.GLONASS_MU); - } - - /** - * Gets the underlying Field GLONASS orbital elements. - * - * @return the underlying Field GLONASS orbital elements - */ - public FieldGLONASSOrbitalElements getGLONASSOrbitalElements() { - return glonassOrbit; - } - - /** - * Gets the Earth Centered Inertial frame used to propagate the orbit. - * - * @return the ECI frame - */ - public Frame getECI() { - return eci; - } - - /** - * Gets the Earth Centered Earth Fixed frame used to propagate GLONASS orbits. - * - * @return the ECEF frame - */ - public Frame getECEF() { - return ecef; - } - - /** {@inheritDoc} */ - public Frame getFrame() { - return eci; - } - - /** {@inheritDoc} */ - public void resetInitialState(final FieldSpacecraftState state) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - /** {@inheritDoc} */ - protected void resetIntermediateState(FieldSpacecraftState state, boolean forward) { - throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); - } - - /** {@inheritDoc} */ - protected FieldOrbit propagateOrbit(FieldAbsoluteDate date, T[] parameters) { - // Gets the PVCoordinates in ECEF frame - final FieldPVCoordinates pvaInECEF = propagateInEcef(date); - // Transforms the PVCoordinates to ECI frame - final FieldPVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); - // Returns the Cartesian orbit - return new FieldCartesianOrbit<>(pvaInECI, eci, date, date.getField().getZero().add(FieldGLONASSOrbitalElements.GLONASS_MU)); - } - - /** Get the parameters driver for the Field Glonass propagation model. + extends + FieldAbstractAnalyticalPropagator { + + // Constants + /** Constant 7.0 / 3.0. */ + private static final double SEVEN_THIRD = 7.0 / 3.0; + + /** Constant 7.0 / 6.0. */ + private static final double SEVEN_SIXTH = 7.0 / 6.0; + + /** Constant 7.0 / 24.0. */ + private static final double SEVEN_24TH = 7.0 / 24.0; + + /** Constant 49.0 / 72.0. */ + private static final double FN_72TH = 49.0 / 72.0; + + /** Value of the earth's rotation rate in rad/s. */ + private static final double GLONASS_AV = 7.2921150e-5; + + /** Mean value of inclination for Glonass orbit is equal to 63°. */ + private static final double GLONASS_MEAN_INCLINATION = 64.8; + + /** + * Mean value of Draconian period for Glonass orbit is equal to 40544s : 11 + * hours 15 minutes 44 seconds. + */ + private static final double GLONASS_MEAN_DRACONIAN_PERIOD = 40544; + + /** Second degree zonal coefficient of normal potential. */ + private static final double GLONASS_J20 = 1.08262575e-3; + + /** Equatorial radius of Earth (m). */ + private static final double GLONASS_EARTH_EQUATORIAL_RADIUS = 6378136; + + // Data used to solve Kepler's equation + /** First coefficient to compute Kepler equation solver starter. */ + private static final double A; + + /** Second coefficient to compute Kepler equation solver starter. */ + private static final double B; + + static { + final double k1 = 3 * FastMath.PI + 2; + final double k2 = FastMath.PI - 1; + final double k3 = 6 * FastMath.PI - 1; + A = 3 * k2 * k2 / k1; + B = k3 * k3 / (6 * k1); + } + + // Fields + /** The GLONASS orbital elements used. */ + private final FieldGLONASSOrbitalElements glonassOrbit; + + /** The spacecraft mass (kg). */ + private final T mass; + + /** The ECI frame used for GLONASS propagation. */ + private final Frame eci; + + /** The ECEF frame used for GLONASS propagation. */ + private final Frame ecef; + + /** Data context for propagation. */ + private final DataContext dataContext; + + /** + * Default constructor. + *

+ * The Field GLONASS orbital elements is the only requested parameter to + * build a FieldGLONASSPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The data context is by default to the {@link DataContext#getDefault() + * default data context}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. Another data context can be set using + * {@code FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, final DataContext dataContext)} + *

+ * + * @param field + * @param glonassOrbElt the Field GLONASS orbital elements to be used by the + * Field GLONASS propagator. + */ + @DefaultDataContext + public FieldGLONASSAnalyticalPropagator(final Field field, + final FieldGLONASSOrbitalElements glonassOrbElt) { + this(field, glonassOrbElt, DataContext.getDefault()); + } + + /** + * Constructor. + *

+ * The Field GLONASS orbital elements is the only requested parameter to + * build a FieldGLONASSPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The data context is by default to the {@link DataContext#getDefault() + * default data context}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. Another data context can be set using + * {@code FieldGLONASSAnalyticalPropagator(final Field field, final FieldGLONASSOrbitalElements glonassOrbElt, + * final DataContext dataContext, final AttitudeProvider attitudeProvider, T mass, final Frame eci, final Frame ecef)} + *

+ * + * @param field + * @param glonassOrbElt the Field GLONASS orbital elements to be used by the + * Field GLONASS propagator. + * @param dataContext the data + */ + public FieldGLONASSAnalyticalPropagator(final Field field, + final FieldGLONASSOrbitalElements glonassOrbElt, + final DataContext dataContext) { + this(field, glonassOrbElt, dataContext, + Propagator.getDefaultLaw(dataContext.getFrames()), + field.getZero().add(DEFAULT_MASS), + dataContext.getFrames().getEME2000(), + dataContext.getFrames().getITRF(IERSConventions.IERS_2010, true)); + } + + /** + * Constructor. * + *

+ * The Field GLONASS orbital elements is the only requested parameter to + * build a FieldGLONASSPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The data context is by default to the {@link DataContext#getDefault() + * default data context}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + * @param field + * @param glonassOrbElt + * @param dataContext + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldGLONASSAnalyticalPropagator(final Field field, + final FieldGLONASSOrbitalElements glonassOrbElt, + final DataContext dataContext, + final AttitudeProvider attitudeProvider, + final T mass, final Frame eci, + final Frame ecef) { + + super(field, attitudeProvider); + this.dataContext = dataContext; + // Stores the GLONASS orbital elements + this.glonassOrbit = glonassOrbElt; + // Sets the start date as the date of the orbital elements + setStartDate(glonassOrbit.getDate()); + // Sets the mass + this.mass = mass; + // Sets the Earth Centered Inertial frame + this.eci = eci; + // Sets the Earth Centered Earth Fixed frame + this.ecef = ecef; + } + + /** + * Gets the FieldPVCoordinates of the GLONASS SV in {@link #getECEF() ECEF + * frame}. + *

+ * The algorithm is defined at Appendix M.1 from GLONASS Interface Control + * Document, with automatic differentiation added to compute velocity and + * acceleration. + *

+ * + * @param date the computation date + * @return the GLONASS SV FieldPVCoordinates in {@link #getECEF() ECEF + * frame} + */ + public FieldPVCoordinates + propagateInEcef(final FieldAbsoluteDate date) { + + // Interval of prediction dTpr + final FieldUnivariateDerivative2 dTpr = getdTpr(date); + + // Zero + final FieldUnivariateDerivative2 zero = dTpr.getField().getZero(); + + // The number of whole orbits "w" on a prediction interval + final FieldUnivariateDerivative2 w = + FastMath.floor(dTpr.divide(glonassOrbit.getDeltaT() + .add(GLONASS_MEAN_DRACONIAN_PERIOD))); + + // Current inclination + final FieldUnivariateDerivative2 i = + zero.add(zero + .add(GLONASS_MEAN_INCLINATION / + 180 * GLONASSOrbitalElements.GLONASS_PI) + .add(glonassOrbit.getDeltaI())); + + // Eccentricity + final FieldUnivariateDerivative2 e = zero.add(glonassOrbit.getE()); + + // Mean draconique period in orbite w+1 and mean motion + final FieldUnivariateDerivative2 tDR = + w.multiply(2.0).add(1.0).multiply(glonassOrbit.getDeltaTDot()) + .add(glonassOrbit.getDeltaT()) + .add(GLONASS_MEAN_DRACONIAN_PERIOD); + final FieldUnivariateDerivative2 n = + tDR.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI).reciprocal(); + + // Semi-major axis : computed by successive approximation + final FieldUnivariateDerivative2 sma = computeSma(tDR, i, e); + + // (ae / p)^2 term + final FieldUnivariateDerivative2 p = + sma.multiply(e.multiply(e).negate().add(1.0)); + final FieldUnivariateDerivative2 aeop = + p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); + final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); + + // Current longitude of the ascending node + final FieldUnivariateDerivative2 lambda = + computeLambda(dTpr, n, aeop2, i); + + // Current argument of perigee + final FieldUnivariateDerivative2 pa = computePA(dTpr, n, aeop2, i); + + // Mean longitude at the instant the spacecraft passes the current + // ascending + // node + final FieldUnivariateDerivative2 tanPAo2 = + FastMath.tan(pa.divide(2.0)); + final FieldUnivariateDerivative2 coef = + tanPAo2.multiply(FastMath + .sqrt(e.negate().add(1.0).divide(e.add(1.0)))); + final FieldUnivariateDerivative2 e0 = + FastMath.atan(coef).multiply(2.0).negate(); + final FieldUnivariateDerivative2 m1 = + pa.add(e0).subtract(FastMath.sin(e0).multiply(e)); + + // Current mean longitude + final FieldUnivariateDerivative2 correction = + dTpr.subtract(w.multiply(zero.add(GLONASS_MEAN_DRACONIAN_PERIOD) + .add(glonassOrbit.getDeltaT()))) + .subtract(w.multiply(w).multiply(glonassOrbit.getDeltaTDot())); + final FieldUnivariateDerivative2 m = m1.add(n.multiply(correction)); + + // Take into consideration the periodic perturbations + final FieldSinCos> scPa = + FastMath.sinCos(pa); + final FieldUnivariateDerivative2 h = e.multiply(scPa.sin()); + final FieldUnivariateDerivative2 l = e.multiply(scPa.cos()); + // δa1 + final FieldUnivariateDerivative2[] d1 = + getParameterDifferentials(sma, i, h, l, m1); + // δa2 + final FieldUnivariateDerivative2[] d2 = + getParameterDifferentials(sma, i, h, l, m); + // Apply corrections + final FieldUnivariateDerivative2 smaCorr = + sma.add(d2[0]).subtract(d1[0]); + final FieldUnivariateDerivative2 hCorr = + h.add(d2[1]).subtract(d1[1]); + final FieldUnivariateDerivative2 lCorr = + l.add(d2[2]).subtract(d1[2]); + final FieldUnivariateDerivative2 lambdaCorr = + lambda.add(d2[3]).subtract(d1[3]); + final FieldUnivariateDerivative2 iCorr = + i.add(d2[4]).subtract(d1[4]); + final FieldUnivariateDerivative2 mCorr = + m.add(d2[5]).subtract(d1[5]); + final FieldUnivariateDerivative2 eCorr = + FastMath.sqrt(hCorr.multiply(hCorr).add(lCorr.multiply(lCorr))); + final FieldUnivariateDerivative2 paCorr; + if (eCorr.getValue().getReal() == 0.) { + paCorr = zero; + } else { + if (lCorr.getValue() == eCorr.getValue()) { + paCorr = zero.add(0.5 * FieldGLONASSOrbitalElements.GLONASS_PI); + } else if (lCorr.getValue() + .getReal() == -eCorr.getValue().getReal()) { + paCorr = + zero.add(-0.5 * FieldGLONASSOrbitalElements.GLONASS_PI); + } else { + paCorr = FastMath.atan2(hCorr, lCorr); + } + } + + // Eccentric Anomaly + final FieldUnivariateDerivative2 mk = mCorr.subtract(paCorr); + final FieldUnivariateDerivative2 ek = getEccentricAnomaly(mk, eCorr); + + // True Anomaly + final FieldUnivariateDerivative2 vk = getTrueAnomaly(ek, eCorr); + + // Argument of Latitude + final FieldUnivariateDerivative2 phik = vk.add(paCorr); + + // Corrected Radius + final FieldUnivariateDerivative2 pCorr = + smaCorr.multiply(eCorr.multiply(eCorr).negate().add(1.0)); + final FieldUnivariateDerivative2 rk = + pCorr.divide(eCorr.multiply(FastMath.cos(vk)).add(1.0)); + + // Positions in orbital plane + final FieldSinCos> scPhik = + FastMath.sinCos(phik); + final FieldUnivariateDerivative2 xk = scPhik.cos().multiply(rk); + final FieldUnivariateDerivative2 yk = scPhik.sin().multiply(rk); + + // Coordinates of position + final FieldSinCos> scL = + FastMath.sinCos(lambdaCorr); + final FieldSinCos> scI = + FastMath.sinCos(iCorr); + final FieldVector3D> positionwithDerivatives = + new FieldVector3D<>(xk.multiply(scL.cos()) + .subtract(yk.multiply(scL.sin()).multiply(scI.cos())), + xk.multiply(scL.sin()) + .add(yk.multiply(scL.cos()) + .multiply(scI.cos())), + yk.multiply(scI.sin())); + + return new FieldPVCoordinates(new FieldVector3D(positionwithDerivatives + .getX().getValue(), positionwithDerivatives.getY().getValue(), + positionwithDerivatives + .getZ() + .getValue()), + new FieldVector3D(positionwithDerivatives + .getX().getFirstDerivative(), + positionwithDerivatives + .getY() + .getFirstDerivative(), + positionwithDerivatives + .getZ() + .getFirstDerivative()), + new FieldVector3D(positionwithDerivatives + .getX().getSecondDerivative(), + positionwithDerivatives + .getY() + .getSecondDerivative(), + positionwithDerivatives + .getZ() + .getSecondDerivative())); + } + + /** + * Gets eccentric anomaly from mean anomaly. + *

+ * The algorithm used to solve the Kepler equation has been published in: + * "Procedures for solving Kepler's Equation", A. W. Odell and R. H. + * Gooding, Celestial Mechanics 38 (1986) 307-334 + *

+ *

+ * It has been copied from the OREKIT library (KeplerianOrbit class). + *

+ * + * @param mk the mean anomaly (rad) + * @param e the eccentricity + * @return the eccentric anomaly (rad) + */ + private FieldUnivariateDerivative2 + getEccentricAnomaly(final FieldUnivariateDerivative2 mk, + final FieldUnivariateDerivative2 e) { + + // reduce M to [-PI PI] interval + final T zero = mk.getValue().getField().getZero(); + final FieldUnivariateDerivative2 reducedM = + new FieldUnivariateDerivative2(MathUtils + .normalizeAngle(mk.getValue(), zero), mk.getFirstDerivative(), + mk.getSecondDerivative()); + + // compute start value according to A. W. Odell and R. H. Gooding S12 + // starter + FieldUnivariateDerivative2 ek; + if (FastMath.abs(reducedM.getValue()).getReal() < 1.0 / 6.0) { + if (FastMath.abs(reducedM.getValue()) + .getReal() < Precision.SAFE_MIN) { + // this is an Orekit change to the S12 starter. + // If reducedM is 0.0, the derivative of cbrt is infinite which + // induces NaN + // appearing later in + // the computation. As in this case E and M are almost equal, we + // initialize ek + // with reducedM + ek = reducedM; + } else { + // this is the standard S12 starter + ek = + reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM) + .multiply(e)); + } + } else { + if (reducedM.getValue().getReal() < 0) { + final FieldUnivariateDerivative2 w = + reducedM.add(FastMath.PI); + ek = + reducedM.add(w.multiply(-A).divide(w.subtract(B)) + .subtract(FastMath.PI).subtract(reducedM).multiply(e)); + } else { + final FieldUnivariateDerivative2 minusW = + reducedM.subtract(FastMath.PI); + ek = + reducedM.add(minusW.multiply(A).divide(minusW.add(B)) + .add(FastMath.PI).subtract(reducedM).multiply(e)); + } + } + + final FieldUnivariateDerivative2 e1 = e.negate().add(1.0); + final boolean noCancellationRisk = + (e1.getReal() + + ek.getValue().getReal() * ek.getValue().getReal() / 6.0) >= 0.1; + + // perform two iterations, each consisting of one Halley step and one + // Newton-Raphson step + for (int j = 0; j < 2; ++j) { + final FieldUnivariateDerivative2 f; + FieldUnivariateDerivative2 fd; + final FieldUnivariateDerivative2 fdd = ek.sin().multiply(e); + final FieldUnivariateDerivative2 fddd = ek.cos().multiply(e); + if (noCancellationRisk) { + f = ek.subtract(fdd).subtract(reducedM); + fd = fddd.subtract(1).negate(); + } else { + f = eMeSinE(ek, e).subtract(reducedM); + final FieldUnivariateDerivative2 s = ek.multiply(0.5).sin(); + fd = s.multiply(s).multiply(e.multiply(2.0)).add(e1); + } + final FieldUnivariateDerivative2 dee = + f.multiply(fd).divide(f.multiply(0.5).multiply(fdd) + .subtract(fd.multiply(fd))); + + // update eccentric anomaly, using expressions that limit underflow + // problems + final FieldUnivariateDerivative2 w = + fd.add(dee.multiply(0.5) + .multiply(fdd.add(dee.multiply(fdd).divide(3)))); + fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd)))); + ek = + ek.subtract(f.subtract(dee.multiply(fd.subtract(w))) + .divide(fd)); + } + + // expand the result back to original range + ek = ek.add(mk.getValue().subtract(reducedM.getValue())); + + // Returns the eccentric anomaly + return ek; + } + + /** + * Accurate computation of E - e sin(E). + * + * @param E eccentric anomaly + * @param ecc the eccentricity + * @return E - e sin(E) + */ + private FieldUnivariateDerivative2 + eMeSinE(final FieldUnivariateDerivative2 E, + final FieldUnivariateDerivative2 ecc) { + FieldUnivariateDerivative2 x = + E.sin().multiply(ecc.negate().add(1.0)); + final FieldUnivariateDerivative2 mE2 = E.negate().multiply(E); + FieldUnivariateDerivative2 term = E; + FieldUnivariateDerivative2 d = E.getField().getZero(); + // the inequality test below IS intentional and should NOT be replaced + // by a + // check with a small tolerance + for (FieldUnivariateDerivative2 x0 = d.add(Double.NaN); + !(x.getValue()).equals(x0.getValue());) { + d = d.add(2); + term = term.multiply(mE2.divide(d.multiply(d.add(1)))); + x0 = x; + x = x.subtract(term); + } + return x; + } + + /** + * Gets true anomaly from eccentric anomaly. + * + * @param ek the eccentric anomaly (rad) + * @param ecc the eccentricity + * @return the true anomaly (rad) + */ + private FieldUnivariateDerivative2 + getTrueAnomaly(final FieldUnivariateDerivative2 ek, + final FieldUnivariateDerivative2 ecc) { + final FieldUnivariateDerivative2 svk = + ek.sin() + .multiply(FastMath.sqrt(ecc.multiply(ecc).negate().add(1.0))); + final FieldUnivariateDerivative2 cvk = ek.cos().subtract(ecc); + return svk.atan2(cvk); + } + + /** + * Get the interval of prediction. + * + * @param date the considered date + * @return the duration from GLONASS orbit Reference epoch (s) + */ + private FieldUnivariateDerivative2 + getdTpr(final FieldAbsoluteDate date) { + final TimeScale glonass = dataContext.getTimeScales().getGLONASS(); + final FieldGLONASSDate tEnd = + new FieldGLONASSDate(date.getField(), date, glonass); + final FieldGLONASSDate tSta = + new FieldGLONASSDate(date.getField(), glonassOrbit.getDate(), + glonass); + final int n = tEnd.getDayNumber(); + final int na = tSta.getDayNumber(); + final int deltaN; + if (na == 27) { + deltaN = n - na - FastMath.round((float) (n - na) / 1460) * 1460; + } else { + deltaN = n - na - FastMath.round((float) (n - na) / 1461) * 1461; + } + + final T zero = date.getField().getZero(); + final FieldUnivariateDerivative2 ti = + new FieldUnivariateDerivative2(zero.add(tEnd.getSecInDay()), + zero.add(1.0), zero); + + return ti.subtract(glonassOrbit.getTime()).add(86400 * deltaN); + } + + /** + * Computes the semi-major axis of orbit using technique of successive + * approximations. + * + * @param tDR mean draconique period (s) + * @param i current inclination (rad) + * @param e eccentricity + * @return the semi-major axis (m). + */ + private FieldUnivariateDerivative2 + computeSma(final FieldUnivariateDerivative2 tDR, + final FieldUnivariateDerivative2 i, + final FieldUnivariateDerivative2 e) { + + // Zero + final FieldUnivariateDerivative2 zero = tDR.getField().getZero(); + + // If one of the input parameter is equal to Double.NaN, an infinite + // loop can + // occur. + // In that case, we do not compute the value of the semi major axis. + // We decided to return a Double.NaN value instead. + if (Double.isNaN(tDR.getValue().getReal()) || + Double.isNaN(i.getValue().getReal()) || + Double.isNaN(e.getValue().getReal())) { + return zero.add(Double.NaN); + } + + // Common parameters + final FieldUnivariateDerivative2 sinI = FastMath.sin(i); + final FieldUnivariateDerivative2 sin2I = sinI.multiply(sinI); + final FieldUnivariateDerivative2 ome2 = + e.multiply(e).negate().add(1.0); + final FieldUnivariateDerivative2 ome2Pow3o2 = + FastMath.sqrt(ome2).multiply(ome2); + final FieldUnivariateDerivative2 pa = zero.add(glonassOrbit.getPa()); + final FieldUnivariateDerivative2 cosPA = FastMath.cos(pa); + final FieldUnivariateDerivative2 opecosPA = + e.multiply(cosPA).add(1.0); + final FieldUnivariateDerivative2 opecosPAPow2 = + opecosPA.multiply(opecosPA); + final FieldUnivariateDerivative2 opecosPAPow3 = + opecosPAPow2.multiply(opecosPA); + + // Initial approximation + FieldUnivariateDerivative2 tOCK = tDR; + + // Successive approximations + // The process of approximation ends when fulfilling the following + // condition: + // |a(n+1) - a(n)| < 1cm + FieldUnivariateDerivative2 an = zero; + FieldUnivariateDerivative2 anp1 = zero; + boolean isLastStep = false; + while (!isLastStep) { + + // a(n+1) computation + final FieldUnivariateDerivative2 tOCKo2p = + tOCK.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI); + final FieldUnivariateDerivative2 tOCKo2pPow2 = + tOCKo2p.multiply(tOCKo2p); + anp1 = + FastMath.cbrt(tOCKo2pPow2 + .multiply(GLONASSOrbitalElements.GLONASS_MU)); + + // p(n+1) computation + final FieldUnivariateDerivative2 p = anp1.multiply(ome2); + + // Tock(n+1) computation + final FieldUnivariateDerivative2 aeop = + p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); + final FieldUnivariateDerivative2 aeop2 = aeop.multiply(aeop); + final FieldUnivariateDerivative2 term1 = + aeop2.multiply(GLONASS_J20).multiply(1.5); + final FieldUnivariateDerivative2 term2 = + sin2I.multiply(2.5).negate().add(2.0); + final FieldUnivariateDerivative2 term3 = + ome2Pow3o2.divide(opecosPAPow2); + final FieldUnivariateDerivative2 term4 = + opecosPAPow3.divide(ome2); + tOCK = + tDR.divide(term1.multiply(term2.multiply(term3).add(term4)) + .negate().add(1.0)); + + // Check convergence + if (FastMath.abs(anp1.subtract(an).getReal()) <= 0.01) { + isLastStep = true; + } + + an = anp1; + } + + return an; + + } + + /** + * Computes the current longitude of the ascending node. + * + * @param dTpr interval of prediction (s) + * @param n mean motion (rad/s) + * @param aeop2 square of the ratio between the radius of the ellipsoid and + * p, with p = sma * (1 - ecc²) + * @param i inclination (rad) + * @return the current longitude of the ascending node (rad) + */ + private FieldUnivariateDerivative2 + computeLambda(final FieldUnivariateDerivative2 dTpr, + final FieldUnivariateDerivative2 n, + final FieldUnivariateDerivative2 aeop2, + final FieldUnivariateDerivative2 i) { + final FieldUnivariateDerivative2 cosI = FastMath.cos(i); + final FieldUnivariateDerivative2 precession = + aeop2.multiply(n).multiply(cosI).multiply(1.5 * GLONASS_J20); + return dTpr.multiply(precession.add(GLONASS_AV)).negate() + .add(glonassOrbit.getLambda()); + } + + /** + * Computes the current argument of perigee. + * + * @param dTpr interval of prediction (s) + * @param n mean motion (rad/s) + * @param aeop2 square of the ratio between the radius of the ellipsoid and + * p, with p = sma * (1 - ecc²) + * @param i inclination (rad) + * @return the current argument of perigee (rad) + */ + private FieldUnivariateDerivative2 + computePA(final FieldUnivariateDerivative2 dTpr, + final FieldUnivariateDerivative2 n, + final FieldUnivariateDerivative2 aeop2, + final FieldUnivariateDerivative2 i) { + final FieldUnivariateDerivative2 cosI = FastMath.cos(i); + final FieldUnivariateDerivative2 cos2I = cosI.multiply(cosI); + final FieldUnivariateDerivative2 precession = + aeop2.multiply(n).multiply(cos2I.multiply(5.0).negate().add(1.0)) + .multiply(0.75 * GLONASS_J20); + return dTpr.multiply(precession).negate().add(glonassOrbit.getPa()); + } + + /** + * Computes the differentials δai. + *

+ * The value of i depends of the type of longitude (i = 2 for the current + * mean longitude; i = 1 for the mean longitude at the instant the + * spacecraft passes the current ascending node) + *

+ * + * @param a semi-major axis (m) + * @param i inclination (rad) + * @param h x component of the eccentricity (rad) + * @param l y component of the eccentricity (rad) + * @param m longitude (current or at the ascending node instant) + * @return the differentials of the orbital parameters + */ + private FieldUnivariateDerivative2[] + getParameterDifferentials(final FieldUnivariateDerivative2 a, + final FieldUnivariateDerivative2 i, + final FieldUnivariateDerivative2 h, + final FieldUnivariateDerivative2 l, + final FieldUnivariateDerivative2 m) { + + // B constant + final FieldUnivariateDerivative2 aeoa = + a.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal(); + final FieldUnivariateDerivative2 aeoa2 = aeoa.multiply(aeoa); + final FieldUnivariateDerivative2 b = + aeoa2.multiply(1.5 * GLONASS_J20); + + // Commons Parameters + final FieldSinCos> scI = + FastMath.sinCos(i); + final FieldSinCos> scLk = + FastMath.sinCos(m); + final FieldSinCos> sc2Lk = + FieldSinCos.sum(scLk, scLk); + final FieldSinCos> sc3Lk = + FieldSinCos.sum(scLk, sc2Lk); + final FieldSinCos> sc4Lk = + FieldSinCos.sum(sc2Lk, sc2Lk); + final FieldUnivariateDerivative2 cosI = scI.cos(); + final FieldUnivariateDerivative2 sinI = scI.sin(); + final FieldUnivariateDerivative2 cosI2 = cosI.multiply(cosI); + final FieldUnivariateDerivative2 sinI2 = sinI.multiply(sinI); + final FieldUnivariateDerivative2 cosLk = scLk.cos(); + final FieldUnivariateDerivative2 sinLk = scLk.sin(); + final FieldUnivariateDerivative2 cos2Lk = sc2Lk.cos(); + final FieldUnivariateDerivative2 sin2Lk = sc2Lk.sin(); + final FieldUnivariateDerivative2 cos3Lk = sc3Lk.cos(); + final FieldUnivariateDerivative2 sin3Lk = sc3Lk.sin(); + final FieldUnivariateDerivative2 cos4Lk = sc4Lk.cos(); + final FieldUnivariateDerivative2 sin4Lk = sc4Lk.sin(); + + // h*cos(nLk), l*cos(nLk), h*sin(nLk) and l*sin(nLk) + // n = 1 + final FieldUnivariateDerivative2 hCosLk = h.multiply(cosLk); + final FieldUnivariateDerivative2 hSinLk = h.multiply(sinLk); + final FieldUnivariateDerivative2 lCosLk = l.multiply(cosLk); + final FieldUnivariateDerivative2 lSinLk = l.multiply(sinLk); + // n = 2 + final FieldUnivariateDerivative2 hCos2Lk = h.multiply(cos2Lk); + final FieldUnivariateDerivative2 hSin2Lk = h.multiply(sin2Lk); + final FieldUnivariateDerivative2 lCos2Lk = l.multiply(cos2Lk); + final FieldUnivariateDerivative2 lSin2Lk = l.multiply(sin2Lk); + // n = 3 + final FieldUnivariateDerivative2 hCos3Lk = h.multiply(cos3Lk); + final FieldUnivariateDerivative2 hSin3Lk = h.multiply(sin3Lk); + final FieldUnivariateDerivative2 lCos3Lk = l.multiply(cos3Lk); + final FieldUnivariateDerivative2 lSin3Lk = l.multiply(sin3Lk); + // n = 4 + final FieldUnivariateDerivative2 hCos4Lk = h.multiply(cos4Lk); + final FieldUnivariateDerivative2 hSin4Lk = h.multiply(sin4Lk); + final FieldUnivariateDerivative2 lCos4Lk = l.multiply(cos4Lk); + final FieldUnivariateDerivative2 lSin4Lk = l.multiply(sin4Lk); + + // 1 - (3 / 2)*sin²i + final FieldUnivariateDerivative2 om3o2xSinI2 = + sinI2.multiply(1.5).negate().add(1.0); + + // Compute Differentials + // δa + final FieldUnivariateDerivative2 dakT1 = + b.multiply(2.0).multiply(om3o2xSinI2).multiply(lCosLk.add(hSinLk)); + final FieldUnivariateDerivative2 dakT2 = + b.multiply(sinI2) + .multiply(hSinLk.multiply(0.5).subtract(lCosLk.multiply(0.5)) + .add(cos2Lk).add(lCos3Lk.multiply(3.5)) + .add(hSin3Lk.multiply(3.5))); + final FieldUnivariateDerivative2 dak = dakT1.add(dakT2); + + // δh + final FieldUnivariateDerivative2 dhkT1 = + b.multiply(om3o2xSinI2).multiply(sinLk.add(lSin2Lk.multiply(1.5)) + .subtract(hCos2Lk.multiply(1.5))); + final FieldUnivariateDerivative2 dhkT2 = + b.multiply(sinI2).multiply(0.25) + .multiply(sinLk.subtract(sin3Lk.multiply(SEVEN_THIRD)) + .add(lSin2Lk.multiply(5.0)).subtract(lSin4Lk.multiply(8.5)) + .add(hCos4Lk.multiply(8.5)).add(hCos2Lk)); + final FieldUnivariateDerivative2 dhkT3 = + lSin2Lk.multiply(cosI2).multiply(b).multiply(0.5).negate(); + final FieldUnivariateDerivative2 dhk = + dhkT1.subtract(dhkT2).add(dhkT3); + + // δl + final FieldUnivariateDerivative2 dlkT1 = + b.multiply(om3o2xSinI2).multiply(cosLk.add(lCos2Lk.multiply(1.5)) + .add(hSin2Lk.multiply(1.5))); + final FieldUnivariateDerivative2 dlkT2 = + b.multiply(sinI2).multiply(0.25) + .multiply(cosLk.negate().subtract(cos3Lk.multiply(SEVEN_THIRD)) + .subtract(hSin2Lk.multiply(5.0)) + .subtract(lCos4Lk.multiply(8.5)) + .subtract(hSin4Lk.multiply(8.5)).add(lCos2Lk)); + final FieldUnivariateDerivative2 dlkT3 = + hSin2Lk.multiply(cosI2).multiply(b).multiply(0.5); + final FieldUnivariateDerivative2 dlk = + dlkT1.subtract(dlkT2).add(dlkT3); + + // δλ + final FieldUnivariateDerivative2 dokT1 = b.negate().multiply(cosI); + final FieldUnivariateDerivative2 dokT2 = + lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)) + .subtract(sin2Lk.multiply(0.5)) + .subtract(lSin3Lk.multiply(SEVEN_SIXTH)) + .add(hCos3Lk.multiply(SEVEN_SIXTH)); + final FieldUnivariateDerivative2 dok = dokT1.multiply(dokT2); + + // δi + final FieldUnivariateDerivative2 dik = + b.multiply(sinI).multiply(cosI).multiply(0.5) + .multiply(lCosLk.negate().add(hSinLk).add(cos2Lk) + .add(lCos3Lk.multiply(SEVEN_THIRD)) + .add(hSin3Lk.multiply(SEVEN_THIRD))); + + // δL + final FieldUnivariateDerivative2 dLkT1 = + b.multiply(2.0).multiply(om3o2xSinI2).multiply(lSinLk.multiply(1.75) + .subtract(hCosLk.multiply(1.75))); + final FieldUnivariateDerivative2 dLkT2 = + b.multiply(sinI2).multiply(3.0) + .multiply(hCosLk.multiply(SEVEN_24TH).negate() + .subtract(lSinLk.multiply(SEVEN_24TH)) + .subtract(hCos3Lk.multiply(FN_72TH)) + .add(lSin3Lk.multiply(FN_72TH)).add(sin2Lk.multiply(0.25))); + final FieldUnivariateDerivative2 dLkT3 = + b.multiply(cosI2) + .multiply(lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)) + .subtract(sin2Lk.multiply(0.5)) + .subtract(lSin3Lk.multiply(SEVEN_SIXTH)) + .add(hCos3Lk.multiply(SEVEN_SIXTH))); + final FieldUnivariateDerivative2 dLk = dLkT1.add(dLkT2).add(dLkT3); + + // Final array + final FieldUnivariateDerivative2[] differentials = + MathArrays.buildArray(a.getField(), 6); + differentials[0] = dak.multiply(a); + differentials[1] = dhk; + differentials[2] = dlk; + differentials[3] = dok; + differentials[4] = dik; + differentials[5] = dLk; + + return differentials; + } + + /** {@inheritDoc} */ + protected T getMass(final FieldAbsoluteDate date) { + return mass; + } + + /** + * Get the Earth gravity coefficient used for GLONASS propagation. + * + * @return the Earth gravity coefficient. + */ + public T getMU() { + return getField().getZero().add(GLONASSOrbitalElements.GLONASS_MU); + } + + /** + * Gets the underlying Field GLONASS orbital elements. + * + * @return the underlying Field GLONASS orbital elements + */ + public FieldGLONASSOrbitalElements getGLONASSOrbitalElements() { + return glonassOrbit; + } + + /** + * Gets the Earth Centered Inertial frame used to propagate the orbit. + * + * @return the ECI frame + */ + public Frame getECI() { + return eci; + } + + /** + * Gets the Earth Centered Earth Fixed frame used to propagate GLONASS + * orbits. + * + * @return the ECEF frame + */ + public Frame getECEF() { + return ecef; + } + + /** {@inheritDoc} */ + public Frame getFrame() { + return eci; + } + + /** {@inheritDoc} */ + public void resetInitialState(final FieldSpacecraftState state) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected void resetIntermediateState(final FieldSpacecraftState state, + final boolean forward) { + throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE); + } + + /** {@inheritDoc} */ + protected FieldOrbit propagateOrbit(final FieldAbsoluteDate date, + final T[] parameters) { + // Gets the PVCoordinates in ECEF frame + final FieldPVCoordinates pvaInECEF = propagateInEcef(date); + // Transforms the PVCoordinates to ECI frame + final FieldPVCoordinates pvaInECI = + ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF); + // Returns the Cartesian orbit + return new FieldCartesianOrbit<>(pvaInECI, eci, date, date.getField() + .getZero().add(FieldGLONASSOrbitalElements.GLONASS_MU)); + } + + /** + * Get the parameters driver for the Field Glonass propagation model. + * * @return an empty list. */ - @Override - protected List getParametersDrivers() { - // The Field Glonass propagation model does not have parameter drivers. - return Collections.emptyList(); - } + @Override + protected List getParametersDrivers() { + // The Field Glonass propagation model does not have parameter drivers. + return Collections.emptyList(); + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java index 8d9689499..f5753aa67 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSDate.java @@ -1,3 +1,19 @@ +/* Copyright 2002-2021 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ package org.orekit.propagation.analytical.gnss; import java.io.Serializable; @@ -16,272 +32,304 @@ import org.orekit.time.TimeComponents; import org.orekit.time.TimeScale; import org.orekit.utils.Constants; -public class FieldGLONASSDate> implements Serializable, FieldTimeStamped { - - /** Serializable UID. */ - private static final long serialVersionUID = 20190131L; - - /** Constant for date computation. */ - private static final int C1 = 44195; - - /** Constant for date computation. */ - private static final int C2 = 45290; - - /** The number of the current day in a four year interval Na. */ - private final int na; - - /** The number of the current four year interval N4. */ - private final int n4; - - /** Number of seconds since Na. */ - private final T secInNa; - - /** Current Julian date JD0. */ - private T jd0; - - /** Greenwich Mean Sidereal Time (rad). */ - private T gmst; - - /** Corresponding date. */ - private final transient FieldAbsoluteDate date; - - private final Field field; - - - /** - * Build an instance corresponding to a GLONASS date. - * - *

- * This method uses the {@link DataContext#getDefault() default data context}. - * - * @param na the number of the current day in a four year interval - * @param n4 the number of the current four year interval - * @param secInNa the number of seconds since na start - * @see #GLONASSDate(int, int, T, TimeScale) - */ - @DefaultDataContext - public FieldGLONASSDate(final Field field, final int na, final int n4, final T secInNa) { - this(field, na, n4, secInNa, DataContext.getDefault().getTimeScales().getGLONASS()); - } - - /** - * Build an instance corresponding to a GLONASS date. - * - * @param na the number of the current day in a four year interval - * @param n4 the number of the current four year interval - * @param secInNa the number of seconds since na start - * @param glonass time scale. - * @since 10.1 - */ - public FieldGLONASSDate(final Field field, final int na, final int n4, final T secInNa, final TimeScale glonass) { - this.field = field; - this.na = na; - this.n4 = n4; - final T zero = field.getZero(); - this.secInNa = zero.add(secInNa); - // Compute JD0 - final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); - this.jd0 = zero.add(1461 * (n4 - 1) + na + 2450082.5 - ratio); - // GMST - this.gmst = computeGMST(); - this.date = computeDate(field, glonass); - } - - /** - * Build an instance from an absolute date. - * - *

- * This method uses the {@link DataContext#getDefault() default data context}. - * - * @param date absolute date to consider - * @see #GLONASSDate(AbsoluteDate, TimeScale) - */ - @DefaultDataContext - public FieldGLONASSDate(final Field field, final FieldAbsoluteDate date) { - this(field, date, DataContext.getDefault().getTimeScales().getGLONASS()); - } - - /** - * Build an instance from an absolute date. - * - * @param date absolute date to consider - * @param glonass time scale. - * @since 10.1 - */ - public FieldGLONASSDate(final Field field, final FieldAbsoluteDate date, final TimeScale glonass) { - this.field = field; - final DateTimeComponents dateTime = date.getComponents(glonass); - // N4 - final int year = dateTime.getDate().getYear(); - this.n4 = ((int) (year - 1996) / 4) + 1; - // Na - final int start = 1996 + 4 * (n4 - 1); - - final T zero = field.getZero(); - final T duration = zero.add(date.durationFrom(new FieldAbsoluteDate(field, start, 1, 1, glonass)) - .getReal()); - - this.na = (int) (duration.getReal() / 86400) + 1; - this.secInNa = zero.add(dateTime.getTime().getSecondsInLocalDay()); - // Compute JD0 - final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); - this.jd0 = zero.add(1461 * (n4 - 1) + na + 2450082.5 - ratio); - // GMST - this.gmst = computeGMST(); - this.date = date; - } - - @Override - public FieldAbsoluteDate getDate() { - return date; - } - - /** - * Get the number of seconds since Na start. - * - * @return number of seconds since Na start - */ - public T getSecInDay() { - return secInNa; - } - - /** - * Get the number of the current day in a four year interval. - * - * @return the number of the current day in a four year interval - */ - public int getDayNumber() { - return na; - } - - /** - * Get the number of the current four year interval. - * - * @return the number of the current four year interval - */ - public int getIntervalNumber() { - return n4; - } - - /** - * Get the current Julian date JD0. - * - * @return the current date JD0 - */ - public T getJD0() { - return jd0; - } - - /** - * Get the Greenwich Mean Sidereal Time. - * - * @return the Greenwich Mean Sidereal Time (rad) - */ - public T getGMST() { - return gmst; - } - - /** - * Compute the Greenwich Mean Sidereal Time using the current Julian date JD0. - * - * @return the Greenwich Mean Sidereal Time (rad) - */ - private T computeGMST() { - final T zero = field.getZero(); - final T ref = zero.add(2451545.0); - // Earth's rotation angle in radians - final T era = zero.add(2. * GLONASSOrbitalElements.GLONASS_PI - * (0.7790572732640)).add((jd0.subtract(ref)).multiply(1.00273781191135448)); - // Time from Epoch 2000 (1st January, 00:00 UTC) till current Epoch in Julian - // centuries - final T time = (jd0.subtract(ref)).divide(Constants.JULIAN_CENTURY); - // Time to the power n - final T time2 = time.multiply(time); - final T time3 = time2.multiply(time); - final T time4 = time2.multiply(time2); - final T time5 = time2.multiply(time3); - // GMST computation - final T gTime = era.add(7.03270726e-8).add(time.multiply(2.23603658710194e-2)).add(time2.add(6.7465784654e-6)) - .subtract(time3.multiply(2.1332e-12)).subtract(time4.multiply(1.452308e-10)).subtract(time5.multiply(1.784e-13)); - return gTime; - } - - /** - * Compute the GLONASS date. - * - * @return the date - * @param glonass time scale. - */ - private FieldAbsoluteDate computeDate(final Field field, final TimeScale glonass) { - // Compute the number of Julian day for the current date - final T jdn = jd0.add(0.5); - // Coefficients - final int a = (int) (jdn.getReal() + 32044); - final int b = (4 * a + 3) / 146097; - final int c = a - (146097 * b) / 4; - final int d = (4 * c + 3) / 1461; - final int e = c - (1461 * d) / 4; - final int m = (5 * e + 2) / 153; - // Year, month and day - final int day = e - (153 * m + 2) / 5 + 1; - final int month = m + 3 - 12 * (m / 10); - final int year = 100 * b + d - 4800 + m / 10; - - return new FieldAbsoluteDate(field, new DateComponents(year, month, day), - new TimeComponents(secInNa.getReal()), glonass); - } - - /** - * Replace the instance with a data transfer object for serialization. - * - * @return data transfer object that will be serialized - */ - @DefaultDataContext - private Object writeReplace() { - return new DataTransferObject(field, na, n4, secInNa); - } - - /** Internal class used only for serialization. */ - @DefaultDataContext - private class DataTransferObject implements Serializable { - - /** Serializable UID. */ - private static final long serialVersionUID = 20190131L; - - /** The number of the current day in a four year interval Na. */ - private final int na; - - /** The number of the current four year interval N4. */ - private final int n4; - - /** Number of seconds since Na. */ - private final T secInNa; - - private final Field field; - - /** - * Simple constructor. - * - * @param na the number of the current day in a four year interval - * @param n4 the number of the current four year interval - * @param secInNa the number of seconds since na start - */ - DataTransferObject(final Field field, final int na, final int n4, final T secInNa) { - this.field = field; - this.na = na; - this.n4 = n4; - this.secInNa = secInNa; - } - - /** - * Replace the deserialized data transfer object with a {@link GPSDate}. - * - * @return replacement {@link GPSDate} - */ - private Object readResolve() { - return new FieldGLONASSDate<>(field, na, n4, secInNa); - } - - } +public class FieldGLONASSDate> + implements + Serializable, + FieldTimeStamped { + + /** Serializable UID. */ + private static final long serialVersionUID = 20190131L; + + /** Constant for date computation. */ + private static final int C1 = 44195; + + /** Constant for date computation. */ + private static final int C2 = 45290; + + /** The number of the current day in a four year interval Na. */ + private final int na; + + /** The number of the current four year interval N4. */ + private final int n4; + + /** Number of seconds since Na. */ + private final T secInNa; + + /** Current Julian date JD0. */ + private T jd0; + + /** Greenwich Mean Sidereal Time (rad). */ + private T gmst; + + /** Corresponding date. */ + private final transient FieldAbsoluteDate date; + + /** Field. */ + private final Field field; + + /** + * Build an instance corresponding to a GLONASS date. + *

+ * This method uses the {@link DataContext#getDefault() default data + * context}. + * + * @param field + * @param na the number of the current day in a four year interval + * @param n4 the number of the current four year interval + * @param secInNa the number of seconds since na start + * @see #GLONASSDate(int, int, T, TimeScale) + */ + @DefaultDataContext + public FieldGLONASSDate(final Field field, final int na, final int n4, + final T secInNa) { + this(field, na, n4, secInNa, + DataContext.getDefault().getTimeScales().getGLONASS()); + } + + /** + * Build an instance corresponding to a GLONASS date. + * + * @param field + * @param na the number of the current day in a four year interval + * @param n4 the number of the current four year interval + * @param secInNa the number of seconds since na start + * @param glonass time scale. + * @since 10.1 + */ + public FieldGLONASSDate(final Field field, final int na, final int n4, + final T secInNa, final TimeScale glonass) { + this.field = field; + this.na = na; + this.n4 = n4; + final T zero = field.getZero(); + this.secInNa = zero.add(secInNa); + // Compute JD0 + final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); + this.jd0 = zero.add(1461 * (n4 - 1) + na + 2450082.5 - ratio); + // GMST + this.gmst = computeGMST(); + this.date = computeDate(glonass); + } + + /** + * Build an instance from an absolute date. + *

+ * This method uses the {@link DataContext#getDefault() default data + * context}. + * + * @param field + * @param date absolute date to consider + * @see #GLONASSDate(AbsoluteDate, TimeScale) + */ + @DefaultDataContext + public FieldGLONASSDate(final Field field, + final FieldAbsoluteDate date) { + this(field, date, + DataContext.getDefault().getTimeScales().getGLONASS()); + } + + /** + * Build an instance from an absolute date. + * + * @param field + * @param date absolute date to consider + * @param glonass time scale. + * @since 10.1 + */ + public FieldGLONASSDate(final Field field, + final FieldAbsoluteDate date, + final TimeScale glonass) { + this.field = field; + final DateTimeComponents dateTime = date.getComponents(glonass); + // N4 + final int year = dateTime.getDate().getYear(); + this.n4 = ((int) (year - 1996) / 4) + 1; + // Na + final int start = 1996 + 4 * (n4 - 1); + + final T zero = field.getZero(); + final T duration = + zero.add(date.durationFrom(new FieldAbsoluteDate(field, start, 1, + 1, glonass)) + .getReal()); + + this.na = (int) (duration.getReal() / 86400) + 1; + this.secInNa = zero.add(dateTime.getTime().getSecondsInLocalDay()); + // Compute JD0 + final int ratio = FastMath.round((float) (na - 3) / (25 + C1 + C2)); + this.jd0 = zero.add(1461 * (n4 - 1) + na + 2450082.5 - ratio); + // GMST + this.gmst = computeGMST(); + this.date = date; + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + /** + * Get the number of seconds since Na start. + * + * @return number of seconds since Na start + */ + public T getSecInDay() { + return secInNa; + } + + /** + * Get the number of the current day in a four year interval. + * + * @return the number of the current day in a four year interval + */ + public int getDayNumber() { + return na; + } + + /** + * Get the number of the current four year interval. + * + * @return the number of the current four year interval + */ + public int getIntervalNumber() { + return n4; + } + + /** + * Get the current Julian date JD0. + * + * @return the current date JD0 + */ + public T getJD0() { + return jd0; + } + + /** + * Get the Greenwich Mean Sidereal Time. + * + * @return the Greenwich Mean Sidereal Time (rad) + */ + public T getGMST() { + return gmst; + } + + /** + * Compute the Greenwich Mean Sidereal Time using the current Julian date + * JD0. + * + * @return the Greenwich Mean Sidereal Time (rad) + */ + private T computeGMST() { + final T zero = field.getZero(); + final T ref = zero.add(2451545.0); + // Earth's rotation angle in radians + final T era = + zero.add(2. * GLONASSOrbitalElements.GLONASS_PI * 0.7790572732640) + .add((jd0.subtract(ref)).multiply(1.00273781191135448)); + // Time from Epoch 2000 (1st January, 00:00 UTC) till current Epoch in + // Julian + // centuries + final T time = (jd0.subtract(ref)).divide(Constants.JULIAN_CENTURY); + // Time to the power n + final T time2 = time.multiply(time); + final T time3 = time2.multiply(time); + final T time4 = time2.multiply(time2); + final T time5 = time2.multiply(time3); + // GMST computation + final T gTime = + era.add(7.03270726e-8).add(time.multiply(2.23603658710194e-2)) + .add(time2.add(6.7465784654e-6)) + .subtract(time3.multiply(2.1332e-12)) + .subtract(time4.multiply(1.452308e-10)) + .subtract(time5.multiply(1.784e-13)); + return gTime; + } + + /** + * Compute the GLONASS date. + * + * @param glonass time scale. + * @return the date + */ + private FieldAbsoluteDate computeDate(final TimeScale glonass) { + // Compute the number of Julian day for the current date + final T jdn = jd0.add(0.5); + // Coefficients + final int a = (int) (jdn.getReal() + 32044); + final int b = (4 * a + 3) / 146097; + final int c = a - (146097 * b) / 4; + final int d = (4 * c + 3) / 1461; + final int e = c - (1461 * d) / 4; + final int m = (5 * e + 2) / 153; + // Year, month and day + final int day = e - (153 * m + 2) / 5 + 1; + final int month = m + 3 - 12 * (m / 10); + final int year = 100 * b + d - 4800 + m / 10; + + return new FieldAbsoluteDate(field, + new DateComponents(year, month, day), + new TimeComponents(secInNa.getReal()), + glonass); + } + + /** + * Replace the instance with a data transfer object for serialization. + * + * @return data transfer object that will be serialized + */ + @DefaultDataContext + private Object writeReplace() { + return new DataTransferObject(field, na, n4, secInNa); + } + + /** Internal class used only for serialization. */ + @DefaultDataContext + private class DataTransferObject + implements + Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 20190131L; + + /** + * The number of the current day in a four year interval Na. + */ + private final int na; + + /** The number of the current four year interval N4. */ + private final int n4; + + /** Number of seconds since Na. */ + private final T secInNa; + + /** Field.*/ + private final Field field; + + /** + * Simple constructor. + * + * @param field + * @param na the number of the current day in a four year interval + * @param n4 the number of the current four year interval + * @param secInNa the number of seconds since na start + */ + DataTransferObject(final Field field, final int na, final int n4, + final T secInNa) { + this.field = field; + this.na = na; + this.n4 = n4; + this.secInNa = secInNa; + } + + /** + * Replace the deserialized data transfer object with a {@link GPSDate}. + * + * @return replacement {@link GPSDate} + */ + private Object readResolve() { + return new FieldGLONASSDate<>(field, na, n4, secInNa); + } + + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSOrbitalElements.java index 7cf79b8ad..795282cad 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGLONASSOrbitalElements.java @@ -22,30 +22,38 @@ import org.orekit.gnss.GLONASSEphemeris; import org.orekit.propagation.numerical.GLONASSNumericalPropagator; import org.orekit.time.FieldTimeStamped; -/** This interface provides the minimal set of orbital elements needed by the {@link GLONASSAnalyticalPropagator} and - * the {@link GLONASSNumericalPropagator}. +/** + * This interface provides the minimal set of orbital elements needed by the + * {@link GLONASSAnalyticalPropagator} and the + * {@link GLONASSNumericalPropagator}. *

- * Because input data are different between numerical and analytical GLONASS propagators the - * methods present in this interface are implemented by default. - * Depending if the user wants to use a {@link GLONASSNumericalPropagator} or a {@link GLONASSAnalyticalPropagator} - * he can create an instance of a {@link GLONASSEphemeris} or {@link GLONASSAlmanac}. + * Because input data are different between numerical and analytical GLONASS + * propagators the methods present in this interface are implemented by default. + * Depending if the user wants to use a {@link GLONASSNumericalPropagator} or a + * {@link GLONASSAnalyticalPropagator} he can create an instance of a + * {@link GLONASSEphemeris} or {@link GLONASSAlmanac}. *

* - * @see - * GLONASS Interface Control Document - * + * @see + * GLONASS Interface Control Document * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) - * */ -public interface FieldGLONASSOrbitalElements> extends FieldTimeStamped { - // Constants - /** Value of the Earth's universal gravitational parameter for GLONASS user in m³/s². */ +public interface FieldGLONASSOrbitalElements> + extends + FieldTimeStamped { + + // Constants + /** + * Value of the Earth's universal gravitational parameter for GLONASS user + * in m³/s². + */ double GLONASS_MU = 3.986004418e+14; /** Value of Pi for conversion from semicircles to radian. */ double GLONASS_PI = 3.14159265358979; - + /** * Get the number of the current day in a four year interval. * @@ -110,24 +118,27 @@ public interface FieldGLONASSOrbitalElements> exte T getDeltaTDot(); /** - * Get the relative deviation of predicted satellite carrier frequency from nominal value. + * Get the relative deviation of predicted satellite carrier frequency from + * nominal value. * - * @return the relative deviation of predicted satellite carrier frequency from nominal value + * @return the relative deviation of predicted satellite carrier frequency + * from nominal value */ - T getGammaN() ; + T getGammaN(); /** * Get the correction to the satellite time relative to GLONASS system time. * - * @return the correction to the satellite time relative to GLONASS system time (s) - * + * @return the correction to the satellite time relative to GLONASS system + * time (s) */ T getTN(); /** * Get the ECEF-X component of satellite velocity vector in PZ-90 datum. * - * @return the the ECEF-X component of satellite velocity vector in PZ-90 datum (m/s) + * @return the the ECEF-X component of satellite velocity vector in PZ-90 + * datum (m/s) */ T getXDot(); @@ -137,17 +148,21 @@ public interface FieldGLONASSOrbitalElements> exte * @return the ECEF-X component of satellite coordinates in PZ-90 datum (m) */ T getX(); + /** - * Get the GLONASS ECEF-X component of satellite acceleration vector in PZ-90 datum. + * Get the GLONASS ECEF-X component of satellite acceleration vector in + * PZ-90 datum. * - * @return the GLONASS ECEF-X component of satellite acceleration vector in PZ-90 datum (m/s²) + * @return the GLONASS ECEF-X component of satellite acceleration vector in + * PZ-90 datum (m/s²) */ T getXDotDot(); /** * Get the ECEF-Y component of satellite velocity vector in PZ-90 datum. * - * @return the ECEF-Y component of satellite velocity vector in PZ-90 datum (m/s) + * @return the ECEF-Y component of satellite velocity vector in PZ-90 datum + * (m/s) */ T getYDot(); @@ -159,16 +174,19 @@ public interface FieldGLONASSOrbitalElements> exte T getY(); /** - * Get the GLONASS ECEF-Y component of satellite acceleration vector in PZ-90 datum. + * Get the GLONASS ECEF-Y component of satellite acceleration vector in + * PZ-90 datum. * - * @return the GLONASS ECEF-Y component of satellite acceleration vector in PZ-90 datum (m/s²) + * @return the GLONASS ECEF-Y component of satellite acceleration vector in + * PZ-90 datum (m/s²) */ T getYDotDot(); /** * Get the ECEF-Z component of satellite velocity vector in PZ-90 datum. * - * @return the the ECEF-Z component of satellite velocity vector in PZ-90 datum (m/s) + * @return the the ECEF-Z component of satellite velocity vector in PZ-90 + * datum (m/s) */ T getZDot(); @@ -180,9 +198,11 @@ public interface FieldGLONASSOrbitalElements> exte T getZ(); /** - * Get the GLONASS ECEF-Z component of satellite acceleration vector in PZ-90 datum. + * Get the GLONASS ECEF-Z component of satellite acceleration vector in + * PZ-90 datum. * - * @return the GLONASS ECEF-Z component of satellite acceleration vector in PZ-90 datum (m/s²) + * @return the GLONASS ECEF-Z component of satellite acceleration vector in + * PZ-90 datum (m/s²) */ T getZDotDot(); diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSDate.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSDate.java index cc6fb2922..e2c93cea1 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSDate.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSDate.java @@ -42,15 +42,22 @@ import org.orekit.time.UT1Scale; import org.orekit.utils.Constants; import org.orekit.utils.IERSConventions; -/** Container for date in GNSS form. - *

This class can be used to handle {@link SatelliteSystem#GPS GPS}, - * {@link SatelliteSystem#GALILEO Galileo}, {@link SatelliteSystem#BEIDOU BeiDou} - * and {@link SatelliteSystem#QZSS QZSS} dates.

+/** + * Container for date in GNSS form. + *

+ * This class can be used to handle {@link SatelliteSystem#GPS GPS}, + * {@link SatelliteSystem#GALILEO Galileo}, {@link SatelliteSystem#BEIDOU + * BeiDou} and {@link SatelliteSystem#QZSS QZSS} dates. + *

+ * * @author Luc Maisonobe (original code) * @author Bryan Cazabonne (generalization to all GNSS constellations) * @author Nicolas Fialton (field translation) */ -public class FieldGNSSDate> implements Serializable, FieldTimeStamped { +public class FieldGNSSDate> + implements + Serializable, + FieldTimeStamped { /** Serializable UID. */ private static final long serialVersionUID = 201902141L; @@ -64,10 +71,13 @@ public class FieldGNSSDate> implements Serializabl /** Conversion factor from seconds to milliseconds. */ private static final double S_TO_MS = 1000.0; - /** Reference date for ensuring continuity across GNSS week rollover. + /** + * Reference date for ensuring continuity across GNSS week rollover. + * * @since 9.3.1 */ - private static AtomicReference rolloverReference = new AtomicReference(null); + private static AtomicReference rolloverReference = + new AtomicReference(null); /** Week number since the GNSS reference epoch. */ private final int weekNumber; @@ -81,21 +91,24 @@ public class FieldGNSSDate> implements Serializabl /** Corresponding date. */ private final transient FieldAbsoluteDate date; - /** Build an instance corresponding to a GNSS date. + /** + * Build an instance corresponding to a GNSS date. *

- * GNSS dates are provided as a week number starting at - * the GNSS reference epoch and as a number of milliseconds - * since week start. + * GNSS dates are provided as a week number starting at the GNSS reference + * epoch and as a number of milliseconds since week start. *

*

- * Many interfaces provide week number modulo the constellation week cycle. In order to cope with - * this, when the week number is smaller than the week cycle, this constructor assumes a modulo operation - * has been performed and it will fix the week number according to the reference date set up for - * handling rollover (see {@link #setRolloverReference(DateComponents) setRolloverReference(reference)}). - * If the week number is equal to the week cycle or larger, it will be used without any correction. + * Many interfaces provide week number modulo the constellation week cycle. + * In order to cope with this, when the week number is smaller than the week + * cycle, this constructor assumes a modulo operation has been performed and + * it will fix the week number according to the reference date set up for + * handling rollover (see {@link #setRolloverReference(DateComponents) + * setRolloverReference(reference)}). If the week number is equal to the + * week cycle or larger, it will be used without any correction. *

- * - *

This method uses the {@link DataContext#getDefault() default data context}. + *

+ * This method uses the {@link DataContext#getDefault() default data + * context}. * * @param weekNumber week number * @param milliInWeek number of milliseconds since week start @@ -104,53 +117,61 @@ public class FieldGNSSDate> implements Serializabl */ @DefaultDataContext public FieldGNSSDate(final int weekNumber, final T milliInWeek, - final SatelliteSystem system) { - this(weekNumber, milliInWeek, system, DataContext.getDefault().getTimeScales()); + final SatelliteSystem system) { + this(weekNumber, milliInWeek, system, + DataContext.getDefault().getTimeScales()); } /** * Build an instance corresponding to a GNSS date. *

- * GNSS dates are provided as a week number starting at the GNSS reference epoch and - * as a number of milliseconds since week start. + * GNSS dates are provided as a week number starting at the GNSS reference + * epoch and as a number of milliseconds since week start. *

*

- * Many interfaces provide week number modulo the constellation week cycle. In order - * to cope with this, when the week number is smaller than the week cycle, this - * constructor assumes a modulo operation has been performed and it will fix the week - * number according to the reference date set up for handling rollover (see {@link - * #setRolloverReference(DateComponents) setRolloverReference(reference)}). If the - * week number is equal to the week cycle or larger, it will be used without any - * correction. + * Many interfaces provide week number modulo the constellation week cycle. + * In order to cope with this, when the week number is smaller than the week + * cycle, this constructor assumes a modulo operation has been performed and + * it will fix the week number according to the reference date set up for + * handling rollover (see {@link #setRolloverReference(DateComponents) + * setRolloverReference(reference)}). If the week number is equal to the + * week cycle or larger, it will be used without any correction. *

* - * @param weekNumber week number + * @param weekNumber week number * @param milliInWeek number of milliseconds since week start - * @param system satellite system to consider - * @param timeScales the set of time scales. Used to retrieve the appropriate time - * scale for the given {@code system}. + * @param system satellite system to consider + * @param timeScales the set of time scales. Used to retrieve the + * appropriate time scale for the given {@code system}. * @since 10.1 */ - public FieldGNSSDate(final int weekNumber, - final T milliInWeek, - final SatelliteSystem system, - final TimeScales timeScales) { + public FieldGNSSDate(final int weekNumber, final T milliInWeek, + final SatelliteSystem system, + final TimeScales timeScales) { - final int day = (int) FastMath.floor(milliInWeek.getReal() / (Constants.JULIAN_DAY * S_TO_MS)); - final double secondsInDay = milliInWeek.getReal() / S_TO_MS - day * Constants.JULIAN_DAY; + final int day = + (int) FastMath.floor(milliInWeek.getReal() / + (Constants.JULIAN_DAY * S_TO_MS)); + final double secondsInDay = + milliInWeek.getReal() / S_TO_MS - day * Constants.JULIAN_DAY; int w = weekNumber; - DateComponents dc = new DateComponents(getWeekReferenceDateComponents(system), weekNumber * 7 + day); + DateComponents dc = + new DateComponents(getWeekReferenceDateComponents(system), + weekNumber * 7 + day); final int cycleW = GNSSDateType.getRollOverWeek(system); if (weekNumber < cycleW) { DateComponents reference = rolloverReference.get(); if (reference == null) { // lazy setting of a default reference, using end of EOP entries - final UT1Scale ut1 = timeScales.getUT1(IERSConventions.IERS_2010, true); - final List eop = ut1.getEOPHistory().getEntries(); - final int lastMJD = eop.get(eop.size() - 1).getMjd(); - reference = new DateComponents(DateComponents.MODIFIED_JULIAN_EPOCH, lastMJD); + final UT1Scale ut1 = + timeScales.getUT1(IERSConventions.IERS_2010, true); + final List eop = ut1.getEOPHistory().getEntries(); + final int lastMJD = eop.get(eop.size() - 1).getMjd(); + reference = + new DateComponents(DateComponents.MODIFIED_JULIAN_EPOCH, + lastMJD); rolloverReference.compareAndSet(null, reference); } @@ -163,61 +184,75 @@ public class FieldGNSSDate> implements Serializabl } - this.weekNumber = w; + this.weekNumber = w; this.milliInWeek = milliInWeek; - this.system = system; + this.system = system; - date = new FieldAbsoluteDate(milliInWeek.getField(), dc, new TimeComponents(secondsInDay), getTimeScale(system, timeScales)); + date = + new FieldAbsoluteDate(milliInWeek.getField(), dc, + new TimeComponents(secondsInDay), + getTimeScale(system, timeScales)); } - /** Build an instance from an absolute date. - * - *

This method uses the {@link DataContext#getDefault() default data context}. + /** + * Build an instance from an absolute date. + *

+ * This method uses the {@link DataContext#getDefault() default data + * context}. * * @param date absolute date to consider * @param system satellite system to consider * @see #GNSSDate(AbsoluteDate, SatelliteSystem, TimeScales) */ @DefaultDataContext - public FieldGNSSDate(final FieldAbsoluteDate date, final SatelliteSystem system) { + public FieldGNSSDate(final FieldAbsoluteDate date, + final SatelliteSystem system) { this(date, system, DataContext.getDefault().getTimeScales()); } /** * Build an instance from an absolute date. * - * @param date absolute date to consider - * @param system satellite system to consider - * @param timeScales the set of time scales. Used to retrieve the appropriate time - * scale for the given {@code system}. + * @param date absolute date to consider + * @param system satellite system to consider + * @param timeScales the set of time scales. Used to retrieve the + * appropriate time scale for the given {@code system}. * @since 10.1 */ public FieldGNSSDate(final FieldAbsoluteDate date, - final SatelliteSystem system, - final TimeScales timeScales) { + final SatelliteSystem system, + final TimeScales timeScales) { this.system = system; - final AbsoluteDate epoch = getWeekReferenceAbsoluteDate(system, timeScales); - this.weekNumber = (int) FastMath.floor(date.durationFrom(epoch).divide(WEEK_S).getReal()); - final AbsoluteDate weekStart = new AbsoluteDate(epoch, WEEK_S * weekNumber); + final AbsoluteDate epoch = + getWeekReferenceAbsoluteDate(system, timeScales); + this.weekNumber = + (int) FastMath + .floor(date.durationFrom(epoch).divide(WEEK_S).getReal()); + final AbsoluteDate weekStart = + new AbsoluteDate(epoch, WEEK_S * weekNumber); this.milliInWeek = date.durationFrom(weekStart).multiply(S_TO_MS); - this.date = date; + this.date = date; } - /** Set a reference date for ensuring continuity across GNSS week rollover. + /** + * Set a reference date for ensuring continuity across GNSS week rollover. *

- * Instance created using the {@link #GNSSDate(int, double, SatelliteSystem) GNSSDate(weekNumber, milliInWeek, system)} - * constructor and with a week number between 0 and the constellation week cycle (cycleW) after this method has been called will - * fix the week number to ensure they correspond to dates between {@code reference - cycleW / 2 weeks} - * and {@code reference + cycleW / 2 weeks}. + * Instance created using the {@link #GNSSDate(int, double, SatelliteSystem) + * GNSSDate(weekNumber, milliInWeek, system)} constructor and with a week + * number between 0 and the constellation week cycle (cycleW) after this + * method has been called will fix the week number to ensure they correspond + * to dates between {@code reference - cycleW / 2 weeks} and + * {@code reference + cycleW / 2 weeks}. *

*

- * If this method is never called, a default reference date for rollover will be set using - * the date of the last known EOP entry retrieved from {@link UT1Scale#getEOPHistory() UT1} - * time scale. + * If this method is never called, a default reference date for rollover + * will be set using the date of the last known EOP entry retrieved from + * {@link UT1Scale#getEOPHistory() UT1} time scale. *

+ * * @param reference reference date for GNSS week rollover * @see #getRolloverReference() * @see #GNSSDate(int, double, SatelliteSystem) @@ -227,7 +262,9 @@ public class FieldGNSSDate> implements Serializabl rolloverReference.set(reference); } - /** Get the reference date ensuring continuity across GNSS week rollover. + /** + * Get the reference date ensuring continuity across GNSS week rollover. + * * @return reference reference date for GNSS week rollover * @see #setRolloverReference(DateComponents) * @see #GNSSDate(int, double, SatelliteSystem) @@ -237,18 +274,22 @@ public class FieldGNSSDate> implements Serializabl return rolloverReference.get(); } - /** Get the week number since the GNSS reference epoch. + /** + * Get the week number since the GNSS reference epoch. *

* The week number returned here has been fixed for GNSS week rollover, i.e. * it may be larger than the corresponding week cycle of the constellation. *

+ * * @return week number since since the GNSS reference epoch */ public int getWeekNumber() { return weekNumber; } - /** Get the number of milliseconds since week start. + /** + * Get the number of milliseconds since week start. + * * @return number of milliseconds since week start */ public T getMilliInWeek() { @@ -261,7 +302,9 @@ public class FieldGNSSDate> implements Serializabl return date; } - /** Get the time scale related to the given satellite system. + /** + * Get the time scale related to the given satellite system. + * * @param satellite satellite system * @param timeScales set of time scales. * @return the time scale @@ -269,53 +312,91 @@ public class FieldGNSSDate> implements Serializabl private TimeScale getTimeScale(final SatelliteSystem satellite, final TimeScales timeScales) { switch (satellite) { - case GPS : return timeScales.getGPS(); - case GALILEO : return timeScales.getGST(); - case QZSS : return timeScales.getQZSS(); - case BEIDOU : return timeScales.getBDT(); - case IRNSS : return timeScales.getIRNSS(); - case SBAS : return timeScales.getGPS(); - default : throw new OrekitException(OrekitMessages.INVALID_SATELLITE_SYSTEM, satellite); + case GPS: + return timeScales.getGPS(); + case GALILEO: + return timeScales.getGST(); + case QZSS: + return timeScales.getQZSS(); + case BEIDOU: + return timeScales.getBDT(); + case IRNSS: + return timeScales.getIRNSS(); + case SBAS: + return timeScales.getGPS(); + default: + throw new OrekitException(OrekitMessages.INVALID_SATELLITE_SYSTEM, + satellite); } } - /** Get the reference epoch of the week number for the given satellite system. - *

Returned parameter is an AbsoluteDate.

+ /** + * Get the reference epoch of the week number for the given satellite + * system. + *

+ * Returned parameter is an AbsoluteDate. + *

+ * * @param satellite satellite system * @param timeScales set of time scales. * @return the reference epoch */ - private AbsoluteDate getWeekReferenceAbsoluteDate(final SatelliteSystem satellite, - final TimeScales timeScales) { + private AbsoluteDate + getWeekReferenceAbsoluteDate(final SatelliteSystem satellite, + final TimeScales timeScales) { switch (satellite) { - case GPS : return timeScales.getGpsEpoch(); - case GALILEO : return timeScales.getGalileoEpoch(); - case QZSS : return timeScales.getQzssEpoch(); - case BEIDOU : return timeScales.getBeidouEpoch(); - case IRNSS : return timeScales.getIrnssEpoch(); - case SBAS : return timeScales.getGpsEpoch(); - default : throw new OrekitException(OrekitMessages.INVALID_SATELLITE_SYSTEM, satellite); + case GPS: + return timeScales.getGpsEpoch(); + case GALILEO: + return timeScales.getGalileoEpoch(); + case QZSS: + return timeScales.getQzssEpoch(); + case BEIDOU: + return timeScales.getBeidouEpoch(); + case IRNSS: + return timeScales.getIrnssEpoch(); + case SBAS: + return timeScales.getGpsEpoch(); + default: + throw new OrekitException(OrekitMessages.INVALID_SATELLITE_SYSTEM, + satellite); } } - /** Get the reference epoch of the week number for the given satellite system. - *

Returned parameter is a DateComponents.

+ /** + * Get the reference epoch of the week number for the given satellite + * system. + *

+ * Returned parameter is a DateComponents. + *

+ * * @param satellite satellite system * @return the reference epoch */ - private DateComponents getWeekReferenceDateComponents(final SatelliteSystem satellite) { + private DateComponents + getWeekReferenceDateComponents(final SatelliteSystem satellite) { switch (satellite) { - case GPS : return DateComponents.GPS_EPOCH; - case GALILEO : return DateComponents.GALILEO_EPOCH; - case QZSS : return DateComponents.QZSS_EPOCH; - case BEIDOU : return DateComponents.BEIDOU_EPOCH; - case IRNSS : return DateComponents.IRNSS_EPOCH; - case SBAS : return DateComponents.GPS_EPOCH; - default : throw new OrekitException(OrekitMessages.INVALID_SATELLITE_SYSTEM, satellite); + case GPS: + return DateComponents.GPS_EPOCH; + case GALILEO: + return DateComponents.GALILEO_EPOCH; + case QZSS: + return DateComponents.QZSS_EPOCH; + case BEIDOU: + return DateComponents.BEIDOU_EPOCH; + case IRNSS: + return DateComponents.IRNSS_EPOCH; + case SBAS: + return DateComponents.GPS_EPOCH; + default: + throw new OrekitException(OrekitMessages.INVALID_SATELLITE_SYSTEM, + satellite); } } - /** Replace the instance with a data transfer object for serialization. + /** + * Replace the instance with a data transfer object for serialization. + * * @return data transfer object that will be serialized */ @DefaultDataContext @@ -325,7 +406,9 @@ public class FieldGNSSDate> implements Serializabl /** Internal class used only for serialization. */ @DefaultDataContext - private class DataTransferObject implements Serializable { + private class DataTransferObject + implements + Serializable { /** Serializable UID. */ private static final long serialVersionUID = 201902141L; @@ -339,19 +422,24 @@ public class FieldGNSSDate> implements Serializabl /** Satellite system to consider. */ private final SatelliteSystem system; - /** Simple constructor. + /** + * Simple constructor. + * * @param weekNumber week number since the GNSS reference epoch * @param milliInWeek number of milliseconds since week start * @param system satellite system to consider */ DataTransferObject(final int weekNumber, final T milliInWeek, final SatelliteSystem system) { - this.weekNumber = weekNumber; + this.weekNumber = weekNumber; this.milliInWeek = milliInWeek; - this.system = system; + this.system = system; } - /** Replace the deserialized data transfer object with a {@link GNSSDate}. + /** + * Replace the deserialized data transfer object with a + * {@link GNSSDate}. + * * @return replacement {@link GNSSDate} */ private Object readResolve() { @@ -363,29 +451,30 @@ public class FieldGNSSDate> implements Serializabl /** Enumerate for GNSS data. */ private enum GNSSDateType { - /** GPS. */ - GPS(SatelliteSystem.GPS, 1024), + /** GPS. */ + GPS(SatelliteSystem.GPS, 1024), - /** Galileo. */ - GALILEO(SatelliteSystem.GALILEO, 4096), + /** Galileo. */ + GALILEO(SatelliteSystem.GALILEO, 4096), - /** QZSS. */ - QZSS(SatelliteSystem.QZSS, 1024), + /** QZSS. */ + QZSS(SatelliteSystem.QZSS, 1024), - /** BeiDou. */ - BEIDOU(SatelliteSystem.BEIDOU, 8192), + /** BeiDou. */ + BEIDOU(SatelliteSystem.BEIDOU, 8192), - /** IRNSS. */ - IRNSS(SatelliteSystem.IRNSS, 1024), + /** IRNSS. */ + IRNSS(SatelliteSystem.IRNSS, 1024), - /** SBAS. */ - SBAS(SatelliteSystem.SBAS, 1024); + /** SBAS. */ + SBAS(SatelliteSystem.SBAS, 1024); /** Map for the number of week in one GNSS rollover cycle. */ - private static final Map CYCLE_MAP = new HashMap(); + private static final Map CYCLE_MAP = + new HashMap(); static { for (final GNSSDateType type : values()) { - final int val = type.getRollOverCycle(); + final int val = type.getRollOverCycle(); final SatelliteSystem satellite = type.getSatelliteSystem(); CYCLE_MAP.put(satellite, val); } @@ -405,31 +494,38 @@ public class FieldGNSSDate> implements Serializabl */ GNSSDateType(final SatelliteSystem system, final int rollover) { this.satelliteSystem = system; - this.numberOfWeek = rollover; + this.numberOfWeek = rollover; } - /** Get the number of week in one rollover cycle. - * @return the number of week in one rollover cycle + /** + * Get the number of week in one rollover cycle. + * + * @return the number of week in one rollover cycle */ private int getRollOverCycle() { return numberOfWeek; } - /** Get the satellite system. + /** + * Get the satellite system. + * * @return the satellite system */ private SatelliteSystem getSatelliteSystem() { return satelliteSystem; } - /** Get the number of week in one rollover cycle for the given satellite system. + /** + * Get the number of week in one rollover cycle for the given satellite + * system. * * @param satellite satellite system - * @return the number of week in one rollover cycle for the given satellite system + * @return the number of week in one rollover cycle for the given + * satellite system */ private static int getRollOverWeek(final SatelliteSystem satellite) { return CYCLE_MAP.get(satellite); } - + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java index db58f51e9..413f897aa 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGNSSOrbitalElements.java @@ -19,16 +19,18 @@ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; import org.orekit.time.FieldTimeStamped; -/** This interface provides the minimal set of Field orbital elements needed by the {@link FieldAbstractGNSSPropagator}. -* -* @author Pascal Parraud -* @author Nicolas Fialton (field translation) -* -*/ -public interface FieldGNSSOrbitalElements> extends FieldTimeStamped { - - - /** +/** + * This interface provides the minimal set of Field orbital elements needed by + * the {@link FieldAbstractGNSSPropagator}. + * + * @author Pascal Parraud + * @author Nicolas Fialton (field translation) + */ +public interface FieldGNSSOrbitalElements> + extends + FieldTimeStamped { + + /** * Gets the PRN number of the GNSS satellite. * * @return the PRN number of the GNSS satellite @@ -87,7 +89,8 @@ public interface FieldGNSSOrbitalElements> extends /** * Gets the Longitude of Ascending Node of Orbit Plane at Weekly Epoch. * - * @return the Longitude of Ascending Node of Orbit Plane at Weekly Epoch (rad) + * @return the Longitude of Ascending Node of Orbit Plane at Weekly Epoch + * (rad) */ T getOmega0(); @@ -113,44 +116,56 @@ public interface FieldGNSSOrbitalElements> extends T getM0(); /** - * Gets the Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude. + * Gets the Amplitude of the Cosine Harmonic Correction Term to the Argument + * of Latitude. * - * @return the Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude (rad) + * @return the Amplitude of the Cosine Harmonic Correction Term to the + * Argument of Latitude (rad) */ T getCuc(); /** - * Gets the Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude. + * Gets the Amplitude of the Sine Harmonic Correction Term to the Argument + * of Latitude. * - * @return the Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude (rad) + * @return the Amplitude of the Sine Harmonic Correction Term to the + * Argument of Latitude (rad) */ T getCus(); /** - * Gets the Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius. + * Gets the Amplitude of the Cosine Harmonic Correction Term to the Orbit + * Radius. * - * @return the Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius (m) + * @return the Amplitude of the Cosine Harmonic Correction Term to the Orbit + * Radius (m) */ T getCrc(); /** - * Gets the Amplitude of the Sine Harmonic Correction Term to the Orbit Radius. + * Gets the Amplitude of the Sine Harmonic Correction Term to the Orbit + * Radius. * - * @return the Amplitude of the Sine Harmonic Correction Term to the Orbit Radius (m) + * @return the Amplitude of the Sine Harmonic Correction Term to the Orbit + * Radius (m) */ T getCrs(); /** - * Gets the Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination. + * Gets the Amplitude of the Cosine Harmonic Correction Term to the Angle of + * Inclination. * - * @return the Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination (rad) + * @return the Amplitude of the Cosine Harmonic Correction Term to the Angle + * of Inclination (rad) */ T getCic(); /** - * Gets the Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination. + * Gets the Amplitude of the Sine Harmonic Correction Term to the Angle of + * Inclination. * - * @return the Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination (rad) + * @return the Amplitude of the Sine Harmonic Correction Term to the Angle + * of Inclination (rad) */ T getCis(); @@ -174,6 +189,7 @@ public interface FieldGNSSOrbitalElements> extends * @since 9.3 */ T getAf1(); + /** * Gets the Second Order Clock Correction. * diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java index 55c959306..39262e843 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSAlmanac.java @@ -27,7 +27,6 @@ import org.orekit.time.FieldAbsoluteDate; /** * This class holds a Field GPS almanac as read from SEM or YUMA files. - * *

* Depending on the source (SEM or YUMA), some fields may be filled in or not. * An almanac read from a YUMA file doesn't hold SVN number, average URA and @@ -36,347 +35,369 @@ import org.orekit.time.FieldAbsoluteDate; * * @author Pascal Parraud * @author Nicolas Fialton (field translation) - * */ -public class FieldGPSAlmanac> implements FieldGPSOrbitalElements { - - private final T zero; - // Fields - /** Source of the almanac. */ - private final String src; - /** PRN number. */ - private final int prn; - /** SVN number. */ - private final int svn; - /** Health status. */ - private final int health; - /** Average URA. */ - private final int ura; - /** Satellite configuration. */ - private final int config; - /** GPS week. */ - private final int week; - /** Time of applicability. */ - private final T toa; - /** Semi-major axis. */ - private final T sma; - /** Eccentricity. */ - private final T ecc; - /** Inclination. */ - private final T inc; - /** Longitude of Orbital Plane. */ - private final T om0; - /** Rate of Right Ascension. */ - private final T dom; - /** Argument of perigee. */ - private final T aop; - /** Mean anomaly. */ - private final T anom; - /** Zeroth order clock correction. */ - private final T af0; - /** First order clock correction. */ - private final T af1; - /** Date of validity. */ - private final FieldAbsoluteDate date; - - /** - * Constructor. - * - *

- * This method uses the {@link DataContext#getDefault() default data context}. - * - * @param source the source of the almanac (SEM, YUMA, user defined) - * @param prn the PRN number - * @param svn the SVN number - * @param week the GPS week - * @param toa the Time of Applicability - * @param sqa the Square Root of Semi-Major Axis (m^1/2) - * @param ecc the eccentricity - * @param inc the inclination (rad) - * @param om0 the geographic longitude of the orbital plane at the weekly - * epoch (rad) - * @param dom the Rate of Right Ascension (rad/s) - * @param aop the Argument of Perigee (rad) - * @param anom the Mean Anomaly (rad) - * @param af0 the Zeroth Order Clock Correction (s) - * @param af1 the First Order Clock Correction (s/s) - * @param health the Health status - * @param ura the average URA - * @param config the satellite configuration - * @see #FieldGPSAlmanac(String, int, int, int, T, T, T, T, T, T, T, T, T, T, - * int, int, int, FieldAbsoluteDate) - */ - @DefaultDataContext - public FieldGPSAlmanac(final String source, final int prn, final int svn, final int week, final T toa, final T sqa, - final T ecc, final T inc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, - final int health, final int ura, final int config) { - this(source, prn, svn, week, toa, sqa, ecc, inc, om0, dom, aop, anom, af0, af1, health, ura, config, - new FieldGNSSDate<>(week, toa.multiply(1000), SatelliteSystem.GPS, - DataContext.getDefault().getTimeScales()).getDate()); - } - - /** - * Constructor. - * - * @param source the source of the almanac (SEM, YUMA, user defined) - * @param prn the PRN number - * @param svn the SVN number - * @param week the GPS week - * @param toa the Time of Applicability - * @param sqa the Square Root of Semi-Major Axis (m^1/2) - * @param ecc the eccentricity - * @param inc the inclination (rad) - * @param om0 the geographic longitude of the orbital plane at the weekly - * epoch (rad) - * @param dom the Rate of Right Ascension (rad/s) - * @param aop the Argument of Perigee (rad) - * @param anom the Mean Anomaly (rad) - * @param af0 the Zeroth Order Clock Correction (s) - * @param af1 the First Order Clock Correction (s/s) - * @param health the Health status - * @param ura the average URA - * @param config the satellite configuration - * @param date built from the {@code week} and {@code toa}: - * {@code new GNSSDate(week, - * toa * 1000., SatelliteSystem.GPS, timeScales).getDate()} - * @since 10.1 - */ - public FieldGPSAlmanac(final String source, final int prn, final int svn, final int week, final T toa, final T sqa, - final T ecc, final T inc, final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, - final int health, final int ura, final int config, final FieldAbsoluteDate date) { - this.zero = toa.getField().getZero(); - this.src = source; - this.prn = prn; - this.svn = svn; - this.week = week; - this.toa = toa; - this.sma = sqa.multiply(sqa); - this.ecc = ecc; - this.inc = inc; - this.om0 = om0; - this.dom = dom; - this.aop = aop; - this.anom = anom; - this.af0 = af0; - this.af1 = af1; - this.health = health; - this.ura = ura; - this.config = config; - this.date = date; - } - - /** - * Constructor - * - * This constructor converts a GPSAlmanac into a FieldGPSAlmanac - * - * @param field - * @param almanac a GPSAlmanac - */ - public FieldGPSAlmanac(Field field, GPSAlmanac almanac) { - this.zero = field.getZero(); - this.src = almanac.getSource(); - this.prn = almanac.getPRN(); - this.svn = almanac.getSVN(); - this.week = almanac.getWeek(); - this.toa = zero.add(almanac.getTime()); - this.sma = zero.add(almanac.getSma()); - this.ecc = zero.add(almanac.getE()); - this.inc = zero.add(almanac.getI0()); - this.om0 = zero.add(almanac.getOmega0()); - this.dom = zero.add(almanac.getOmegaDot()); - this.aop = zero.add(almanac.getPa()); - this.anom = zero.add(almanac.getM0()); - this.af0 = zero.add(almanac.getAf0()); - this.af1 = zero.add(almanac.getAf1()); - this.health = almanac.getHealth(); - this.ura = almanac.getURA(); - this.config = almanac.getSatConfiguration(); - this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); - } - - @Override - public FieldAbsoluteDate getDate() { - return date; - } - - /** - * Gets the source of this GPS almanac. - *

- * Sources can be SEM or YUMA, when the almanac is read from a file. - *

- * - * @return the source of this GPS almanac - */ - public String getSource() { - return src; - } - - @Override - public int getPRN() { - return prn; - } - - /** - * Gets the satellite "SVN" reference number. - * - * @return the satellite "SVN" reference number - */ - public int getSVN() { - return svn; - } - - @Override - public int getWeek() { - return week; - } - - @Override - public T getTime() { - return toa; - } - - @Override - public T getSma() { - return sma; - } - - @Override - public T getMeanMotion() { - final T absA = FastMath.abs(sma); - return FastMath.sqrt(absA.getField().getZero().add(GPS_MU).divide(absA)).divide(absA); - } - - @Override - public T getE() { - return ecc; - } - - @Override - public T getI0() { - return inc; - } - - @Override - public T getIDot() { - return zero; - } - - @Override - public T getOmega0() { - return om0; - } - - @Override - public T getOmegaDot() { - return dom; - } - - @Override - public T getPa() { - return aop; - } - - @Override - public T getM0() { - return anom; - } - - @Override - public T getCuc() { - return zero; - } - - @Override - public T getCus() { - return zero; - } - - @Override - public T getCrc() { - return zero; - } - - @Override - public T getCrs() { - return zero; - } - - @Override - public T getCic() { - return zero; - } - - @Override - public T getCis() { - return zero; - } - - @Override - public T getAf0() { - return af0; - } - - @Override - public T getAf1() { - return af1; - } - - /** - * Gets the Health status. - * - * @return the Health status - */ - public int getHealth() { - return health; - } - - /** - * Gets the average URA number. - * - * @return the average URA number - */ - public int getURA() { - return ura; - } - - /** - * Gets the satellite configuration. - * - * @return the satellite configuration - */ - public int getSatConfiguration() { - return config; - } - - @Override - public T getAf2() { - // TODO Auto-generated method stub - return null; - } - - @Override - public T getToc() { - // TODO Auto-generated method stub - return null; - } - - @Override - public int getIODC() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public int getIODE() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public T getTGD() { - // TODO Auto-generated method stub - return null; - } +public class FieldGPSAlmanac> + implements + FieldGPSOrbitalElements { + + // Fields + + /** Field Zero. */ + private final T zero; + + /** Source of the almanac. */ + private final String src; + + /** PRN number. */ + private final int prn; + + /** SVN number. */ + private final int svn; + + /** Health status. */ + private final int health; + + /** Average URA. */ + private final int ura; + + /** Satellite configuration. */ + private final int config; + + /** GPS week. */ + private final int week; + + /** Time of applicability. */ + private final T toa; + + /** Semi-major axis. */ + private final T sma; + + /** Eccentricity. */ + private final T ecc; + + /** Inclination. */ + private final T inc; + + /** Longitude of Orbital Plane. */ + private final T om0; + + /** Rate of Right Ascension. */ + private final T dom; + + /** Argument of perigee. */ + private final T aop; + + /** Mean anomaly. */ + private final T anom; + + /** Zeroth order clock correction. */ + private final T af0; + + /** First order clock correction. */ + private final T af1; + + /** Date of validity. */ + private final FieldAbsoluteDate date; + + /** + * Constructor. + *

+ * This method uses the {@link DataContext#getDefault() default data + * context}. + * + * @param source the source of the almanac (SEM, YUMA, user defined) + * @param prn the PRN number + * @param svn the SVN number + * @param week the GPS week + * @param toa the Time of Applicability + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc the inclination (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @param ura the average URA + * @param config the satellite configuration + * @see #FieldGPSAlmanac(String, int, int, int, T, T, T, T, T, T, T, T, T, + * T, int, int, int, FieldAbsoluteDate) + */ + @DefaultDataContext + public FieldGPSAlmanac(final String source, final int prn, final int svn, + final int week, final T toa, final T sqa, + final T ecc, final T inc, final T om0, final T dom, + final T aop, final T anom, final T af0, final T af1, + final int health, final int ura, final int config) { + this(source, prn, svn, week, toa, sqa, ecc, inc, om0, dom, aop, anom, + af0, af1, health, ura, config, + new FieldGNSSDate<>(week, toa.multiply(1000), SatelliteSystem.GPS, + DataContext.getDefault().getTimeScales()) + .getDate()); + } + + /** + * Constructor. + * + * @param source the source of the almanac (SEM, YUMA, user defined) + * @param prn the PRN number + * @param svn the SVN number + * @param week the GPS week + * @param toa the Time of Applicability + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc the inclination (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param health the Health status + * @param ura the average URA + * @param config the satellite configuration + * @param date built from the {@code week} and {@code toa}: + * {@code new GNSSDate(week, + * toa * 1000., SatelliteSystem.GPS, timeScales).getDate()} + * @since 10.1 + */ + public FieldGPSAlmanac(final String source, final int prn, final int svn, + final int week, final T toa, final T sqa, + final T ecc, final T inc, final T om0, final T dom, + final T aop, final T anom, final T af0, final T af1, + final int health, final int ura, final int config, + final FieldAbsoluteDate date) { + this.zero = toa.getField().getZero(); + this.src = source; + this.prn = prn; + this.svn = svn; + this.week = week; + this.toa = toa; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.inc = inc; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.af0 = af0; + this.af1 = af1; + this.health = health; + this.ura = ura; + this.config = config; + this.date = date; + } + + /** + * Constructor This constructor converts a GPSAlmanac into a FieldGPSAlmanac. + * + * @param field + * @param almanac a GPSAlmanac + */ + public FieldGPSAlmanac(final Field field, final GPSAlmanac almanac) { + this.zero = field.getZero(); + this.src = almanac.getSource(); + this.prn = almanac.getPRN(); + this.svn = almanac.getSVN(); + this.week = almanac.getWeek(); + this.toa = zero.add(almanac.getTime()); + this.sma = zero.add(almanac.getSma()); + this.ecc = zero.add(almanac.getE()); + this.inc = zero.add(almanac.getI0()); + this.om0 = zero.add(almanac.getOmega0()); + this.dom = zero.add(almanac.getOmegaDot()); + this.aop = zero.add(almanac.getPa()); + this.anom = zero.add(almanac.getM0()); + this.af0 = zero.add(almanac.getAf0()); + this.af1 = zero.add(almanac.getAf1()); + this.health = almanac.getHealth(); + this.ura = almanac.getURA(); + this.config = almanac.getSatConfiguration(); + this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + /** + * Gets the source of this GPS almanac. + *

+ * Sources can be SEM or YUMA, when the almanac is read from a file. + *

+ * + * @return the source of this GPS almanac + */ + public String getSource() { + return src; + } + + @Override + public int getPRN() { + return prn; + } + + /** + * Gets the satellite "SVN" reference number. + * + * @return the satellite "SVN" reference number + */ + public int getSVN() { + return svn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toa; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(absA.getField().getZero().add(GPS_MU).divide(absA)) + .divide(absA); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return zero; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return zero; + } + + @Override + public T getCus() { + return zero; + } + + @Override + public T getCrc() { + return zero; + } + + @Override + public T getCrs() { + return zero; + } + + @Override + public T getCic() { + return zero; + } + + @Override + public T getCis() { + return zero; + } + + @Override + public T getAf0() { + return af0; + } + + @Override + public T getAf1() { + return af1; + } + + /** + * Gets the Health status. + * + * @return the Health status + */ + public int getHealth() { + return health; + } + + /** + * Gets the average URA number. + * + * @return the average URA number + */ + public int getURA() { + return ura; + } + + /** + * Gets the satellite configuration. + * + * @return the satellite configuration + */ + public int getSatConfiguration() { + return config; + } + + @Override + public T getAf2() { + return zero; + } + + @Override + public T getToc() { + return zero; + } + + @Override + public int getIODC() { + return 0; + } + + @Override + public int getIODE() { + return 0; + } + + @Override + public T getTGD() { + return zero; + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java index 446f34c13..c68d0febf 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSOrbitalElements.java @@ -18,17 +18,23 @@ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; -/** This interface provides the minimal set of Field orbital elements needed by the {@link FieldGPSPropagator}. -* -* @see GPS Interface Specification -* @author Pascal Parraud -* @author Nicolas Fialton (field translation) -*/ +/** + * This interface provides the minimal set of Field orbital elements needed by + * the {@link FieldGPSPropagator}. + * + * @see GPS + * Interface Specification + * @author Pascal Parraud + * @author Nicolas Fialton (field translation) + */ public interface FieldGPSOrbitalElements> extends FieldGNSSOrbitalElements { - - // Constants - /** WGS 84 value of the Earth's universal gravitational parameter for GPS user in m³/s². */ + + // Constants + /** + * WGS 84 value of the Earth's universal gravitational parameter for GPS + * user in m³/s². + */ double GPS_MU = 3.986005e+14; /** Value of Pi for conversion from semicircles to radian. */ @@ -59,9 +65,10 @@ public interface FieldGPSOrbitalElements> extends /** * Gets the estimated group delay differential TGD for L1-L2 correction. * - * @return the estimated group delay differential TGD for L1-L2 correction (s) + * @return the estimated group delay differential TGD for L1-L2 correction + * (s) * @since 9.3 */ T getTGD(); - + } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java index 4e55a5cd9..57c3c5671 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGPSPropagator.java @@ -31,7 +31,8 @@ import org.orekit.utils.IERSConventions; import org.orekit.utils.ParameterDriver; /** - * This class aims at propagating a GPS orbit from {@link FieldGPSOrbitalElements}. + * This class aims at propagating a GPS orbit from + * {@link FieldGPSOrbitalElements}. * * @see GPS * Interface Specification @@ -40,132 +41,155 @@ import org.orekit.utils.ParameterDriver; */ public class FieldGPSPropagator> extends FieldAbstractGNSSPropagator { - // Constants - /** WGS 84 value of the earth's rotation rate in rad/s. */ - private static final double GPS_AV = 7.2921151467e-5; + // Constants + /** WGS 84 value of the earth's rotation rate in rad/s. */ + private static final double GPS_AV = 7.2921151467e-5; - /** Duration of the GPS cycle in seconds. */ - private static final double GPS_CYCLE_DURATION = FieldGPSOrbitalElements.GPS_WEEK_IN_SECONDS - * FieldGPSOrbitalElements.GPS_WEEK_NB; + /** Duration of the GPS cycle in seconds. */ + private static final double GPS_CYCLE_DURATION = + FieldGPSOrbitalElements.GPS_WEEK_IN_SECONDS * + FieldGPSOrbitalElements.GPS_WEEK_NB; - /** The GPS orbital elements used. */ - private final FieldGPSOrbitalElements gpsOrbit; + /** The GPS orbital elements used. */ + private final FieldGPSOrbitalElements gpsOrbit; - /** - * Default constructor. - * - *

The Field GPS orbital elements is the only requested parameter to build a FieldGPSPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, final Frames frames)}

- * - * @param field - * @param gpsOrbit the Field GPS orbital elements to be used by the GPSpropagator. - * @see #attitudeProvider(AttitudeProvider provider) - * @see #mass(double mass) - * @see #eci(Frame inertial) - * @see #ecef(Frame bodyFixed) - */ - @DefaultDataContext - public FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit) { - this(field, gpsOrbit, DataContext.getDefault().getFrames()); - } + /** + * Default constructor. + *

+ * The Field GPS orbital elements is the only requested parameter to build a + * FieldGPSPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. Another data context can be set using + * {@code FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, final Frames frames)} + *

+ * + * @param field + * @param gpsOrbit the Field GPS orbital elements to be used by the + * GPSpropagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) + */ + @DefaultDataContext + public FieldGPSPropagator(final Field field, + final FieldGPSOrbitalElements gpsOrbit) { + this(field, gpsOrbit, DataContext.getDefault().getFrames()); + } - /** - * Constructor. - * - *

The Field GPS orbital elements is the only requested parameter to build a FieldGPSPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, - final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)}

- * - * @param field - * @param gpsOrbit the Field GPS orbital elements to be used by the GPSpropagator. - * @param frames set of frames to use. - * @see #attitudeProvider(AttitudeProvider provider) - * @see #mass(double mass) - * @see #eci(Frame inertial) - * @see #ecef(Frame bodyFixed) - */ - public FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, final Frames frames) { - this(field, gpsOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), - frames.getITRF(IERSConventions.IERS_2010, true)); - } + /** + * Constructor. + *

+ * The Field GPS orbital elements is the only requested parameter to build a + * FieldGPSPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. Another data context can be set using + * {@code FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)} + *

+ * + * @param field + * @param gpsOrbit the Field GPS orbital elements to be used by the + * GPSpropagator. + * @param frames set of frames to use. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) + */ + public FieldGPSPropagator(final Field field, + final FieldGPSOrbitalElements gpsOrbit, + final Frames frames) { + this(field, gpsOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, + frames.getEME2000(), + frames.getITRF(IERSConventions.IERS_2010, true)); + } - /** - * Constructor. - * - *

The Field GPS orbital elements is the only requested parameter to build a FieldGPSPropagator.

- *

The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data - * context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - * @param field - * @param gpsOrbit the Field GPS orbital elements to be used by the GPSpropagator. - * @param attitudeProvider - * @param mass - * @param eci - * @param ecef - */ - public FieldGPSPropagator(final Field field, final FieldGPSOrbitalElements gpsOrbit, - final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { - super(field, gpsOrbit, attitudeProvider, eci, ecef, mass, GPS_AV, GPS_CYCLE_DURATION, - FieldGPSOrbitalElements.GPS_MU); - // Stores the GPS orbital elements - this.gpsOrbit = gpsOrbit; - } + /** + * Constructor. + *

+ * The Field GPS orbital elements is the only requested parameter to build a + * FieldGPSPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + * @param field + * @param gpsOrbit the Field GPS orbital elements to be used by the + * GPSpropagator. + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldGPSPropagator(final Field field, + final FieldGPSOrbitalElements gpsOrbit, + final AttitudeProvider attitudeProvider, + final double mass, final Frame eci, + final Frame ecef) { + super(field, gpsOrbit, attitudeProvider, eci, ecef, mass, GPS_AV, + GPS_CYCLE_DURATION, FieldGPSOrbitalElements.GPS_MU); + // Stores the GPS orbital elements + this.gpsOrbit = gpsOrbit; + } - /** - * Get the underlying Field GPS orbital elements. - * - * @return the underlying Field GPS orbital elements - */ - public FieldGPSOrbitalElements getFieldGPSOrbitalElements() { - return gpsOrbit; - } + /** + * Get the underlying Field GPS orbital elements. + * + * @return the underlying Field GPS orbital elements + */ + public FieldGPSOrbitalElements getFieldGPSOrbitalElements() { + return gpsOrbit; + } - /** Get the parameters driver for the Field GPS propagation model. + /** + * Get the parameters driver for the Field GPS propagation model. + * * @return an empty list. */ - @Override - protected List getParametersDrivers() { - // The Field GPS propagation model does not have parameter drivers. - return Collections.emptyList(); - } + @Override + protected List getParametersDrivers() { + // The Field GPS propagation model does not have parameter drivers. + return Collections.emptyList(); + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java index ad04fb2af..82f9a7a99 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoAlmanac.java @@ -23,7 +23,6 @@ import org.orekit.annotation.DefaultDataContext; import org.orekit.data.DataContext; import org.orekit.gnss.GalileoAlmanac; import org.orekit.gnss.SatelliteSystem; -import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; /** @@ -31,87 +30,91 @@ import org.orekit.time.FieldAbsoluteDate; * * @see "European GNSS (Galileo) Open Service, Signal In Space, Interface * Control Document, Table 75" - * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) - * */ -public class FieldGalileoAlmanac> implements FieldGalileoOrbitalElements { +public class FieldGalileoAlmanac> + implements + FieldGalileoOrbitalElements { + + // Nominal parameters + /** Nominal inclination (Ref: Galileo ICD - Table 75). */ + private static final double I0 = FastMath.toRadians(56.0); - private final T zero; - - // Nominal parameters - /** Nominal inclination (Ref: Galileo ICD - Table 75). */ - private final static double I0 = FastMath.toRadians(56.0); + /** Nominal semi-major axis in meters (Ref: Galileo ICD - Table 75). */ + private static final double A0 = 29600000; - /** Nominal semi-major axis in meters (Ref: Galileo ICD - Table 75). */ - private final static double A0 = 29600000; + /** Field Zero. */ + private final T zero; - /** PRN number. */ - private final int prn; + /** PRN number. */ + private final int prn; - /** Satellite E5a signal health status. */ - private final int healthE5a; + /** Satellite E5a signal health status. */ + private final int healthE5a; - /** Satellite E5b signal health status. */ - private final int healthE5b; + /** Satellite E5b signal health status. */ + private final int healthE5b; - /** Satellite E1-B/C signal health status. */ - private final int healthE1; + /** Satellite E1-B/C signal health status. */ + private final int healthE1; - /** Galileo week. */ - private final int week; + /** Galileo week. */ + private final int week; - /** Time of applicability. */ - private final T toa; + /** Time of applicability. */ + private final T toa; - /** Semi-major axis. */ - private final T sma; + /** Semi-major axis. */ + private final T sma; - /** Eccentricity. */ - private final T ecc; + /** Eccentricity. */ + private final T ecc; - /** Inclination. */ - private final T inc; + /** Inclination. */ + private final T inc; - /** Longitude of Orbital Plane. */ - private final T om0; + /** Longitude of Orbital Plane. */ + private final T om0; - /** Rate of Right Ascension. */ - private final T dom; + /** Rate of Right Ascension. */ + private final T dom; - /** Argument of perigee. */ - private final T aop; + /** Argument of perigee. */ + private final T aop; - /** Mean anomaly. */ - private final T anom; + /** Mean anomaly. */ + private final T anom; - /** Zeroth order clock correction. */ - private final T af0; + /** Zeroth order clock correction. */ + private final T af0; - /** First order clock correction. */ - private final T af1; + /** First order clock correction. */ + private final T af1; - /** Almanac Issue Of Data. */ - private final int iod; + /** Almanac Issue Of Data. */ + private final int iod; - private final FieldAbsoluteDate date; + /** Date. */ + private final FieldAbsoluteDate date; - - /** + /** * Build a new almanac. - * - *

This method uses the {@link DataContext#getDefault() default data context}. + *

+ * This method uses the {@link DataContext#getDefault() default data + * context}. * * @param prn the PRN number * @param week the Galileo week * @param toa the Almanac Time of Applicability (s) - * @param dsqa difference between the square root of the semi-major axis - * and the square root of the nominal semi-major axis + * @param dsqa difference between the square root of the semi-major axis and + * the square root of the nominal semi-major axis * @param ecc the eccentricity - * @param dinc the correction of orbit reference inclination at reference time (rad) + * @param dinc the correction of orbit reference inclination at reference + * time (rad) * @param iod the issue of data - * @param om0 the geographic longitude of the orbital plane at the weekly epoch (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) * @param dom the Rate of Right Ascension (rad/s) * @param aop the Argument of Perigee (rad) * @param anom the Mean Anomaly (rad) @@ -120,34 +123,38 @@ public class FieldGalileoAlmanac> implements Field * @param healthE5a the E5a signal health status * @param healthE5b the E5b signal health status * @param healthE1 the E1-B/C signal health status - * @see #GalileoAlmanac(int, int, T, T, T, T, int, T, T, - * T, T, T, T, int, int, int, AbsoluteDate) + * @see #GalileoAlmanac(int, int, T, T, T, T, int, T, T, T, T, T, T, int, + * int, int, AbsoluteDate) */ @DefaultDataContext public FieldGalileoAlmanac(final int prn, final int week, final T toa, - final T dsqa, final T ecc, final T dinc, - final int iod, final T om0, final T dom, - final T aop, final T anom, final T af0, - final T af1, final int healthE5a, final int healthE5b, - final int healthE1) { - this(prn, week, toa, dsqa, ecc, dinc, iod, om0, dom, aop, anom, af0, af1, - healthE5a, healthE5b, healthE1, - new FieldGNSSDate<>(week, toa.multiply(1000), SatelliteSystem.GALILEO, - DataContext.getDefault().getTimeScales()).getDate()); - } - - /** + final T dsqa, final T ecc, final T dinc, + final int iod, final T om0, final T dom, + final T aop, final T anom, final T af0, + final T af1, final int healthE5a, + final int healthE5b, final int healthE1) { + this(prn, week, toa, dsqa, ecc, dinc, iod, om0, dom, aop, anom, af0, + af1, healthE5a, healthE5b, healthE1, + new FieldGNSSDate<>(week, toa.multiply(1000), + SatelliteSystem.GALILEO, + DataContext.getDefault().getTimeScales()) + .getDate()); + } + + /** * Build a new almanac. * * @param prn the PRN number * @param week the Galileo week * @param toa the Almanac Time of Applicability (s) - * @param dsqa difference between the square root of the semi-major axis - * and the square root of the nominal semi-major axis + * @param dsqa difference between the square root of the semi-major axis and + * the square root of the nominal semi-major axis * @param ecc the eccentricity - * @param dinc the correction of orbit reference inclination at reference time (rad) + * @param dinc the correction of orbit reference inclination at reference + * time (rad) * @param iod the issue of data - * @param om0 the geographic longitude of the orbital plane at the weekly epoch (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) * @param dom the Rate of Right Ascension (rad/s) * @param aop the Argument of Perigee (rad) * @param anom the Mean Anomaly (rad) @@ -160,12 +167,13 @@ public class FieldGalileoAlmanac> implements Field * @since 10.1 */ public FieldGalileoAlmanac(final int prn, final int week, final T toa, - final T dsqa, final T ecc, final T dinc, - final int iod, final T om0, final T dom, - final T aop, final T anom, final T af0, - final T af1, final int healthE5a, final int healthE5b, - final int healthE1, final FieldAbsoluteDate date) { - this.zero = toa.getField().getZero(); + final T dsqa, final T ecc, final T dinc, + final int iod, final T om0, final T dom, + final T aop, final T anom, final T af0, + final T af1, final int healthE5a, + final int healthE5b, final int healthE1, + final FieldAbsoluteDate date) { + this.zero = toa.getField().getZero(); this.prn = prn; this.week = week; this.toa = toa; @@ -187,212 +195,205 @@ public class FieldGalileoAlmanac> implements Field final T sqa = dsqa.add(FastMath.sqrt(A0)); this.sma = sqa.multiply(sqa); } - + /** - * Constructor - * - * This constructor converts a GalileoAlmanac into a FieldGalileoAlmanac - * - * @param field - * @param almanac a GalileoAlmanac - */ - public FieldGalileoAlmanac(Field field, GalileoAlmanac almanac) { - this.zero = field.getZero(); - this.prn = almanac.getPRN(); - this.week = almanac.getWeek(); - this.toa = zero.add(almanac.getTime()); - this.ecc = zero.add(almanac.getE()); - this.inc = zero.add(almanac.getI0()); - this.iod = almanac.getIOD(); - this.om0 = zero.add(almanac.getOmega0()); - this.dom = zero.add(almanac.getOmegaDot()); - this.aop = zero.add(almanac.getPa()); - this.anom = zero.add(almanac.getM0()); - this.af0 = zero.add(almanac.getAf0()); - this.af1 = zero.add(almanac.getAf1()); - this.healthE1 = almanac.getHealthE1(); - this.healthE5a = almanac.getHealthE5a(); + * Constructor This constructor converts a GalileoAlmanac into a + * FieldGalileoAlmanac. + * + * @param field + * @param almanac a GalileoAlmanac + */ + public FieldGalileoAlmanac(final Field field, final GalileoAlmanac almanac) { + this.zero = field.getZero(); + this.prn = almanac.getPRN(); + this.week = almanac.getWeek(); + this.toa = zero.add(almanac.getTime()); + this.ecc = zero.add(almanac.getE()); + this.inc = zero.add(almanac.getI0()); + this.iod = almanac.getIOD(); + this.om0 = zero.add(almanac.getOmega0()); + this.dom = zero.add(almanac.getOmegaDot()); + this.aop = zero.add(almanac.getPa()); + this.anom = zero.add(almanac.getM0()); + this.af0 = zero.add(almanac.getAf0()); + this.af1 = zero.add(almanac.getAf1()); + this.healthE1 = almanac.getHealthE1(); + this.healthE5a = almanac.getHealthE5a(); this.healthE5b = almanac.getHealthE5b(); - this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); - this.sma = zero.add(almanac.getSma()); - } - - @Override - public FieldAbsoluteDate getDate() { - return date; - } - - @Override - public int getPRN() { - return prn; - } - - @Override - public int getWeek() { - return week; - } - - @Override - public T getTime() { - return toa; - } - - @Override - public T getSma() { - return sma; - } - - @Override - public T getMeanMotion() { - final T absA = FastMath.abs(sma); - return FastMath.sqrt(zero.add(GALILEO_MU).divide(absA)).divide(absA); - } - - @Override - public T getE() { - return ecc; - } - - @Override - public T getI0() { - return inc; - } - - @Override - public T getIDot() { - return zero; - } - - @Override - public T getOmega0() { - return om0; - } - - @Override - public T getOmegaDot() { - return dom; - } - - @Override - public T getPa() { - return aop; - } - - @Override - public T getM0() { - return anom; - } - - @Override - public T getCuc() { - return zero; - } - - @Override - public T getCus() { - return zero; - } - - @Override - public T getCrc() { - return zero; - } - - @Override - public T getCrs() { - return zero; - } - - @Override - public T getCic() { - return zero; - } - - @Override - public T getCis() { - return zero; - } - - @Override - public T getAf0() { - return af0; - } - - @Override - public T getAf1() { - return af1; - } - - /** - * Get the Issue of Data (IOD). - * - * @return the Issue Of Data - */ - public int getIOD() { - return iod; - } - - /** - * Gets the E1-B/C signal health status. - * - * @return the E1-B/C signal health status - */ - public int getHealthE1() { - return healthE1; - } - - /** - * Gets the E5a signal health status. - * - * @return the E5a signal health status - */ - public int getHealthE5a() { - return healthE5a; - } - - /** - * Gets the E5b signal health status. - * - * @return the E5b signal health status - */ - public int getHealthE5b() { - return healthE5b; - } - - @Override - public T getAf2() { - // TODO Auto-generated method stub - return null; - } - - @Override - public T getToc() { - // TODO Auto-generated method stub - return null; - } - - @Override - public int getIODNav() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public T getBGD() { - // TODO Auto-generated method stub - return null; - } - - @Override - public T getBGDE1E5a() { - // TODO Auto-generated method stub - return null; - } - - @Override - public T getBGDE5bE1() { - // TODO Auto-generated method stub - return null; - } + this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); + this.sma = zero.add(almanac.getSma()); + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toa; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath.sqrt(zero.add(GALILEO_MU).divide(absA)).divide(absA); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return zero; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return zero; + } + + @Override + public T getCus() { + return zero; + } + + @Override + public T getCrc() { + return zero; + } + + @Override + public T getCrs() { + return zero; + } + + @Override + public T getCic() { + return zero; + } + + @Override + public T getCis() { + return zero; + } + + @Override + public T getAf0() { + return af0; + } + + @Override + public T getAf1() { + return af1; + } + + /** + * Get the Issue of Data (IOD). + * + * @return the Issue Of Data + */ + public int getIOD() { + return iod; + } + + /** + * Gets the E1-B/C signal health status. + * + * @return the E1-B/C signal health status + */ + public int getHealthE1() { + return healthE1; + } + + /** + * Gets the E5a signal health status. + * + * @return the E5a signal health status + */ + public int getHealthE5a() { + return healthE5a; + } + + /** + * Gets the E5b signal health status. + * + * @return the E5b signal health status + */ + public int getHealthE5b() { + return healthE5b; + } + + @Override + public T getAf2() { + return zero; + } + + @Override + public T getToc() { + return zero; + } + + @Override + public int getIODNav() { + return 0; + } + + @Override + public T getBGD() { + return zero; + } + + @Override + public T getBGDE1E5a() { + return zero; + } + + @Override + public T getBGDE5bE1() { + return zero; + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java index f67a14d4f..ef2a96915 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoOrbitalElements.java @@ -18,19 +18,22 @@ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; -/** This interface provides the minimal set of orbital elements needed by the {@link GalileoPropagator}. -* -* @see -* Galileo Interface Control Document -* -* @author Bryan Cazabonne -* @author Nicolas Fialton (field translation) -* -*/ +/** + * This interface provides the minimal set of orbital elements needed by the + * {@link GalileoPropagator}. + * + * @see + * Galileo Interface Control Document + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + */ + +public interface FieldGalileoOrbitalElements> + extends + FieldGNSSOrbitalElements { -public interface FieldGalileoOrbitalElements> extends FieldGNSSOrbitalElements { - - // Constants + // Constants /** Earth's universal gravitational parameter for Galileo user in m³/s². */ double GALILEO_MU = 3.986004418e+14; diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java index 8b315e1e1..d5fe27f29 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldGalileoPropagator.java @@ -37,158 +37,164 @@ import org.orekit.utils.ParameterDriver; * @see Galileo * Interface Control Document - * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) */ -public class FieldGalileoPropagator> extends FieldAbstractGNSSPropagator { +public class FieldGalileoPropagator> + extends + FieldAbstractGNSSPropagator { - // Constants - /** Value of the earth's rotation rate in rad/s. */ - private static final double GALILEO_AV = 7.2921151467e-5; + // Constants + /** Value of the earth's rotation rate in rad/s. */ + private static final double GALILEO_AV = 7.2921151467e-5; - /** Duration of the Galileo cycle in seconds. */ - private static final double GALILEO_CYCLE_DURATION = GalileoOrbitalElements.GALILEO_WEEK_IN_SECONDS - * GalileoOrbitalElements.GALILEO_WEEK_NB; + /** Duration of the Galileo cycle in seconds. */ + private static final double GALILEO_CYCLE_DURATION = + GalileoOrbitalElements.GALILEO_WEEK_IN_SECONDS * + GalileoOrbitalElements.GALILEO_WEEK_NB; - // Fields - /** The Galileo orbital elements used. */ - private final FieldGalileoOrbitalElements galileoOrbit; + // Fields + /** The Galileo orbital elements used. */ + private final FieldGalileoOrbitalElements galileoOrbit; - /** - * Default constructor - * - *

- * The Field Galileo orbital elements is the only requested parameter to build a - * FieldGalileoPropagator. - *

- *

- * The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default - * data context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - *

- * This constructor uses the {@link DataContext#getDefault() default data - * context}. Another data context can be set using - * {@code FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, - final Frames frames)} - *

- * - * @param field - * @param galileoOrbit the Field Galileo orbital elements to be used by the Field Galileo propagator. - * @see #attitudeProvider(AttitudeProvider provider) - * @see #mass(double mass) - * @see #eci(Frame inertial) - * @see #ecef(Frame bodyFixed) - */ - @DefaultDataContext - public FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit) { - this(field, galileoOrbit, DataContext.getDefault().getFrames()); - } + /** + * Default constructor + *

+ * The Field Galileo orbital elements is the only requested parameter to + * build a FieldGalileoPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. Another data context can be set using + * {@code FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, final Frames frames)} + *

+ * + * @param field + * @param galileoOrbit the Field Galileo orbital elements to be used by the + * Field Galileo propagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) + */ + @DefaultDataContext + public FieldGalileoPropagator(final Field field, + final FieldGalileoOrbitalElements galileoOrbit) { + this(field, galileoOrbit, DataContext.getDefault().getFrames()); + } - /** - * Constructor - * - *

- * The Field Galileo orbital elements is the only requested parameter to build a - * FieldGalileoPropagator. - *

- *

- * The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default - * data context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, - final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)}

- * - * @param field - * @param galileoOrbit the Field Galileo orbital elements to be used by the - * Field Galileo propagator. - * @param frames to use building the propagator. - * @see #attitudeProvider(AttitudeProvider provider) - * @see #mass(double mass) - * @see #eci(Frame inertial) - * @see #ecef(Frame bodyFixed) - */ - public FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, - final Frames frames) { - this(field, galileoOrbit, Propagator.getDefaultLaw(frames), DEFAULT_MASS, frames.getEME2000(), - frames.getITRF(IERSConventions.IERS_2010, true)); - } + /** + * Constructor + *

+ * The Field Galileo orbital elements is the only requested parameter to + * build a FieldGalileoPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ *

+ * This constructor uses the {@link DataContext#getDefault() default data + * context}. Another data context can be set using + * {@code FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)} + *

+ * + * @param field + * @param galileoOrbit the Field Galileo orbital elements to be used by the + * Field Galileo propagator. + * @param frames to use building the propagator. + * @see #attitudeProvider(AttitudeProvider provider) + * @see #mass(double mass) + * @see #eci(Frame inertial) + * @see #ecef(Frame bodyFixed) + */ + public FieldGalileoPropagator(final Field field, + final FieldGalileoOrbitalElements galileoOrbit, + final Frames frames) { + this(field, galileoOrbit, Propagator.getDefaultLaw(frames), + DEFAULT_MASS, frames.getEME2000(), + frames.getITRF(IERSConventions.IERS_2010, true)); + } - /** - * Constructor - * - * - *

- * The Field Galileo orbital elements is the only requested parameter to build a - * FieldGalileoPropagator. - *

- *

- * The attitude provider is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the - * default data context.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default - * data context.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP - * CIO/2010-based ITRF simple EOP} in the default data context. - *

- * - * @param field - * @param galileoOrbit - * @param attitudeProvider - * @param mass - * @param eci - * @param ecef - */ - public FieldGalileoPropagator(final Field field, final FieldGalileoOrbitalElements galileoOrbit, - final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef) { - super(field, galileoOrbit, attitudeProvider, eci, ecef, mass, GALILEO_AV, GALILEO_CYCLE_DURATION, - FieldGalileoOrbitalElements.GALILEO_MU); - // Stores the Galileo orbital elements - this.galileoOrbit = galileoOrbit; - } + /** + * Constructor + *

+ * The Field Galileo orbital elements is the only requested parameter to + * build a FieldGalileoPropagator. + *

+ *

+ * The attitude provider is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the + * default data context.
+ * The mass is set by default to the + * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
+ * The ECI frame is set by default to the + * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default + * data context.
+ * The ECEF frame is set by default to the + * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP + * CIO/2010-based ITRF simple EOP} in the default data context. + *

+ * + * @param field + * @param galileoOrbit + * @param attitudeProvider + * @param mass + * @param eci + * @param ecef + */ + public FieldGalileoPropagator(final Field field, + final FieldGalileoOrbitalElements galileoOrbit, + final AttitudeProvider attitudeProvider, + final double mass, final Frame eci, + final Frame ecef) { + super(field, galileoOrbit, attitudeProvider, eci, ecef, mass, + GALILEO_AV, GALILEO_CYCLE_DURATION, + FieldGalileoOrbitalElements.GALILEO_MU); + // Stores the Galileo orbital elements + this.galileoOrbit = galileoOrbit; + } - /** - * Get the underlying Field Galileo orbital elements. - * - * @return the underlying Field Galileo orbital elements - */ - public FieldGalileoOrbitalElements getFieldGalileoOrbitalElements() { - return galileoOrbit; - } + /** + * Get the underlying Field Galileo orbital elements. + * + * @return the underlying Field Galileo orbital elements + */ + public FieldGalileoOrbitalElements getFieldGalileoOrbitalElements() { + return galileoOrbit; + } - /** Get the parameters driver for the Field Galileo propagation model. + /** + * Get the parameters driver for the Field Galileo propagation model. + * * @return an empty list. */ - @Override - protected List getParametersDrivers() { - // Field Galileo propagation model does not have parameter drivers. - return Collections.emptyList(); - } + @Override + protected List getParametersDrivers() { + // Field Galileo propagation model does not have parameter drivers. + return Collections.emptyList(); + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java index 71c1c713b..256e7cd45 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSAlmanac.java @@ -26,243 +26,244 @@ import org.orekit.time.FieldAbsoluteDate; * * @see "Indian Regiona Navigation Satellite System, Signal In Space ICD for * standard positioning service, version 1.1 - Table 28" - * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) - * */ -public class FieldIRNSSAlmanac> implements FieldIRNSSOrbitalElements { - - private final T zero; - /** PRN number. */ - private final int prn; - - /** IRNSS week. */ - private final int week; - - /** Time of applicability. */ - private final T toa; - - /** Semi-major axis. */ - private final T sma; - - /** Eccentricity. */ - private final T ecc; - - /** Inclination. */ - private final T inc; - - /** Longitude of Orbital Plane. */ - private final T om0; - - /** Rate of Right Ascension. */ - private final T dom; - - /** Argument of perigee. */ - private final T aop; - - /** Mean anomaly. */ - private final T anom; - - /** Zeroth order clock correction. */ - private final T af0; - - /** First order clock correction. */ - private final T af1; - - /** Date of aplicability. */ - private final FieldAbsoluteDate date; - - /** - * Constructor. - * - * @param prn the PRN number - * @param week the GPS week - * @param toa the Time of Applicability - * @param sqa the Square Root of Semi-Major Axis (m^1/2) - * @param ecc the eccentricity - * @param inc the inclination (rad) - * @param om0 the geographic longitude of the orbital plane at the weekly epoch - * (rad) - * @param dom the Rate of Right Ascension (rad/s) - * @param aop the Argument of Perigee (rad) - * @param anom the Mean Anomaly (rad) - * @param af0 the Zeroth Order Clock Correction (s) - * @param af1 the First Order Clock Correction (s/s) - * @param date of applicability corresponding to {@code toa}. - */ - public FieldIRNSSAlmanac(final int prn, final int week, final T toa, final T sqa, final T ecc, final T inc, - final T om0, final T dom, final T aop, final T anom, final T af0, final T af1, - final FieldAbsoluteDate date) { - this.zero = toa.getField().getZero(); - this.prn = prn; - this.week = week; - this.toa = toa; - this.sma = sqa.multiply(sqa); - this.ecc = ecc; - this.inc = inc; - this.om0 = om0; - this.dom = dom; - this.aop = aop; - this.anom = anom; - this.af0 = af0; - this.af1 = af1; - this.date = date; - } - - /** - * Constructor - * - * This constructor converts an IRNSSOrbitalElements (almanac) into a FieldIRNSSAlmanac - * - * @param field - * @param almanac - */ - public FieldIRNSSAlmanac(Field field, IRNSSOrbitalElements almanac) { - this.zero = field.getZero(); - this.prn = almanac.getPRN(); - this.week = almanac.getWeek(); - this.toa = zero.add(almanac.getTime()); - this.sma = zero.add(almanac.getSma()); - this.ecc = zero.add(almanac.getE()); - this.inc = zero.add(almanac.getI0()); - this.om0 = zero.add(almanac.getOmega0()); - this.dom = zero.add(almanac.getOmegaDot()); - this.aop = zero.add(almanac.getPa()); - this.anom = zero.add(almanac.getM0()); - this.af0 = zero.add(almanac.getAf0()); - this.af1 = zero.add(almanac.getAf1()); - this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); - } - - @Override - public FieldAbsoluteDate getDate() { - return date; - } - - @Override - public int getPRN() { - return prn; - } - - @Override - public int getWeek() { - return week; - } - - @Override - public T getTime() { - return toa; - } - - @Override - public T getSma() { - return sma; - } - - @Override - public T getMeanMotion() { - final T absA = FastMath.abs(sma); - return FastMath.sqrt(absA.getField().getZero().add(IRNSS_MU).divide(absA)).divide(absA); - } - - @Override - public T getE() { - return ecc; - } - - @Override - public T getI0() { - return inc; - } - - @Override - public T getIDot() { - return zero; - } - - @Override - public T getOmega0() { - return om0; - } - - @Override - public T getOmegaDot() { - return dom; - } - - @Override - public T getPa() { - return aop; - } - - @Override - public T getM0() { - return anom; - } - - @Override - public T getCuc() { - return zero; - } - - @Override - public T getCus() { - return zero; - } - - @Override - public T getCrc() { - return zero; - } - - @Override - public T getCrs() { - return zero; - } - - @Override - public T getCic() { - return zero; - } - - @Override - public T getCis() { - return zero; - } - - @Override - public T getAf0() { - return af0; - } - - @Override - public T getAf1() { - return af1; - } - - @Override - public T getAf2() { - // TODO Auto-generated method stub - return null; - } - - @Override - public T getToc() { - // TODO Auto-generated method stub - return null; - } - - @Override - public int getIODEC() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public T getTGD() { - // TODO Auto-generated method stub - return null; - } +public class FieldIRNSSAlmanac> + implements + FieldIRNSSOrbitalElements { + + /** Field Zero.*/ + private final T zero; + + /** PRN number. */ + private final int prn; + + /** IRNSS week. */ + private final int week; + + /** Time of applicability. */ + private final T toa; + + /** Semi-major axis. */ + private final T sma; + + /** Eccentricity. */ + private final T ecc; + + /** Inclination. */ + private final T inc; + + /** Longitude of Orbital Plane. */ + private final T om0; + + /** Rate of Right Ascension. */ + private final T dom; + + /** Argument of perigee. */ + private final T aop; + + /** Mean anomaly. */ + private final T anom; + + /** Zeroth order clock correction. */ + private final T af0; + + /** First order clock correction. */ + private final T af1; + + /** Date of aplicability. */ + private final FieldAbsoluteDate date; + + /** + * Constructor. + * + * @param prn the PRN number + * @param week the GPS week + * @param toa the Time of Applicability + * @param sqa the Square Root of Semi-Major Axis (m^1/2) + * @param ecc the eccentricity + * @param inc the inclination (rad) + * @param om0 the geographic longitude of the orbital plane at the weekly + * epoch (rad) + * @param dom the Rate of Right Ascension (rad/s) + * @param aop the Argument of Perigee (rad) + * @param anom the Mean Anomaly (rad) + * @param af0 the Zeroth Order Clock Correction (s) + * @param af1 the First Order Clock Correction (s/s) + * @param date of applicability corresponding to {@code toa}. + */ + public FieldIRNSSAlmanac(final int prn, final int week, final T toa, + final T sqa, final T ecc, final T inc, final T om0, + final T dom, final T aop, final T anom, + final T af0, final T af1, + final FieldAbsoluteDate date) { + this.zero = toa.getField().getZero(); + this.prn = prn; + this.week = week; + this.toa = toa; + this.sma = sqa.multiply(sqa); + this.ecc = ecc; + this.inc = inc; + this.om0 = om0; + this.dom = dom; + this.aop = aop; + this.anom = anom; + this.af0 = af0; + this.af1 = af1; + this.date = date; + } + + /** + * Constructor This constructor converts an IRNSSOrbitalElements (almanac) + * into a FieldIRNSSAlmanac. + * + * @param field + * @param almanac + */ + public FieldIRNSSAlmanac(final Field field, final IRNSSOrbitalElements almanac) { + this.zero = field.getZero(); + this.prn = almanac.getPRN(); + this.week = almanac.getWeek(); + this.toa = zero.add(almanac.getTime()); + this.sma = zero.add(almanac.getSma()); + this.ecc = zero.add(almanac.getE()); + this.inc = zero.add(almanac.getI0()); + this.om0 = zero.add(almanac.getOmega0()); + this.dom = zero.add(almanac.getOmegaDot()); + this.aop = zero.add(almanac.getPa()); + this.anom = zero.add(almanac.getM0()); + this.af0 = zero.add(almanac.getAf0()); + this.af1 = zero.add(almanac.getAf1()); + this.date = new FieldAbsoluteDate<>(field, almanac.getDate()); + } + + @Override + public FieldAbsoluteDate getDate() { + return date; + } + + @Override + public int getPRN() { + return prn; + } + + @Override + public int getWeek() { + return week; + } + + @Override + public T getTime() { + return toa; + } + + @Override + public T getSma() { + return sma; + } + + @Override + public T getMeanMotion() { + final T absA = FastMath.abs(sma); + return FastMath + .sqrt(absA.getField().getZero().add(IRNSS_MU).divide(absA)) + .divide(absA); + } + + @Override + public T getE() { + return ecc; + } + + @Override + public T getI0() { + return inc; + } + + @Override + public T getIDot() { + return zero; + } + + @Override + public T getOmega0() { + return om0; + } + + @Override + public T getOmegaDot() { + return dom; + } + + @Override + public T getPa() { + return aop; + } + + @Override + public T getM0() { + return anom; + } + + @Override + public T getCuc() { + return zero; + } + + @Override + public T getCus() { + return zero; + } + + @Override + public T getCrc() { + return zero; + } + + @Override + public T getCrs() { + return zero; + } + + @Override + public T getCic() { + return zero; + } + + @Override + public T getCis() { + return zero; + } + + @Override + public T getAf0() { + return af0; + } + + @Override + public T getAf1() { + return af1; + } + + @Override + public T getAf2() { + return zero; + } + + @Override + public T getToc() { + return zero; + } + + @Override + public int getIODEC() { + return 0; + } + + @Override + public T getTGD() { + return zero; + } } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java index 7f9a39de3..a527be18d 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSOrbitalElements.java @@ -18,18 +18,24 @@ package org.orekit.propagation.analytical.gnss; import org.hipparchus.RealFieldElement; -/** This interface provides the minimal set of Field orbital elements needed by the {@link FieldIRNSSPropagator}. -* -* @see "Indian Regiona Navigation Satellite System, Signal In Space ICD -* for standard positioning service, version 1.1" -* -* @author Bryan Cazabonne -* @author Nicolas Fialton (field translation) -*/ - -public interface FieldIRNSSOrbitalElements> extends FieldGNSSOrbitalElements { - - /** WGS 84 value of the Earth's universal gravitational parameter for IRNSS user in m³/s². */ +/** + * This interface provides the minimal set of Field orbital elements needed by + * the {@link FieldIRNSSPropagator}. + * + * @see "Indian Regiona Navigation Satellite System, Signal In Space ICD for + * standard positioning service, version 1.1" + * @author Bryan Cazabonne + * @author Nicolas Fialton (field translation) + */ + +public interface FieldIRNSSOrbitalElements> + extends + FieldGNSSOrbitalElements { + + /** + * WGS 84 value of the Earth's universal gravitational parameter for IRNSS + * user in m³/s². + */ double IRNSS_MU = 3.986005e+14; /** Value of Pi for conversion from semicircles to radian. */ @@ -51,7 +57,8 @@ public interface FieldIRNSSOrbitalElements> extend /** * Gets the estimated group delay differential TGD for L5-S correction. * - * @return the estimated group delay differential TGD for L5-S correction (s) + * @return the estimated group delay differential TGD for L5-S correction + * (s) */ T getTGD(); } diff --git a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java index d30b1e119..ed1b4d9b7 100644 --- a/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java +++ b/src/main/java/org/orekit/propagation/analytical/gnss/FieldIRNSSPropagator.java @@ -36,123 +36,151 @@ import org.orekit.utils.ParameterDriver; * * @see "Indian Regional Navigation Satellite System, Signal In Space ICD for * standard positioning service, version 1.1" - * * @author Bryan Cazabonne * @author Nicolas Fialton (field translation) */ -public class FieldIRNSSPropagator> extends FieldAbstractGNSSPropagator { - // Constants - /** WGS 84 value of the earth's rotation rate in rad/s. */ - private static final double IRNSS_AV = 7.2921151467e-5; +public class FieldIRNSSPropagator> + extends + FieldAbstractGNSSPropagator { - /** Duration of the IRNSS cycle in seconds. */ - private static final double IRNSS_CYCLE_DURATION = FieldIRNSSOrbitalElements.IRNSS_WEEK_IN_SECONDS - * FieldIRNSSOrbitalElements.IRNSS_WEEK_NB; + // Constants + /** WGS 84 value of the earth's rotation rate in rad/s. */ + private static final double IRNSS_AV = 7.2921151467e-5; - // Fields - /** The IRNSS orbital elements used. */ - private final FieldIRNSSOrbitalElements irnssOrbit; + /** Duration of the IRNSS cycle in seconds. */ + private static final double IRNSS_CYCLE_DURATION = + FieldIRNSSOrbitalElements.IRNSS_WEEK_IN_SECONDS * + FieldIRNSSOrbitalElements.IRNSS_WEEK_NB; - /** - * Default constructor. - * - *

The Field IRNSS orbital elements is the only requested parameter to build a FieldIRNSSPropagator.

- *

The attitude provider is set by default to be aligned with the J2000 frame.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame}.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP CIO/2010-based ITRF simple EOP}. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit, - final Frames frames)}

- * - * @param field - * @param irnssOrbit the Field IRNSS orbital elements to be used by the IRNSSpropagator. - */ - @DefaultDataContext - public FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit) { - this(field, irnssOrbit, DataContext.getDefault().getFrames()); - } + // Fields + /** The IRNSS orbital elements used. */ + private final FieldIRNSSOrbitalElements irnssOrbit; - /** - * Constructor. - * - *

The Field IRNSS orbital elements is the only requested parameter to build a FieldIRNSSPropagator.

- *

The attitude provider is set by default to be aligned with the J2000 frame.
- * The mass is set by default to the - * {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
- * The ECI frame is set by default to the - * {@link org.orekit.frames.Predefined#EME2000 EME2000 frame}.
- * The ECEF frame is set by default to the - * {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP CIO/2010-based ITRF simple EOP}. - *

- * - *

This constructor uses the {@link DataContext#getDefault() default data context}. - * Another data context can be set using - * {@code FieldIRNSSPropagator(final Field field, final FieldIRNSSOrbitalElements irnssOrbit, - final AttitudeProvider attitudeProvider, final double mass, final Frame eci, final Frame ecef)}

- * - * @param field - * @param irnssOrbit the Field IRNSS orbital elements to be used by the IRNSSpropagator. - * @param frames set of reference frames to use to initialize {@link - * #ecef(Frame)}, {@link #eci(Frame)}, and