Skip to content
Snippets Groups Projects
Commit 6f44b586 authored by Luc Maisonobe's avatar Luc Maisonobe
Browse files

Improved detection of out of range dates.

parent 92223359
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
core/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java
+ 65
− 74
View file @ 6f44b586
......@@ -20,7 +20,6 @@ import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath;
import org.orekit.errors.OrekitException;
import org.orekit.frames.Transform;
import org.orekit.rugged.utils.SpacecraftToObservedBody;
import org.orekit.time.AbsoluteDate;
......@@ -86,25 +85,21 @@ class SensorMeanPlaneCrossing {
final boolean aberrationOfLightCorrection,
final int maxEval, final double accuracy)
throws RuggedException {
try {
this.sensor = sensor;
this.minLine = minLine;
this.maxLine = maxLine;
this.lightTimeCorrection = lightTimeCorrection;
this.aberrationOfLightCorrection = aberrationOfLightCorrection;
this.maxEval = maxEval;
this.accuracy = accuracy;
this.scToBody = scToBody;
this.midLine = 0.5 * (minLine + maxLine);
final AbsoluteDate midDate = sensor.getDate(midLine);
this.midBodyToInert = scToBody.getBodyToInertial(midDate);
this.midScToInert = scToBody.getScToInertial(midDate);
} catch (OrekitException oe) {
throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
}
this.sensor = sensor;
this.minLine = minLine;
this.maxLine = maxLine;
this.lightTimeCorrection = lightTimeCorrection;
this.aberrationOfLightCorrection = aberrationOfLightCorrection;
this.maxEval = maxEval;
this.accuracy = accuracy;
this.scToBody = scToBody;
this.midLine = 0.5 * (minLine + maxLine);
final AbsoluteDate midDate = sensor.getDate(midLine);
this.midBodyToInert = scToBody.getBodyToInertial(midDate);
this.midScToInert = scToBody.getScToInertial(midDate);
}
/** Get the minimum line number in the search interval.
......@@ -166,65 +161,61 @@ class SensorMeanPlaneCrossing {
*/
public CrossingResult find(final Vector3D target)
throws RuggedException {
try {
final PVCoordinates targetPV = new PVCoordinates(target, Vector3D.ZERO);
// we don't use an Apache Commons Math solver here because we are more
// interested in reducing the number of evaluations than being accurate,
// as we know the solution is improved in the second stage of inverse localization.
// We expect two or three evaluations only. Each new evaluation shows up quickly in
// the performances as it involves frames conversions
double crossingLine = midLine;
Transform bodyToInert = midBodyToInert;
Transform scToInert = midScToInert;
boolean atMin = false;
boolean atMax = false;
for (int i = 0; i < maxEval; ++i) {
final FieldVector3D<DerivativeStructure> targetDirection =
evaluateLine(crossingLine, targetPV, bodyToInert, scToInert);
final DerivativeStructure beta = FieldVector3D.angle(targetDirection, sensor.getMeanPlaneNormal());
final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
if (FastMath.abs(deltaL) <= accuracy) {
// return immediately, without doing any additional evaluation!
return new CrossingResult(crossingLine, targetDirection);
}
crossingLine += deltaL;
if (crossingLine < minLine) {
if (atMin) {
// we were already trying at minLine and we need to go below that
// give up as the solution is out of search interval
return null;
}
atMin = true;
crossingLine = minLine;
} else if (crossingLine > maxLine) {
if (atMax) {
// we were already trying at maxLine and we need to go above that
// give up as the solution is out of search interval
return null;
}
atMax = true;
crossingLine = maxLine;
} else {
// the next evaluation will be a regular point
atMin = false;
atMax = false;
}
final AbsoluteDate date = sensor.getDate(crossingLine);
bodyToInert = scToBody.getBodyToInertial(date);
scToInert = scToBody.getScToInertial(date);
final PVCoordinates targetPV = new PVCoordinates(target, Vector3D.ZERO);
// we don't use an Apache Commons Math solver here because we are more
// interested in reducing the number of evaluations than being accurate,
// as we know the solution is improved in the second stage of inverse localization.
// We expect two or three evaluations only. Each new evaluation shows up quickly in
// the performances as it involves frames conversions
double crossingLine = midLine;
Transform bodyToInert = midBodyToInert;
Transform scToInert = midScToInert;
boolean atMin = false;
boolean atMax = false;
for (int i = 0; i < maxEval; ++i) {
final FieldVector3D<DerivativeStructure> targetDirection =
evaluateLine(crossingLine, targetPV, bodyToInert, scToInert);
final DerivativeStructure beta = FieldVector3D.angle(targetDirection, sensor.getMeanPlaneNormal());
final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
if (FastMath.abs(deltaL) <= accuracy) {
// return immediately, without doing any additional evaluation!
return new CrossingResult(crossingLine, targetDirection);
}
crossingLine += deltaL;
return null;
if (crossingLine < minLine) {
if (atMin) {
// we were already trying at minLine and we need to go below that
// give up as the solution is out of search interval
return null;
}
atMin = true;
crossingLine = minLine;
} else if (crossingLine > maxLine) {
if (atMax) {
// we were already trying at maxLine and we need to go above that
// give up as the solution is out of search interval
return null;
}
atMax = true;
crossingLine = maxLine;
} else {
// the next evaluation will be a regular point
atMin = false;
atMax = false;
}
} catch (OrekitException oe) {
throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
final AbsoluteDate date = sensor.getDate(crossingLine);
bodyToInert = scToBody.getBodyToInertial(date);
scToInert = scToBody.getScToInertial(date);
}
return null;
}
/** Evaluate geometry for a given line number.
......
core/src/main/java/org/orekit/rugged/utils/SpacecraftToObservedBody.java
+ 28
− 8
View file @ 6f44b586
......@@ -36,6 +36,12 @@ import org.orekit.utils.TimeStampedPVCoordinates;
*/
public class SpacecraftToObservedBody {
/** Start of search time span. */
private final AbsoluteDate minDate;
/** End of search time span. */
private final AbsoluteDate maxDate;
/** Step to use for inertial frame to body frame transforms cache computations. */
private final double tStep;
......@@ -68,6 +74,10 @@ public class SpacecraftToObservedBody {
final double tStep)
throws RuggedException {
try {
this.minDate = minDate;
this.maxDate = maxDate;
// safety checks
final AbsoluteDate minPVDate = positionsVelocities.get(0).getDate();
final AbsoluteDate maxPVDate = positionsVelocities.get(positionsVelocities.size() - 1).getDate();
......@@ -130,30 +140,30 @@ public class SpacecraftToObservedBody {
/** Get transform from spacecraft to inertial frame.
* @param date date of the transform
* @return transform from spacecraft to inertial frame
* @exception OrekitException if spacecraft position or attitude cannot be computed at date
* @exception RuggedException if spacecraft position or attitude cannot be computed at date
*/
public Transform getScToInertial(final AbsoluteDate date)
throws OrekitException {
throws RuggedException {
return interpolate(date, scToInertial);
}
/** Get transform from inertial frame to observed body frame.
* @param date date of the transform
* @return transform from inertial frame to observed body frame
* @exception OrekitException if frames cannot be computed at date
* @exception RuggedException if frames cannot be computed at date
*/
public Transform getInertialToBody(final AbsoluteDate date)
throws OrekitException {
throws RuggedException {
return interpolate(date, inertialToBody);
}
/** Get transform from observed body frame to inertial frame.
* @param date date of the transform
* @return transform from observed body frame to inertial frame
* @exception OrekitException if frames cannot be computed at date
* @exception RuggedException if frames cannot be computed at date
*/
public Transform getBodyToInertial(final AbsoluteDate date)
throws OrekitException {
throws RuggedException {
return interpolate(date, bodyToInertial);
}
......@@ -161,14 +171,24 @@ public class SpacecraftToObservedBody {
* @param date date of the transform
* @param list transforms list to interpolate from
* @return interpolated transform
* @exception OrekitException if frames cannot be computed at date
* @exception RuggedException if frames cannot be computed at date
*/
private Transform interpolate(final AbsoluteDate date, final List<Transform> list)
throws OrekitException {
throws RuggedException {
// check date range
if (date.compareTo(minDate) < 0) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, date, minDate, maxDate);
}
if (date.compareTo(maxDate) > 0) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, date, minDate, maxDate);
}
final double s = date.durationFrom(list.get(0).getDate()) / tStep;
final int index = FastMath.max(0, FastMath.min(list.size() - 1, (int) FastMath.rint(s)));
final Transform close = list.get(index);
return close.shiftedBy(date.durationFrom(close.getDate()));
}
}
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