FieldGnssPropagator.java
/* Copyright 2022-2026 Luc Maisonobe
* 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.CalculusFieldElement;
import org.hipparchus.Field;
import org.hipparchus.analysis.differentiation.FieldGradient;
import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.FieldMatrix;
import org.hipparchus.linear.FieldQRDecomposition;
import org.hipparchus.linear.FieldVector;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.FieldSinCos;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.FieldAttitude;
import org.orekit.attitudes.FrameAlignedProvider;
import org.orekit.frames.Frame;
import org.orekit.orbits.FieldKeplerianAnomalyUtility;
import org.orekit.orbits.FieldKeplerianOrbit;
import org.orekit.orbits.FieldKeplerianParameters;
import org.orekit.orbits.FieldOrbit;
import org.orekit.orbits.PositionAngleType;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.analytical.FieldAbstractAnalyticalPropagator;
import org.orekit.propagation.analytical.gnss.data.FieldGnssOrbitalElements;
import org.orekit.propagation.analytical.gnss.data.GNSSOrbitalElements;
import org.orekit.propagation.analytical.gnss.data.GNSSOrbitalElementsFactory;
import org.orekit.propagation.analytical.gnss.data.NonKeplerianDriversFactory;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.FieldPVCoordinates;
import org.orekit.utils.ParameterDriver;
/** Common handling of {@link FieldAbstractAnalyticalPropagator} methods for GNSS propagators.
* <p>
* This class allows to provide easily a subset of {@link FieldAbstractAnalyticalPropagator} methods
* for specific GNSS propagators.
* </p>
* @author Pascal Parraud
* @author Luc Maisonobe
* @param <T> type of the field elements
* @param <O> type of the orbital elements (non-field version)
* @since 13.0
*/
public class FieldGnssPropagator<T extends CalculusFieldElement<T>, O extends GNSSOrbitalElements<O>>
extends FieldAbstractAnalyticalPropagator<T> {
/** Maximum number of iterations for internal loops. */
private static final int MAX_ITER = 100;
/** Tolerance on position for rebuilding orbital elements from initial state. */
private static final double TOL_P = 1.0e-6;
/** Tolerance on velocity for rebuilding orbital elements from initial state. */
private static final double TOL_V = 1.0e-9;
/** Number of free parameters for orbital elements. */
private static final int FREE_PARAMETERS = 6;
/** Convergence parameter. */
private static final double EPS = 1.0e-12;
/** The GNSS propagation model used. */
private FieldGnssOrbitalElements<T, O> orbitalElements;
/** Factory for non-Keplerian elements drivers.
* @since 14.0
*/
private final NonKeplerianDriversFactory driversFactory;
/** 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.
* <p>
* The attitude provider is set by default to be aligned with the provided inertial frame.
* This can be changed (typically to {@link org.orekit.gnss.attitude.GenericGNSS}) after
* construction by calling {@link #setAttitudeProvider(org.orekit.attitudes.AttitudeProvider)
* setAttitudeProvider}
* </p>
* <p>
* The mass is set to the {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.
* </p>
* @param field field to which elements belong
* @param factory factory for the elements and frames
* @since 14.0
*/
public FieldGnssPropagator(final Field<T> field, final GNSSOrbitalElementsFactory<O> factory) {
this(factory.createFromDrivers().toField(field),
factory.getInertial(), factory.getBodyFixed(),
FrameAlignedProvider.of(factory.getInertial()),
field.getZero().newInstance(Propagator.DEFAULT_MASS));
}
/**
* Build a new instance.
* @param orbitalElements GNSS orbital elements
* @param eci Earth Centered Inertial frame
* @param ecef Earth Centered Earth Fixed frame
* @param provider Attitude provider
* @param mass Satellite mass (kg)
*/
public FieldGnssPropagator(final FieldGnssOrbitalElements<T, O> orbitalElements,
final Frame eci, final Frame ecef,
final AttitudeProvider provider, final T mass) {
super(orbitalElements.getDate().getField(), provider);
// Stores the GNSS orbital elements
this.orbitalElements = orbitalElements;
this.driversFactory = new NonKeplerianDriversFactory();
driversFactory.reset(orbitalElements);
// Sets the Earth Centered Inertial frame
this.eci = eci;
// Sets the Earth Centered Earth Fixed frame
this.ecef = ecef;
// Sets initial state
final FieldOrbit<T> orbit = propagateOrbit(orbitalElements.getDate(),
getParameters(orbitalElements.getDate().getField()));
final FieldAttitude<T> attitude = provider.getAttitude(orbit, orbit.getDate(), orbit.getFrame());
// calling the method from constructor because the one overridden below recomputes the orbital elements
super.resetInitialState(new FieldSpacecraftState<>(orbit, attitude).withMass(mass));
}
/**
* Build a new instance from an initial state.
* <p>
* The Keplerian elements already present in the {@code nonKeplerianElements} argument
* will be ignored as it is the {@code initialState} argument that will be used to
* build the complete orbital elements of the propagator
* </p>
* @param initialState initial state
* @param nonKeplerianElements non-Keplerian orbital elements (the Keplerian orbital elements will be ignored)
* @param ecef Earth Centered Earth Fixed frame
* @param provider attitude provider
* @param mass spacecraft mass
*/
public FieldGnssPropagator(final FieldSpacecraftState<T> initialState,
final FieldGnssOrbitalElements<T, O> nonKeplerianElements,
final Frame ecef, final AttitudeProvider provider, final T mass) {
this(buildOrbitalElements(initialState, nonKeplerianElements, new NonKeplerianDriversFactory(),
ecef, provider, mass),
initialState.getFrame(), ecef, provider, initialState.getMass());
}
/** {@inheritDoc} */
@Override
public List<ParameterDriver> getParametersDrivers() {
return driversFactory.getParametersDrivers();
}
/**
* 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;
}
/**
* Gets the Earth gravity coefficient used for GNSS propagation.
*
* @return the Earth gravity coefficient.
*/
public T getMU() {
return orbitalElements.getOrbit().getMu();
}
/** Get the underlying GNSS propagation orbital elements.
* @return the underlying GNSS orbital elements
* @since 14.0
*/
public FieldGnssOrbitalElements<T, O> getOrbitalElements() {
return orbitalElements;
}
/** {@inheritDoc} */
@Override
public FieldOrbit<T> propagateOrbit(final FieldAbsoluteDate<T> date, final T[] parameters) {
// Get the PVCoordinates in ECEF frame
final FieldPVCoordinates<T> pvaInECEF = propagateInEcef(date, parameters);
// Transform the PVCoordinates to ECI frame
final FieldPVCoordinates<T> pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF);
// Return the Keplerian orbit
return new FieldKeplerianOrbit<>(pvaInECI, eci, date, getMU());
}
/**
* Gets the PVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}.
*
* <p>The algorithm uses automatic differentiation to compute velocity and
* acceleration.</p>
*
* @param date the computation date
* @param parameters propagation parameters
* @return the GNSS SV PVCoordinates in {@link #getECEF() ECEF frame}
*/
public FieldPVCoordinates<T> propagateInEcef(final FieldAbsoluteDate<T> date, final T[] parameters) {
final FieldKeplerianOrbit<T> orbit = orbitalElements.getOrbit();
// Duration from GNSS ephemeris Reference date
final FieldUnivariateDerivative2<T> tk = new FieldUnivariateDerivative2<>(getTk(date),
date.getField().getOne(),
date.getField().getZero());
// Semi-major axis
final FieldUnivariateDerivative2<T> ak = tk.multiply(parameters[NonKeplerianDriversFactory.A_DOT_INDEX]).
add(orbit.getA());
// Mean motion
final FieldUnivariateDerivative2<T> nA = tk.multiply(parameters[NonKeplerianDriversFactory.DELTA_N0_DOT_INDEX].multiply(0.5)).
add(parameters[NonKeplerianDriversFactory.DELTA_N0_INDEX]).
add(orbit.getKeplerianMeanMotion());
// Mean anomaly
final FieldUnivariateDerivative2<T> mk = tk.multiply(nA).add(orbit.getMeanAnomaly());
// Eccentric Anomaly
final FieldUnivariateDerivative2<T> e = tk.newInstance(orbit.getE());
final FieldUnivariateDerivative2<T> ek = FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, mk);
// True Anomaly
final FieldUnivariateDerivative2<T> vk = FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(e, ek);
// Argument of Latitude
final FieldUnivariateDerivative2<T> phik = vk.add(orbit.getPeriapsisArgument());
final FieldSinCos<FieldUnivariateDerivative2<T>> cs2phi = FastMath.sinCos(phik.multiply(2));
// Argument of Latitude Correction
final FieldUnivariateDerivative2<T> dphik = cs2phi.cos().multiply(parameters[NonKeplerianDriversFactory.CUC_INDEX]).
add(cs2phi.sin().multiply(parameters[NonKeplerianDriversFactory.CUS_INDEX]));
// Radius Correction
final FieldUnivariateDerivative2<T> drk = cs2phi.cos().multiply(parameters[NonKeplerianDriversFactory.CRC_INDEX]).
add(cs2phi.sin().multiply(parameters[NonKeplerianDriversFactory.CRS_INDEX]));
// Inclination Correction
final FieldUnivariateDerivative2<T> dik = cs2phi.cos().multiply(parameters[NonKeplerianDriversFactory.CIC_INDEX]).
add(cs2phi.sin().multiply(parameters[NonKeplerianDriversFactory.CIS_INDEX]));
// Corrected Argument of Latitude
final FieldSinCos<FieldUnivariateDerivative2<T>> csuk = FastMath.sinCos(phik.add(dphik));
// Corrected Radius
final FieldUnivariateDerivative2<T> rk = ek.cos().multiply(e.negate()).add(1).multiply(ak).add(drk);
// Corrected Inclination
final FieldUnivariateDerivative2<T> ik = tk.multiply(parameters[NonKeplerianDriversFactory.I_DOT_INDEX]).
add(orbit.getI()).add(dik);
final FieldSinCos<FieldUnivariateDerivative2<T>> csik = FastMath.sinCos(ik);
// Positions in orbital plane
final FieldUnivariateDerivative2<T> xk = csuk.cos().multiply(rk);
final FieldUnivariateDerivative2<T> yk = csuk.sin().multiply(rk);
// Corrected longitude of ascending node
final FieldSinCos<FieldUnivariateDerivative2<T>> csomk =
FastMath.sinCos(tk.multiply(parameters[NonKeplerianDriversFactory.OMEGA_DOT_INDEX].
subtract(orbitalElements.getAngularVelocity())).
add(orbit.getRightAscensionOfAscendingNode().
subtract(parameters[NonKeplerianDriversFactory.TIME_INDEX].multiply(orbitalElements.getAngularVelocity()))));
// returns the Earth-fixed coordinates
final FieldVector3D<FieldUnivariateDerivative2<T>> positionWithDerivatives =
new FieldVector3D<>(xk.multiply(csomk.cos()).subtract(yk.multiply(csomk.sin()).multiply(csik.cos())),
xk.multiply(csomk.sin()).add(yk.multiply(csomk.cos()).multiply(csik.cos())),
yk.multiply(csik.sin()));
return new FieldPVCoordinates<>(positionWithDerivatives);
}
/**
* Gets the duration from GNSS Reference epoch.
* <p>This takes the GNSS week roll-over into account.</p>
* @param date the considered date
* @return the duration from GNSS orbit Reference epoch (s)
*/
private T getTk(final FieldAbsoluteDate<T> date) {
// Time from ephemeris reference epoch
T tk = date.durationFrom(orbitalElements.getTimeOfEphemeris());
// Adjusts the time to take roll over week into account
while (tk.getReal() > 0.5 * orbitalElements.getCycleDuration()) {
tk = tk.subtract(orbitalElements.getCycleDuration());
}
while (tk.getReal() < -0.5 * orbitalElements.getCycleDuration()) {
tk = tk.add(orbitalElements.getCycleDuration());
}
// Returns the time from ephemeris reference epoch
return tk;
}
/** {@inheritDoc} */
@Override
public Frame getFrame() {
return eci;
}
/** {@inheritDoc} */
@Override
protected T getMass(final FieldAbsoluteDate<T> date) {
return getInitialState().getMass();
}
/** {@inheritDoc} */
@Override
public void resetInitialState(final FieldSpacecraftState<T> state) {
orbitalElements = buildOrbitalElements(state, orbitalElements, driversFactory,
ecef, getAttitudeProvider(), state.getMass());
final FieldOrbit<T> orbit = propagateOrbit(orbitalElements.getDate(),
getParameters(orbitalElements.getDate().getField()));
final FieldAttitude<T> attitude = getAttitudeProvider().getAttitude(orbit, orbit.getDate(), orbit.getFrame());
super.resetInitialState(new FieldSpacecraftState<>(orbit, attitude).withMass(state.getMass()));
}
/** {@inheritDoc} */
@Override
protected void resetIntermediateState(final FieldSpacecraftState<T> state, final boolean forward) {
resetInitialState(state);
}
/**
* Build orbital elements from initial state.
* <p>
* This method is roughly the inverse of {@link #propagateInEcef(FieldAbsoluteDate, CalculusFieldElement[])},
* except it starts from a state in inertial frame
* </p>
*
* @param <T> type of the field elements
* @param <O> type of the orbital elements (non-field version)
* @param initialState initial state
* @param nonKeplerianElements non-Keplerian orbital elements (the Keplerian orbital elements will be overridden)
* @param driversFactory factory for non-Keplerian drivers
* @param ecef Earth Centered Earth Fixed frame
* @param provider attitude provider
* @param mass satellite mass (kg)
* @return orbital elements that generate the {@code initialState} when used with a propagator
*/
public static <T extends CalculusFieldElement<T>,
O extends GNSSOrbitalElements<O>>
FieldGnssOrbitalElements<T, O> buildOrbitalElements(final FieldSpacecraftState<T> initialState,
final FieldGnssOrbitalElements<T, O> nonKeplerianElements,
final NonKeplerianDriversFactory driversFactory,
final Frame ecef, final AttitudeProvider provider,
final T mass) {
final Field<T> field = initialState.getDate().getField();
// get approximate initial orbit
final Frame frozenEcef = ecef.getFrozenFrame(initialState.getFrame(),
initialState.getDate().toAbsoluteDate(),
GNSSOrbitalElementsFactory.FROZEN + ecef.getName());
final FieldKeplerianOrbit<T> orbit = approximateInitialOrbit(initialState, nonKeplerianElements, frozenEcef);
driversFactory.reset(nonKeplerianElements);
// refine orbit using simple differential correction to reach target PV
final FieldPVCoordinates<T> targetPV = initialState.getPVCoordinates(frozenEcef);
FieldGnssOrbitalElements<FieldGradient<T>, O> gElements = toGradient(nonKeplerianElements, orbit, driversFactory);
for (int i = 0; i < MAX_ITER; ++i) {
// get position-velocity derivatives with respect to initial orbit
final FieldGnssPropagator<FieldGradient<T>, O> gPropagator =
new FieldGnssPropagator<>(gElements, frozenEcef, ecef, provider,
gElements.getOrbit().getMu().newInstance(mass));
final FieldPVCoordinates<FieldGradient<T>> gPV = gPropagator.getInitialState().getPVCoordinates();
// compute Jacobian matrix
final FieldMatrix<T> jacobian = MatrixUtils.createFieldMatrix(field, FREE_PARAMETERS, FREE_PARAMETERS);
jacobian.setRow(0, gPV.getPosition().getX().getGradient());
jacobian.setRow(1, gPV.getPosition().getY().getGradient());
jacobian.setRow(2, gPV.getPosition().getZ().getGradient());
jacobian.setRow(3, gPV.getVelocity().getX().getGradient());
jacobian.setRow(4, gPV.getVelocity().getY().getGradient());
jacobian.setRow(5, gPV.getVelocity().getZ().getGradient());
// linear correction to get closer to target PV
final FieldVector<T> residuals = MatrixUtils.createFieldVector(field, FREE_PARAMETERS);
residuals.setEntry(0, targetPV.getPosition().getX().subtract(gPV.getPosition().getX().getValue()));
residuals.setEntry(1, targetPV.getPosition().getY().subtract(gPV.getPosition().getY().getValue()));
residuals.setEntry(2, targetPV.getPosition().getZ().subtract(gPV.getPosition().getZ().getValue()));
residuals.setEntry(3, targetPV.getVelocity().getX().subtract(gPV.getVelocity().getX().getValue()));
residuals.setEntry(4, targetPV.getVelocity().getY().subtract(gPV.getVelocity().getY().getValue()));
residuals.setEntry(5, targetPV.getVelocity().getZ().subtract(gPV.getVelocity().getZ().getValue()));
final FieldVector<T> correction = new FieldQRDecomposition<>(jacobian, field.getZero().newInstance(EPS)).
getSolver().
solve(residuals);
// prevent correction to produce invalid values
final FieldKeplerianOrbit<FieldGradient<T>> previous = gElements.getOrbit();
T updatedA;
T updatedE;
double factor = 2;
do {
// loop until eccentricity is valid
factor *= 0.5;
updatedA = previous.getA().getValue().add(correction.getEntry(0).multiply(factor));
updatedE = previous.getE().getValue().add(correction.getEntry(1).multiply(factor));
} while (updatedA.getReal() < 0 || updatedE.getReal() < 0 || updatedE.getReal() >= 1);
// update initial orbit
final FieldKeplerianOrbit<T> updated =
new FieldKeplerianOrbit<>(new FieldKeplerianParameters<>(updatedA,
updatedE,
previous.getI().getValue().add(correction.getEntry(2).multiply(factor)),
previous.getPeriapsisArgument().getValue().add(correction.getEntry(3).multiply(factor)),
previous.getRightAscensionOfAscendingNode().getValue().add(correction.getEntry(4).multiply(factor)),
previous.getMeanAnomaly().getValue().add(correction.getEntry(5).multiply(factor)),
PositionAngleType.MEAN),
previous.getFrame(),
new FieldAbsoluteDate<>(previous.getMu().getValue().getField(),
previous.getDate().toAbsoluteDate()),
previous.getMu().getValue());
gElements = toGradient(nonKeplerianElements, updated, driversFactory);
final double deltaP = FastMath.sqrt(residuals.getEntry(0).getReal() * residuals.getEntry(0).getReal() +
residuals.getEntry(1).getReal() * residuals.getEntry(1).getReal() +
residuals.getEntry(2).getReal() * residuals.getEntry(2).getReal());
final double deltaV = FastMath.sqrt(residuals.getEntry(3).getReal() * residuals.getEntry(3).getReal() +
residuals.getEntry(4).getReal() * residuals.getEntry(4).getReal() +
residuals.getEntry(5).getReal() * residuals.getEntry(5).getReal());
if (deltaP < TOL_P && deltaV < TOL_V) {
break;
}
}
final FieldKeplerianOrbit<FieldGradient<T>> initialOrbit = gElements.getOrbit();
return gElements.toField(new FieldKeplerianOrbit<>(new FieldKeplerianParameters<>(initialOrbit.getA().getValue(),
initialOrbit.getE().getValue(),
initialOrbit.getI().getValue(),
initialOrbit.getPeriapsisArgument().getValue(),
initialOrbit.getRightAscensionOfAscendingNode().getValue(),
initialOrbit.getMeanAnomaly().getValue(),
PositionAngleType.MEAN),
initialOrbit.getFrame(),
new FieldAbsoluteDate<>(initialOrbit.getMu().getValue().getField(),
initialOrbit.getDate().toAbsoluteDate()),
initialOrbit.getMu().getValue()),
nonKeplerianElements.toArray(),
FieldGradient::getValue);
}
/** Compute approximate initial orbit.
* @param <T> type of the field elements
* @param initialState initial state
* @param nonKeplerianElements non-Keplerian orbital elements (the Keplerian orbital elements will be ignored)
* @param frozenEcef inertial frame aligned with Earth Centered Earth Fixed frame at orbit date
* @return approximate initial orbit that generate a state close to {@code initialState}
*/
private static <T extends CalculusFieldElement<T>> FieldKeplerianOrbit<T>
approximateInitialOrbit(final FieldSpacecraftState<T> initialState,
final FieldGnssOrbitalElements<T, ?> nonKeplerianElements,
final Frame frozenEcef) {
// rotate the state to a frame that is inertial but aligned with Earth frame,
// as analytical model is expressed in Earth frame
final FieldPVCoordinates<T> pv = initialState.getPVCoordinates(frozenEcef);
final FieldVector3D<T> p = pv.getPosition();
final FieldVector3D<T> v = pv.getVelocity();
// compute Keplerian orbital parameters
final T rk = p.getNorm();
// compute orbital plane orientation
final FieldVector3D<T> normal = pv.getMomentum().normalize();
final T cosIk = normal.getZ();
final T ik = FieldVector3D.angle(normal, Vector3D.PLUS_K);
// compute position in orbital plane
final T q = FastMath.hypot(normal.getX(), normal.getY());
final T cos = normal.getY().negate().divide(q);
final T sin = normal.getX().divide(q);
final T xk = p.getX().multiply(cos).add(p.getY().multiply(sin));
final T yk = p.getY().multiply(cos).subtract(p.getX().multiply(sin)).divide(cosIk);
// corrected latitude argument
final T uk = FastMath.atan2(yk, xk);
// recover latitude argument before correction, using a fixed-point method
T phi = uk;
for (int i = 0; i < MAX_ITER; ++i) {
final T previous = phi;
final FieldSinCos<T> cs2Phi = FastMath.sinCos(phi.multiply(2));
phi = uk.subtract(cs2Phi.cos().multiply(nonKeplerianElements.getCuc()).add(cs2Phi.sin().multiply(nonKeplerianElements.getCus())));
if (FastMath.abs(phi.subtract(previous).getReal()) <= EPS) {
break;
}
}
final FieldSinCos<T> cs2phi = FastMath.sinCos(phi.multiply(2));
// recover plane orientation before correction
// here, we know that tk = 0 since our orbital elements will be at initial state date
final T i0 = ik.subtract(cs2phi.cos().multiply(nonKeplerianElements.getCic()).add(cs2phi.sin().multiply(nonKeplerianElements.getCis())));
final double toe = nonKeplerianElements.getTimeOfEphemeris().getGnssDate().getSecondsInWeek();
final T om0 = FastMath.atan2(sin, cos).
add(nonKeplerianElements.getAngularVelocity() * toe);
// recover eccentricity and anomaly
final T mu = initialState.getOrbit().getMu();
final T rV2OMu = rk.multiply(v.getNorm2Sq()).divide(mu);
final T sma = rk.divide(rV2OMu.negate().add(2));
final T eCosE = rV2OMu.subtract(1);
final T eSinE = FieldVector3D.dotProduct(p, v).divide(FastMath.sqrt(mu.multiply(sma)));
final T e = FastMath.hypot(eCosE, eSinE);
final T eccentricAnomaly = FastMath.atan2(eSinE, eCosE);
final T aop = phi.subtract(eccentricAnomaly);
final T meanAnomaly = FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, eccentricAnomaly);
return new FieldKeplerianOrbit<>(sma, e, i0, aop, om0, meanAnomaly, PositionAngleType.MEAN,
frozenEcef, initialState.getDate(), mu);
}
/** Convert orbital elements to gradient.
* @param <T> type of the field elements
* @param <O> type of the orbital elements (non-field version)
* @param elements primitive double elements
* @param orbit Keplerian orbit
* @param driversFactory factory for non-Kepleria drivers
* @return converted elements, set up as gradient relative to Keplerian orbit
*/
private static <T extends CalculusFieldElement<T>, O extends GNSSOrbitalElements<O>>
FieldGnssOrbitalElements<FieldGradient<T>, O> toGradient(final FieldGnssOrbitalElements<T, O> elements,
final FieldKeplerianOrbit<T> orbit,
final NonKeplerianDriversFactory driversFactory) {
// build orbit with gradient
final FieldGradient<T> aG = FieldGradient.variable(FREE_PARAMETERS, 0, orbit.getA());
final FieldGradient<T> eG = FieldGradient.variable(FREE_PARAMETERS, 1, orbit.getE());
final FieldGradient<T> iG = FieldGradient.variable(FREE_PARAMETERS, 2, orbit.getI());
final FieldGradient<T> paG = FieldGradient.variable(FREE_PARAMETERS, 3, orbit.getPeriapsisArgument());
final FieldGradient<T> raanG = FieldGradient.variable(FREE_PARAMETERS, 4, orbit.getRightAscensionOfAscendingNode());
final FieldGradient<T> mG = FieldGradient.variable(FREE_PARAMETERS, 5, orbit.getMeanAnomaly());
final FieldKeplerianOrbit<FieldGradient<T>> orbitG =
new FieldKeplerianOrbit<>(new FieldKeplerianParameters<>(aG, eG, iG, paG, raanG, mG,
PositionAngleType.MEAN),
orbit.getFrame(),
new FieldAbsoluteDate<>(FieldGradient.constant(FREE_PARAMETERS, orbit.getMu()).
getField(),
orbit.getDate().toAbsoluteDate()),
FieldGradient.constant(FREE_PARAMETERS, orbit.getMu()));
// convert to GNSS orbital elements
return elements.toField(orbitG,
driversFactory.toGradients(orbit.getMu().getField(), FREE_PARAMETERS),
d -> FieldGradient.constant(FREE_PARAMETERS, d));
}
}