Commit 7cf1d485 authored by Luc Maisonobe's avatar Luc Maisonobe

Added derivatives in orbits.

parent 6fdc8a93
......@@ -141,7 +141,10 @@
</Match>
<Match>
<Class name="org.orekit.orbits.KeplerianOrbit"/>
<Method name ="eMeSinE" params="double" returns="double" />
<Or>
<Method name ="eMeSinE" params="double,double" returns="double" />
<Method name ="eMeSinE" params="org.hipparchus.analysis.differentiation.DerivativeStructure,org.hipparchus.analysis.differentiation.DerivativeStructure" returns="org.hipparchus.analysis.differentiation.DerivativeStructure" />
</Or>
<Bug pattern="FE_FLOATING_POINT_EQUALITY" />
</Match>
......
......@@ -19,8 +19,11 @@ package org.orekit.orbits;
import java.io.Serializable;
import java.util.stream.Stream;
import org.hipparchus.analysis.differentiation.DSFactory;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.exception.LocalizedCoreFormats;
import org.hipparchus.exception.MathIllegalStateException;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Rotation;
import org.hipparchus.geometry.euclidean.threed.RotationConvention;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
......@@ -70,7 +73,13 @@ import org.orekit.utils.TimeStampedPVCoordinates;
public class CartesianOrbit extends Orbit {
/** Serializable UID. */
private static final long serialVersionUID = 20140723L;
private static final long serialVersionUID = 20170414L;
/** Factory for first time derivatives. */
private static final DSFactory FACTORY = new DSFactory(1, 1);
/** Indicator for non-Keplerian derivatives. */
private final transient boolean hasNonKeplerianAcceleration;
/** Underlying equinoctial orbit to which high-level methods are delegated. */
private transient EquinoctialOrbit equinoctial;
......@@ -93,6 +102,7 @@ public class CartesianOrbit extends Orbit {
final Frame frame, final double mu)
throws IllegalArgumentException {
super(pvaCoordinates, frame, mu);
hasNonKeplerianAcceleration = hasNonKeplerianAcceleration(pvaCoordinates, mu);
equinoctial = null;
}
......@@ -122,6 +132,7 @@ public class CartesianOrbit extends Orbit {
*/
public CartesianOrbit(final Orbit op) {
super(op.getPVCoordinates(), op.getFrame(), op.getMu());
hasNonKeplerianAcceleration = op.hasDerivatives();
if (op instanceof EquinoctialOrbit) {
equinoctial = (EquinoctialOrbit) op;
} else if (op instanceof CartesianOrbit) {
......@@ -143,14 +154,47 @@ public class CartesianOrbit extends Orbit {
}
}
/** Get position with derivatives.
* @return position with derivatives
*/
private FieldVector3D<DerivativeStructure> getPositionDS() {
final Vector3D p = getPVCoordinates().getPosition();
final Vector3D v = getPVCoordinates().getVelocity();
return new FieldVector3D<>(FACTORY.build(p.getX(), v.getX()),
FACTORY.build(p.getY(), v.getY()),
FACTORY.build(p.getZ(), v.getZ()));
}
/** Get velocity with derivatives.
* @return velocity with derivatives
*/
private FieldVector3D<DerivativeStructure> getVelocityDS() {
final Vector3D v = getPVCoordinates().getVelocity();
final Vector3D a = getPVCoordinates().getAcceleration();
return new FieldVector3D<>(FACTORY.build(v.getX(), a.getX()),
FACTORY.build(v.getY(), a.getY()),
FACTORY.build(v.getZ(), a.getZ()));
}
/** {@inheritDoc} */
public double getA() {
// lazy evaluation of semi-major axis
final double r = getPVCoordinates().getPosition().getNorm();
final double V2 = getPVCoordinates().getVelocity().getNormSq();
return r / (2 - r * V2 / getMu());
}
/** {@inheritDoc} */
public double getADot() {
if (hasDerivatives()) {
final DerivativeStructure r = getPositionDS().getNorm();
final DerivativeStructure V2 = getVelocityDS().getNormSq();
final DerivativeStructure a = r.divide(r.multiply(V2).divide(getMu()).subtract(2).negate());
return a.getPartialDerivative(1);
} else {
return Double.NaN;
}
}
/** {@inheritDoc} */
public double getE() {
final Vector3D pvP = getPVCoordinates().getPosition();
......@@ -161,23 +205,65 @@ public class CartesianOrbit extends Orbit {
return FastMath.sqrt(eCE * eCE + eSE * eSE);
}
/** {@inheritDoc} */
public double getEDot() {
if (hasDerivatives()) {
final FieldVector3D<DerivativeStructure> pvP = getPositionDS();
final FieldVector3D<DerivativeStructure> pvV = getVelocityDS();
final DerivativeStructure r = getPositionDS().getNorm();
final DerivativeStructure V2 = getVelocityDS().getNormSq();
final DerivativeStructure rV2OnMu = r.multiply(V2).divide(getMu());
final DerivativeStructure a = r.divide(rV2OnMu).subtract(2);
final DerivativeStructure eSE = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(getMu()).sqrt());
final DerivativeStructure eCE = rV2OnMu.subtract(1);
final DerivativeStructure e = eCE.multiply(eCE).add(eSE.multiply(eSE)).sqrt();
return e.getPartialDerivative(1);
} else {
return Double.NaN;
}
}
/** {@inheritDoc} */
public double getI() {
return Vector3D.angle(Vector3D.PLUS_K, getPVCoordinates().getMomentum());
}
/** {@inheritDoc} */
public double getIDot() {
if (hasDerivatives()) {
final FieldVector3D<DerivativeStructure> momentum =
FieldVector3D.crossProduct(getPositionDS(), getVelocityDS());
final DerivativeStructure i = FieldVector3D.angle(Vector3D.PLUS_K, momentum);
return i.getPartialDerivative(1);
} else {
return Double.NaN;
}
}
/** {@inheritDoc} */
public double getEquinoctialEx() {
initEquinoctial();
return equinoctial.getEquinoctialEx();
}
/** {@inheritDoc} */
public double getEquinoctialExDot() {
initEquinoctial();
return equinoctial.getEquinoctialExDot();
}
/** {@inheritDoc} */
public double getEquinoctialEy() {
initEquinoctial();
return equinoctial.getEquinoctialEy();
}
/** {@inheritDoc} */
public double getEquinoctialEyDot() {
initEquinoctial();
return equinoctial.getEquinoctialEyDot();
}
/** {@inheritDoc} */
public double getHx() {
final Vector3D w = getPVCoordinates().getMomentum().normalize();
......@@ -188,6 +274,25 @@ public class CartesianOrbit extends Orbit {
return -w.getY() / (1 + w.getZ());
}
/** {@inheritDoc} */
public double getHxDot() {
if (hasDerivatives()) {
final FieldVector3D<DerivativeStructure> w =
FieldVector3D.crossProduct(getPositionDS(), getVelocityDS()).normalize();
// Check for equatorial retrograde orbit
final double x = w.getX().getValue();
final double y = w.getY().getValue();
final double z = w.getZ().getValue();
if (((x * x + y * y) == 0) && z < 0) {
return Double.NaN;
}
final DerivativeStructure hx = w.getY().negate().divide(w.getZ().add(1));
return hx.getPartialDerivative(1);
} else {
return Double.NaN;
}
}
/** {@inheritDoc} */
public double getHy() {
final Vector3D w = getPVCoordinates().getMomentum().normalize();
......@@ -198,24 +303,66 @@ public class CartesianOrbit extends Orbit {
return w.getX() / (1 + w.getZ());
}
/** {@inheritDoc} */
public double getHyDot() {
if (hasDerivatives()) {
final FieldVector3D<DerivativeStructure> w =
FieldVector3D.crossProduct(getPositionDS(), getVelocityDS()).normalize();
// Check for equatorial retrograde orbit
final double x = w.getX().getValue();
final double y = w.getY().getValue();
final double z = w.getZ().getValue();
if (((x * x + y * y) == 0) && z < 0) {
return Double.NaN;
}
final DerivativeStructure hy = w.getX().divide(w.getZ().add(1));
return hy.getPartialDerivative(1);
} else {
return Double.NaN;
}
}
/** {@inheritDoc} */
public double getLv() {
initEquinoctial();
return equinoctial.getLv();
}
/** {@inheritDoc} */
public double getLvDot() {
initEquinoctial();
return equinoctial.getLvDot();
}
/** {@inheritDoc} */
public double getLE() {
initEquinoctial();
return equinoctial.getLE();
}
/** {@inheritDoc} */
public double getLEDot() {
initEquinoctial();
return equinoctial.getLEDot();
}
/** {@inheritDoc} */
public double getLM() {
initEquinoctial();
return equinoctial.getLM();
}
/** {@inheritDoc} */
public double getLMDot() {
initEquinoctial();
return equinoctial.getLMDot();
}
/** {@inheritDoc} */
public boolean hasDerivatives() {
return hasNonKeplerianAcceleration;
}
/** {@inheritDoc} */
protected TimeStampedPVCoordinates initPVCoordinates() {
// nothing to do here, as the canonical elements are already the cartesian ones
......@@ -524,7 +671,7 @@ public class CartesianOrbit extends Orbit {
private static class DTO implements Serializable {
/** Serializable UID. */
private static final long serialVersionUID = 20140723L;
private static final long serialVersionUID = 20170414L;
/** Double values. */
private double[] d;
......
......@@ -26,14 +26,15 @@ import org.hipparchus.linear.FieldMatrix;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathArrays;
import org.orekit.errors.OrekitIllegalArgumentException;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitIllegalArgumentException;
import org.orekit.errors.OrekitInternalError;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.Frame;
import org.orekit.frames.Transform;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.time.FieldTimeInterpolable;
import org.orekit.utils.FieldPVCoordinates;
import org.orekit.utils.FieldPVCoordinatesProvider;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
......@@ -60,6 +61,7 @@ import org.orekit.utils.TimeStampedPVCoordinates;
* @author Guylaine Prat
* @author Fabien Maussion
* @author V&eacute;ronique Pommier-Maurussane
* @since 9.0
*/
public abstract class FieldOrbit<T extends RealFieldElement<T>> implements FieldPVCoordinatesProvider<T>, FieldTimeInterpolable<FieldOrbit<T>, T> {
......@@ -151,6 +153,33 @@ public abstract class FieldOrbit<T extends RealFieldElement<T>> implements Field
this.frame = frame;
}
/** Check if Cartesian coordinates include non-Keplerian acceleration.
* @param pva Cartesian coordinates
* @param mu central attraction coefficient
* @param <T> type of the field elements
* @return true if Cartesian coordinates include non-Keplerian acceleration
*/
protected static <T extends RealFieldElement<T>> boolean hasNonKeplerianAcceleration(final FieldPVCoordinates<T> pva, final double mu) {
final FieldVector3D<T> a = pva.getAcceleration();
if (a == null) {
return false;
}
final FieldVector3D<T> p = pva.getPosition();
final T r2 = p.getNormSq();
final T r = r2.sqrt();
final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(-mu), p);
if (a.getNorm().getReal() > 1.0e-9 * keplerianAcceleration.getNorm().getReal()) {
// we have a relevant acceleration, we can compute derivatives
return true;
} else {
// the provided acceleration is either too small to be reliable (probably even 0), or NaN
return false;
}
}
/** Get the orbit type.
* @return orbit type
*/
......@@ -187,41 +216,105 @@ public abstract class FieldOrbit<T extends RealFieldElement<T>> implements Field
*/
public abstract T getA();
/** Get the semi-major axis derivative.
* <p>Note that the semi-major axis is considered negative for hyperbolic orbits.</p>
* <p>
* If the orbit was created without derivatives, the value returned is null.
* </p>
* @return semi-major axis derivative (m/s)
*/
public abstract T getADot();
/** Get the first component of the equinoctial eccentricity vector.
* @return first component of the equinoctial eccentricity vector
*/
public abstract T getEquinoctialEx();
/** Get the first component of the equinoctial eccentricity vector.
* <p>
* If the orbit was created without derivatives, the value returned is null.
* </p>
* @return first component of the equinoctial eccentricity vector
*/
public abstract T getEquinoctialExDot();
/** Get the second component of the equinoctial eccentricity vector.
* @return second component of the equinoctial eccentricity vector
*/
public abstract T getEquinoctialEy();
/** Get the second component of the equinoctial eccentricity vector.
* <p>
* If the orbit was created without derivatives, the value returned is null.
* </p>
* @return second component of the equinoctial eccentricity vector
*/
public abstract T getEquinoctialEyDot();
/** Get the first component of the inclination vector.
* @return first component of the inclination vector
*/
public abstract T getHx();
/** Get the first component of the inclination vector derivative.
* <p>
* If the orbit was created without derivatives, the value returned is null.
* </p>
* @return first component of the inclination vector derivative
*/
public abstract T getHxDot();
/** Get the second component of the inclination vector.
* @return second component of the inclination vector
*/
public abstract T getHy();
/** Get the second component of the inclination vector derivative.
* <p>
* </p>
* @return second component of the inclination vector derivative
*/
public abstract T getHyDot();
/** Get the eccentric longitude argument.
* @return E + ω + Ω eccentric longitude argument (rad)
*/
public abstract T getLE();
/** Get the eccentric longitude argument derivative.
* <p>
* If the orbit was created without derivatives, the value returned is null.
* </p>
* @return d(E + ω + Ω)/dt eccentric longitude argument derivative (rad/s)
*/
public abstract T getLEDot();
/** Get the true longitude argument.
* @return v + ω + Ω true longitude argument (rad)
*/
public abstract T getLv();
/** Get the true longitude argument derivative.
* <p>
* If the orbit was created without derivatives, the value returned is null.
* </p>
* @return d(v + ω + Ω)/dt true longitude argument derivative (rad/s)
*/
public abstract T getLvDot();
/** Get the mean longitude argument.
* @return M + ω + Ω mean longitude argument (rad)
*/
public abstract T getLM();
/** Get the mean longitude argument derivative.
* <p>
* If the orbit was created without derivatives, the value returned is null.
* </p>
* @return d(M + ω + Ω)/dt mean longitude argument derivative (rad/s)
*/
public abstract T getLMDot();
// Additional orbital elements
/** Get the eccentricity.
......@@ -229,15 +322,47 @@ public abstract class FieldOrbit<T extends RealFieldElement<T>> implements Field
*/
public abstract T getE();
/** Get the eccentricity derivative.
* <p>
* If the orbit was created without derivatives, the value returned is null.
* </p>
* @return eccentricity derivative
*/
public abstract T getEDot();
/** Get the inclination.
* <p>
* If the orbit was created without derivatives, the value returned is null.
* </p>
* @return inclination (rad)
*/
public abstract T getI();
/** Get the inclination derivative.
* @return inclination derivative (rad/s)
*/
public abstract T getIDot();
/** Get the central acceleration constant.
* @return central acceleration constant
*/
/** Check if orbit includes derivatives.
* @return true if orbit includes derivatives
* @see #getADot()
* @see #getEquinoctialExDot()
* @see #getEquinoctialEyDot()
* @see #getHxDot()
* @see #getHyDot()
* @see #getLEDot()
* @see #getLvDot()
* @see #getLMDot()
* @see #getEDot()
* @see #getIDot()
* @since 9.0
*/
public abstract boolean hasDerivatives();
public double getMu() {
return mu;
}
......
This diff is collapsed.
......@@ -22,6 +22,7 @@ import java.util.ArrayList;
import java.util.List;
import java.util.SortedSet;
import org.orekit.attitudes.Attitude;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitInternalError;
......@@ -114,28 +115,53 @@ public class KeplerianPropagator extends AbstractAnalyticalPropagator implements
super(attitudeProv);
// ensure the orbit use the specified mu
final OrbitType type = initialOrbit.getType();
final double[] stateVector = new double[6];
final double[] stateVectorDot = new double[6];
type.mapOrbitToArray(initialOrbit, PositionAngle.TRUE, stateVector, stateVectorDot);
final Orbit orbit = type.mapArrayToOrbit(stateVector, stateVectorDot, PositionAngle.TRUE,
initialOrbit.getDate(), mu, initialOrbit.getFrame());
// ensure the orbit use the specified mu and has no non-Keplerian derivatives
initialState = fixState(initialOrbit,
getAttitudeProvider().getAttitude(initialOrbit,
initialOrbit.getDate(),
initialOrbit.getFrame()),
mass, mu);
states = new TimeSpanMap<SpacecraftState>(initialState);
super.resetInitialState(initialState);
resetInitialState(new SpacecraftState(orbit,
getAttitudeProvider().getAttitude(orbit,
orbit.getDate(),
orbit.getFrame()),
mass));
}
/** Fix state to use a specified mu and remove derivatives.
* <p>
* This ensures the propagation model (which is based on calling
* {@link Orbit#shiftedBy(double)}) is Keplerian only and uses a specified mu.
* </p>
* @param orbit orbit to fix
* @param attitude current attitude
* @param mass current mass
* @param mu gravity coefficient to use
* @return fixed orbit
*/
private SpacecraftState fixState(final Orbit orbit, final Attitude attitude, final double mass,
final double mu) {
final OrbitType type = orbit.getType();
final double[] stateVector = new double[6];
type.mapOrbitToArray(orbit, PositionAngle.TRUE, stateVector, null);
final Orbit fixedOrbit = type.mapArrayToOrbit(stateVector, null, PositionAngle.TRUE,
orbit.getDate(), mu, orbit.getFrame());
return new SpacecraftState(fixedOrbit, attitude, mass);
}
/** {@inheritDoc} */
public void resetInitialState(final SpacecraftState state)
throws OrekitException {
super.resetInitialState(state);
initialState = state;
// ensure the orbit use the specified mu and has no non-Keplerian derivatives
final double mu = initialState == null ? state.getMu() : initialState.getMu();
final SpacecraftState fixedState = fixState(state.getOrbit(),
state.getAttitude(),
state.getMass(),
mu);
initialState = fixedState;
states = new TimeSpanMap<SpacecraftState>(initialState);
super.resetInitialState(fixedState);
}
/** {@inheritDoc} */
......
......@@ -485,7 +485,7 @@ public class TurnAroundRangeAnalyticTest {
else {
// Without modifier
if (!isModifier) {
refErrorsPMedian = 1.1e-09;
refErrorsPMedian = 1.2e-09;
refErrorsPMean = 2.6e-09;
refErrorsPMax = 9.0e-08;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment