GLONASSNumericalPropagator.java
/* Copyright 2002-2020 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.numerical;
import java.util.Arrays;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.ode.ODEIntegrator;
import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaIntegrator;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathUtils;
import org.orekit.annotation.DefaultDataContext;
import org.orekit.attitudes.Attitude;
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.gnss.GLONASSEphemeris;
import org.orekit.orbits.CartesianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.PropagationType;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.analytical.gnss.GLONASSOrbitalElements;
import org.orekit.propagation.integration.AbstractIntegratedPropagator;
import org.orekit.propagation.integration.StateMapper;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.GLONASSDate;
import org.orekit.utils.AbsolutePVCoordinates;
import org.orekit.utils.Constants;
import org.orekit.utils.IERSConventions;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
/**
* This class propagates GLONASS orbits using numerical integration.
* <p>
* As recommended by the GLONASS Interface Control Document (ICD),
* a {@link ClassicalRungeKuttaIntegrator 4th order Runge-Kutta technique}
* shall be used to integrate the equations.
* </p>
* <p>
* Classical used of this orbit propagator is to compute GLONASS satellite
* coordinates from the navigation message.
* </p>
* <p>
* If the projections of luni-solar accelerations to axes of
* Greenwich geocentric coordinates {@link GLONASSOrbitalElements#getXDotDot() X''(tb)},
* {@link GLONASSOrbitalElements#getYDotDot() Y''(tb)} and {@link GLONASSOrbitalElements#getZDotDot() Z''(tb)}
* are available in the navigation message; a transformation is performed to convert these
* accelerations into the correct coordinate system. In the case where they are not
* available into the navigation message, these accelerations are computed.
* </p>
*
* @see <a href="http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD-GLONASS-CDMA-General.-Edition-1.0-2016.pdf">
* GLONASS Interface Control Document</a>
*
* @author Bryan Cazabonne
*
*/
public class GLONASSNumericalPropagator extends AbstractIntegratedPropagator {
/** Second degree 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;
/** Value of the Earth's rotation rate in rad/s (See Ref). */
private static final double GLONASS_AV = 7.2921151467e-5;
// 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 GLONASS orbital elements used. */
private final GLONASSOrbitalElements glonassOrbit;
/** Initial date in GLONASS form. */
private final GLONASSDate initDate;
/** The spacecraft mass (kg). */
private final double mass;
/** The ECI frame used for GLONASS propagation. */
private final Frame eci;
/** Direction cosines and distance of perturbing body: Moon.
* <p>
* <ul>
* <li>double[0] = ξ<sub>m</sub></li>
* <li>double[1] = η<sub>m</sub></li>
* <li>double[2] = ψ<sub>m</sub></li>
* <li>double[3] = r<sub>m</sub></li>
* </ul>
* </p>
*/
private double[] moonElements;
/** Direction cosines and distance of perturbing body: Sun.
* <p>
* <ul>
* <li>double[0] = ξ<sub>s</sub></li>
* <li>double[1] = η<sub>s</sub></li>
* <li>double[2] = ψ<sub>s</sub></li>
* <li>double[3] = r<sub>s</sub></li>
* </ul>
* </p>
*/
private double[] sunElements;
/** Flag for availability of projections of acceleration transmitted within the navigation message. */
private final boolean isAccAvailable;
/** Data context used for propagation. */
private final DataContext dataContext;
/**
* This nested class aims at building a GLONASSNumericalPropagator.
* <p>It implements the classical builder pattern.</p>
*
*/
public static class Builder {
// Required parameter
/** The GLONASS orbital elements. */
private final GLONASSEphemeris orbit;
/** The 4th order Runge-Kutta integrator. */
private final ClassicalRungeKuttaIntegrator integrator;
/** Flag for availability of projections of acceleration transmitted within the navigation message. */
private final boolean isAccAvailable;
// Optional parameters
/** The attitude provider. */
private AttitudeProvider attitudeProvider;
/** The mass. */
private double mass = DEFAULT_MASS;
/** The ECI frame. */
private Frame eci = null;
/** Data context for the propagator. */
private DataContext dataContext;
/**
* Initializes the builder.
* <p>The attitude provider is set by default to the
* {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the
* default data context.<br>
* The mass is set by default to the
* {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.<br>
* The data context is by default to the
* {@link DataContext#getDefault() default data context}.<br>
* The ECI frame is set by default to the
* {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data
* context.<br>
* </p>
*
* @param integrator 4th order Runge-Kutta as recommended by GLONASS ICD
* @param glonassOrbElt the GLONASS orbital elements to be used by the GLONASSNumericalPropagator.
* @param isAccAvailable flag for availability of the projections of accelerations transmitted within
* the navigation message
* @see #attitudeProvider(AttitudeProvider provider)
* @see #mass(double mass)
* @see #eci(Frame inertial)
*/
@DefaultDataContext
public Builder(final ClassicalRungeKuttaIntegrator integrator,
final GLONASSEphemeris glonassOrbElt,
final boolean isAccAvailable) {
this.isAccAvailable = isAccAvailable;
this.integrator = integrator;
this.orbit = glonassOrbElt;
this.dataContext = DataContext.getDefault();
this.eci = dataContext.getFrames().getEME2000();
attitudeProvider = Propagator.getDefaultLaw(dataContext.getFrames());
}
/**
* Sets the attitude provider.
*
* @param userProvider the attitude provider
* @return the updated builder
*/
public Builder attitudeProvider(final AttitudeProvider userProvider) {
this.attitudeProvider = userProvider;
return this;
}
/**
* Sets the mass.
*
* @param userMass the mass (in kg)
* @return the updated builder
*/
public Builder 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 builder
*/
public Builder eci(final Frame inertial) {
this.eci = inertial;
return this;
}
/**
* Sets the data context used by the propagator. Does not update the ECI frame
* which must be done separately using {@link #eci(Frame)}.
*
* @param context used for propagation.
* @return the updated builder.
*/
public Builder dataContext(final DataContext context) {
this.dataContext = context;
return this;
}
/**
* Finalizes the build.
*
* @return the built GPSPropagator
*/
public GLONASSNumericalPropagator build() {
return new GLONASSNumericalPropagator(this);
}
}
/**
* Private constructor.
*
* @param builder the builder
*/
public GLONASSNumericalPropagator(final Builder builder) {
super(builder.integrator, PropagationType.MEAN);
this.dataContext = builder.dataContext;
this.isAccAvailable = builder.isAccAvailable;
// Stores the GLONASS orbital elements
this.glonassOrbit = builder.orbit;
// Sets the Earth Centered Inertial frame
this.eci = builder.eci;
// Sets the mass
this.mass = builder.mass;
this.initDate = new GLONASSDate(
glonassOrbit.getDate(),
dataContext.getTimeScales().getGLONASS());
// Initialize state mapper
initMapper();
setInitialState();
setAttitudeProvider(builder.attitudeProvider);
setOrbitType(OrbitType.CARTESIAN);
// It is not meaningful for propagation in Cartesian parameters
setPositionAngleType(PositionAngle.TRUE);
setMu(GLONASSOrbitalElements.GLONASS_MU);
// As recommended by GLONASS ICD (2016), the direction cosines and distance
// of perturbing body are calculated one time (at tb).
if (!isAccAvailable) {
computeMoonElements(initDate);
computeSunElements(initDate);
}
}
/**
* Gets the underlying GLONASS orbital elements.
*
* @return the underlying GLONASS orbital elements
*/
public GLONASSOrbitalElements getGLONASSOrbitalElements() {
return glonassOrbit;
}
/** {@inheritDoc} */
@Override
public SpacecraftState propagate(final AbsoluteDate date) {
// Spacecraft state in inertial frame
final SpacecraftState stateInInertial = super.propagate(date);
// Build the spacecraft state in inertial frame
final PVCoordinates pvInPZ90 = getPVInPZ90(stateInInertial);
final AbsolutePVCoordinates absPV = new AbsolutePVCoordinates(
dataContext.getFrames().getPZ9011(IERSConventions.IERS_2010, true),
stateInInertial.getDate(), pvInPZ90);
final TimeStampedPVCoordinates pvInInertial = absPV.getPVCoordinates(eci);
final SpacecraftState transformedState = new SpacecraftState(new CartesianOrbit(pvInInertial, eci, pvInInertial.getDate(), GLONASSOrbitalElements.GLONASS_MU),
stateInInertial.getAttitude(),
stateInInertial.getMass(), stateInInertial.getAdditionalStates());
return transformedState;
}
/**
* Set the initial state.
* <p>
* The initial conditions on position and velocity are in the ECEF coordinate system PZ-90.
* Previous to orbit integration, they must be transformed to an absolute inertial coordinate system.
* </p>
*/
private void setInitialState() {
// Transform initial PV coordinates to an absolute inertial coordinate system.
final PVCoordinates pvInInertial = getPVInInertial(initDate);
// Create a new orbit
final Orbit orbit = new CartesianOrbit(pvInInertial,
eci, initDate.getDate(),
GLONASSOrbitalElements.GLONASS_MU);
// Reset the initial state to apply the transformation
resetInitialState(new SpacecraftState(orbit, mass));
}
/**
* This method computes the direction cosines and the distance used to
* compute the gravitational perturbations of the Moon.
*
* @param date the computation date in GLONASS scale
*/
private void computeMoonElements(final GLONASSDate date) {
moonElements = new double[4];
// Constants
// Semi-major axis of the Moon's orbit (m)
final double am = 3.84385243e8;
// The Moon's orbit eccentricity
final double em = 0.054900489;
// Mean inclination of the Moon's orbit to the ecliptic (rad)
final double im = 0.0898041080;
// Computed parameters
// Time from epoch 2000 to the instant tb of GLONASS ephemeris broadcast
final double dtoJD = (glonassOrbit.getTime() - 10800.) / Constants.JULIAN_DAY;
final double t = (date.getJD0() + dtoJD - 2451545.0) / 36525;
final double t2 = t * t;
// Mean inclination of Earth equator to ecliptic (rad)
final double eps = 0.4090926006 - 0.0002270711 * t;
// Mean longitude of the Moon's orbit perigee (rad)
final double gammaM = 1.4547885346 + 71.0176852437 * t - 0.0001801481 * t2;
// Mean longitude of the ascending node of the Moon (rad)
final double omegaM = 2.1824391966 - 33.7570459536 * t + 0.0000362262 * t2;
// Mean anomaly of the Moon (rad)
final double qm = 2.3555557435 + 8328.6914257190 * t + 0.0001545547 * t2;
// Commons parameters
final double cosOm = FastMath.cos(omegaM);
final double sinOm = FastMath.sin(omegaM);
final double cosIm = FastMath.cos(im);
final double sinIm = FastMath.sin(im);
final double cosEs = FastMath.cos(eps);
final double sinEs = FastMath.sin(eps);
final double cosGm = FastMath.cos(gammaM);
final double sinGm = FastMath.sin(gammaM);
// Intermediate parameters
final double psiStar = cosOm * sinIm;
final double etaStar = sinOm * sinIm;
final double epsStar = 1. - cosOm * cosOm * (1. - cosIm);
final double eps11 = sinOm * cosOm * (1. - cosIm);
final double eps12 = 1. - sinOm * sinOm * (1. - cosIm);
final double eta11 = epsStar * cosEs - psiStar * sinEs;
final double eta12 = eps11 * cosEs + etaStar * sinEs;
final double psi11 = epsStar * sinEs + psiStar * cosEs;
final double psi12 = eps11 * sinEs - etaStar * cosEs;
// Eccentric Anomaly
final double ek = getEccentricAnomaly(qm, em);
// True Anomaly
final double vk = getTrueAnomaly(ek, em);
final double sinVk = FastMath.sin(vk);
final double cosVk = FastMath.cos(vk);
// Direction cosine
final double epsM = eps11 * (sinVk * cosGm + cosVk * sinGm) + eps12 * (cosVk * cosGm - sinVk * sinGm);
final double etaM = eta11 * (sinVk * cosGm + cosVk * sinGm) + eta12 * (cosVk * cosGm - sinVk * sinGm);
final double psiM = psi11 * (sinVk * cosGm + cosVk * sinGm) + psi12 * (cosVk * cosGm - sinVk * sinGm);
// Distance
final double rm = am * (1. - em * FastMath.cos(ek));
moonElements[0] = epsM;
moonElements[1] = etaM;
moonElements[2] = psiM;
moonElements[3] = rm;
}
/**
* This method computes the direction cosines and the distance used to
* compute the gravitational perturbations of the Sun.
*
* @param date the computation date in GLONASS scale
*/
private void computeSunElements(final GLONASSDate date) {
sunElements = new double[4];
// Constants
// Major semi-axis of the Earth’s orbit around the Sun (m)
final double as = 1.49598e11;
// The eccentricity of the Earth’s orbit around the Sun
final double es = 0.016719;
// Computed parameters
// Time from epoch 2000 to the instant tb of GLONASS ephemeris broadcast
final double dtoJD = (glonassOrbit.getTime() - 10800.) / Constants.JULIAN_DAY;
final double t = (date.getJD0() + dtoJD - 2451545.0) / 36525;
final double t2 = t * t;
// Mean inclination of Earth equator to ecliptic (rad)
final double eps = 0.4090926006 - 0.0002270711 * t;
// Mean tropic longitude of the Sun orbit perigee (rad)
final double ws = -7.6281824375 + 0.0300101976 * t + 0.0000079741 * t2;
// Mean anomaly of the Sun (rad)
final double qs = 6.2400601269 + 628.3019551714 * t - 0.0000026820 * t2;
// Eccentric Anomaly
final double ek = getEccentricAnomaly(qs, es);
// True Anomaly
final double vk = getTrueAnomaly(ek, es);
final double sinVk = FastMath.sin(vk);
final double cosVk = FastMath.cos(vk);
// Commons parameters
final double cosWs = FastMath.cos(ws);
final double sinWs = FastMath.sin(ws);
final double cosEs = FastMath.cos(eps);
final double sinEs = FastMath.sin(eps);
// Direction cosine
final double epsS = cosVk * cosWs - sinVk * sinWs;
final double etaS = cosEs * (sinVk * cosWs + cosVk * sinWs);
final double psiS = sinEs * (sinVk * cosWs + cosVk * sinWs);
// Distance
final double rs = as * (1. - es * FastMath.cos(ek));
sunElements[0] = epsS;
sunElements[1] = etaS;
sunElements[2] = psiS;
sunElements[3] = rs;
}
/**
* Computes the elliptic eccentric anomaly from the mean anomaly.
* <p>
* The algorithm used here for solving 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
* </p>
* <p>It has been copied from the OREKIT library (KeplerianOrbit class).</p>
*
* @param M mean anomaly (rad)
* @param e eccentricity
* @return E the eccentric anomaly
*/
private double getEccentricAnomaly(final double M, final double e) {
// reduce M to [-PI PI) interval
final double reducedM = MathUtils.normalizeAngle(M, 0.0);
// compute start value according to A. W. Odell and R. H. Gooding S12 starter
double E;
if (FastMath.abs(reducedM) < 1.0 / 6.0) {
E = reducedM + e * (FastMath.cbrt(6 * reducedM) - reducedM);
} else {
if (reducedM < 0) {
final double w = FastMath.PI + reducedM;
E = reducedM + e * (A * w / (B - w) - FastMath.PI - reducedM);
} else {
final double w = FastMath.PI - reducedM;
E = reducedM + e * (FastMath.PI - A * w / (B - w) - reducedM);
}
}
final double e1 = 1 - e;
final boolean noCancellationRisk = (e1 + E * E / 6) >= 0.1;
// perform two iterations, each consisting of one Halley step and one Newton-Raphson step
for (int j = 0; j < 2; ++j) {
final double f;
double fd;
final double fdd = e * FastMath.sin(E);
final double fddd = e * FastMath.cos(E);
if (noCancellationRisk) {
f = (E - fdd) - reducedM;
fd = 1 - fddd;
} else {
f = eMeSinE(E, e) - reducedM;
final double s = FastMath.sin(0.5 * E);
fd = e1 + 2 * e * s * s;
}
final double dee = f * fd / (0.5 * f * fdd - fd * fd);
// update eccentric anomaly, using expressions that limit underflow problems
final double w = fd + 0.5 * dee * (fdd + dee * fddd / 3);
fd += dee * (fdd + 0.5 * dee * fddd);
E -= (f - dee * (fd - w)) / fd;
}
// expand the result back to original range
E += M - reducedM;
return E;
}
/**
* Accurate computation of E - e sin(E).
*
* @param E eccentric anomaly
* @param e eccentricity
* @return E - e sin(E)
*/
private static double eMeSinE(final double E, final double e) {
double x = (1 - e) * FastMath.sin(E);
final double mE2 = -E * E;
double term = E;
double d = 0;
// the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
for (double x0 = Double.NaN; !Double.valueOf(x).equals(Double.valueOf(x0));) {
d += 2;
term *= mE2 / (d * (d + 1));
x0 = x;
x = x - term;
}
return x;
}
/**
* Get true anomaly from eccentric anomaly and eccentricity.
*
* @param ek the eccentric anomaly (rad)
* @param ecc the eccentricity
* @return the true anomaly (rad)
*/
private double getTrueAnomaly(final double ek, final double ecc) {
final double svk = FastMath.sqrt(1. - ecc * ecc) * FastMath.sin(ek);
final double cvk = FastMath.cos(ek) - ecc;
return FastMath.atan2(svk, cvk);
}
/**
* This method transforms the PV coordinates obtained after the numerical
* integration in the ECEF PZ-90.
*
* @param state spacecraft state after integration
* @return the PV coordinates in the ECEF PZ-90.
*/
public PVCoordinates getPVInPZ90(final SpacecraftState state) {
// Compute time difference between start date and end date
final double dt = state.getDate().durationFrom(initDate.getDate());
// Position and velocity vectors
final PVCoordinates pv = state.getPVCoordinates();
final Vector3D pos = pv.getPosition();
final Vector3D vel = pv.getVelocity();
// Components of position and velocity vectors
final double x0 = pos.getX();
final double y0 = pos.getY();
final double z0 = pos.getZ();
final double vx0 = vel.getX();
final double vy0 = vel.getY();
final double vz0 = vel.getZ();
// Greenwich Mean Sidereal Time (GMST)
final GLONASSDate gloDate = new GLONASSDate(
state.getDate(),
dataContext.getTimeScales().getGLONASS());
final double gmst = gloDate.getGMST();
final double ti = glonassOrbit.getTime() + dt;
// We use the GMST instead of the GMT as it is recommended into GLONASS ICD (2016)
final double s = gmst + GLONASS_AV * (ti - 10800.);
// Commons Parameters
final double cosS = FastMath.cos(s);
final double sinS = FastMath.sin(s);
// Transformed coordinates
final double x = x0 * cosS + y0 * sinS;
final double y = -x0 * sinS + y0 * cosS;
final double z = z0;
final double vx = vx0 * cosS + vy0 * sinS + GLONASS_AV * y;
final double vy = -vx0 * sinS + vy0 * cosS - GLONASS_AV * x;
final double vz = vz0;
// Transformed orbit
return new PVCoordinates(new Vector3D(x, y, z),
new Vector3D(vx, vy, vz));
}
/**
* This method computes the PV coordinates of the spacecraft center of mass.
* The returned PV are expressed in inertial coordinates system at the instant tb.
*
* @param date the computation date in GLONASS scale
* @return the PV Coordinates in inertial coordinates system
*/
private PVCoordinates getPVInInertial(final GLONASSDate date) {
// Greenwich Mean Sidereal Time (GMST)
final double gmst = date.getGMST();
final double time = glonassOrbit.getTime();
final double dt = time - 10800.;
// We use the GMST instead of the GMT as it is recommended into GLONASS ICD (2016)
final double s = gmst + GLONASS_AV * dt;
// Commons Parameters
final double cosS = FastMath.cos(s);
final double sinS = FastMath.sin(s);
// PV coordinates in inertial frame
final double x0 = glonassOrbit.getX() * cosS - glonassOrbit.getY() * sinS;
final double y0 = glonassOrbit.getX() * sinS + glonassOrbit.getY() * cosS;
final double z0 = glonassOrbit.getZ();
final double vx0 = glonassOrbit.getXDot() * cosS - glonassOrbit.getYDot() * sinS - GLONASS_AV * y0;
final double vy0 = glonassOrbit.getXDot() * sinS + glonassOrbit.getYDot() * cosS + GLONASS_AV * x0;
final double vz0 = glonassOrbit.getZDot();
return new PVCoordinates(new Vector3D(x0, y0, z0),
new Vector3D(vx0, vy0, vz0));
}
@Override
protected StateMapper createMapper(final AbsoluteDate referenceDate, final double mu,
final OrbitType orbitType, final PositionAngle positionAngleType,
final AttitudeProvider attitudeProvider, final Frame frame) {
return new Mapper(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
}
/** Internal mapper. */
private static class Mapper extends StateMapper {
/**
* Simple constructor.
*
* @param referenceDate reference date
* @param mu central attraction coefficient (m³/s²)
* @param orbitType orbit type to use for mapping
* @param positionAngleType angle type to use for propagation
* @param attitudeProvider attitude provider
* @param frame inertial frame
*/
Mapper(final AbsoluteDate referenceDate, final double mu,
final OrbitType orbitType, final PositionAngle positionAngleType,
final AttitudeProvider attitudeProvider, final Frame frame) {
super(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
}
@Override
public SpacecraftState mapArrayToState(final AbsoluteDate date, final double[] y,
final double[] yDot, final PropagationType type) {
// The parameter meanOnly is ignored for the GLONASS Propagator
final double mass = y[6];
if (mass <= 0.0) {
throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE, mass);
}
final Orbit orbit = getOrbitType().mapArrayToOrbit(y, yDot, getPositionAngleType(), date, getMu(), getFrame());
final Attitude attitude = getAttitudeProvider().getAttitude(orbit, date, getFrame());
return new SpacecraftState(orbit, attitude, mass);
}
@Override
public void mapStateToArray(final SpacecraftState state, final double[] y,
final double[] yDot) {
getOrbitType().mapOrbitToArray(state.getOrbit(), getPositionAngleType(), y, yDot);
y[6] = state.getMass();
}
}
@Override
protected MainStateEquations getMainStateEquations(final ODEIntegrator integ) {
return new Main();
}
/** Internal class for orbital parameters integration. */
private class Main implements MainStateEquations {
/** Derivatives array. */
private final double[] yDot;
/**
* Simple constructor.
*/
Main() {
yDot = new double[7];
}
@Override
public double[] computeDerivatives(final SpacecraftState state) {
// Date in Glonass form
final GLONASSDate gloDate = new GLONASSDate(
state.getDate(),
dataContext.getTimeScales().getGLONASS());
// Position and Velocity vectors
final Vector3D vel = state.getPVCoordinates().getVelocity();
final Vector3D pos = state.getPVCoordinates().getPosition();
Arrays.fill(yDot, 0.0);
// dPos/dt = Vel
yDot[0] += vel.getX();
yDot[1] += vel.getY();
yDot[2] += vel.getZ();
// Components of position and velocity vectors
final double x0 = pos.getX();
final double y0 = pos.getY();
final double z0 = pos.getZ();
// Normalized values
final double r = pos.getNorm();
final double r2 = r * r;
final double oor = 1. / r;
final double oor2 = 1. / r2;
final double x = x0 * oor;
final double y = y0 * oor;
final double z = z0 * oor;
final double g = GLONASSOrbitalElements.GLONASS_MU * oor2;
final double ro = GLONASS_EARTH_EQUATORIAL_RADIUS * oor;
yDot[3] += x * (-g + (-1.5 * GLONASS_J20 * g * ro * ro * (1. - 5. * z * z)));
yDot[4] += y * (-g + (-1.5 * GLONASS_J20 * g * ro * ro * (1. - 5. * z * z)));
yDot[5] += z * (-g + (-1.5 * GLONASS_J20 * g * ro * ro * (3. - 5. * z * z)));
// Luni-Solar contribution
final Vector3D acc;
if (isAccAvailable) {
acc = getLuniSolarPerturbations(gloDate);
} else {
final Vector3D accMoon = computeLuniSolarPerturbations(
state, moonElements[0], moonElements[1], moonElements[2],
moonElements[3],
dataContext.getCelestialBodies().getMoon().getGM());
final Vector3D accSun = computeLuniSolarPerturbations(
state,
sunElements[0], sunElements[1], sunElements[2],
sunElements[3],
dataContext.getCelestialBodies().getSun().getGM());
acc = accMoon.add(accSun);
}
yDot[3] += acc.getX();
yDot[4] += acc.getY();
yDot[5] += acc.getZ();
return yDot.clone();
}
/**
* This method computes the accelerations induced by gravitational
* perturbations of the Sun and the Moon if they are not available into
* the navigation message data.
*
* @param state current state
* @param eps first direction cosine
* @param eta second direction cosine
* @param psi third direction cosine
* @param r distance of perturbing body
* @param g body gravitational field constant
* @return a vector containing the accelerations
*/
private Vector3D computeLuniSolarPerturbations(final SpacecraftState state, final double eps,
final double eta, final double psi,
final double r, final double g) {
// Current pv coordinates
final PVCoordinates pv = state.getPVCoordinates();
final double oor = 1. / r;
final double oor2 = oor * oor;
// Normalized variable
final double x = pv.getPosition().getX() * oor;
final double y = pv.getPosition().getY() * oor;
final double z = pv.getPosition().getZ() * oor;
final double gm = g * oor2;
final double epsmX = eps - x;
final double etamY = eta - y;
final double psimZ = psi - z;
final Vector3D vector = new Vector3D(epsmX, etamY, psimZ);
final double d2 = vector.getNormSq();
final double deltaM = FastMath.sqrt(d2) * d2;
// Accelerations
final double accX = gm * ((epsmX / deltaM) - eps);
final double accY = gm * ((etamY / deltaM) - eta);
final double accZ = gm * ((psimZ / deltaM) - psi);
return new Vector3D(accX, accY, accZ);
}
/**
* Get the accelerations induced by gravitational
* perturbations of the Sun and the Moon in a geocentric
* coordinate system.
* <p>
* The accelerations are obtained using projections of accelerations
* transmitted within navigation message data.
* </p>
*
* @param date the computation date in GLONASS scale
* @return a vector containing the sum of both accelerations
*/
private Vector3D getLuniSolarPerturbations(final GLONASSDate date) {
// Greenwich Mean Sidereal Time (GMST)
final double gmst = date.getGMST();
final double time = glonassOrbit.getTime();
final double dt = time - 10800.;
// We use the GMST instead of the GMT as it is recommended into GLONASS ICD (see Ref)
final double s = gmst + GLONASS_AV * dt;
// Commons Parameters
final double cosS = FastMath.cos(s);
final double sinS = FastMath.sin(s);
// Accelerations
final double accX = glonassOrbit.getXDotDot() * cosS - glonassOrbit.getYDotDot() * sinS;
final double accY = glonassOrbit.getXDotDot() * sinS + glonassOrbit.getYDotDot() * cosS;
final double accZ = glonassOrbit.getZDotDot();
return new Vector3D(accX, accY, accZ);
}
}
}