From 294d3a57c0a0946e23b17e93e23966665c96ca83 Mon Sep 17 00:00:00 2001
From: Luc Maisonobe <luc@orekit.org>
Date: Tue, 19 May 2015 10:39:36 +0200
Subject: [PATCH] Improved expected line in slowFind.
---
.../java/org/orekit/rugged/api/Rugged.java | 4 --
.../linesensor/SensorMeanPlaneCrossing.java | 61 ++++++++++++-------
2 files changed, 40 insertions(+), 25 deletions(-)
diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index 12444ef8..1707d753 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -453,10 +453,6 @@ public class Rugged {
crossingResult.getTargetDirection().toVector3D(),
MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY);
final double coarsePixel = pixelCrossing.locatePixel(crossingResult.getDate());
- if (Double.isNaN(coarsePixel)) {
- // target is out of search interval
- return null;
- }
// fix line by considering the closest pixel exact position and line-of-sight
// (this pixel might point towards a direction slightly above or below the mean sensor plane)
diff --git a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
index 529d0095..39f883a1 100644
--- a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
+++ b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
@@ -496,23 +496,6 @@ public class SensorMeanPlaneCrossing {
public CrossingResult slowFind(final PVCoordinates targetPV, final double initialGuess)
throws RuggedException {
- // set up function evaluating to 0.0 where target matches line
- final UnivariateFunction f = new UnivariateFunction() {
- /** {@inheritDoc} */
- @Override
- public double value(final double x) throws RuggedExceptionWrapper {
- try {
- final AbsoluteDate date = sensor.getDate(x);
- final FieldVector3D<DerivativeStructure> targetDirection =
- evaluateLine(x, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date));
- final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal);
- return 0.5 * FastMath.PI - beta.getValue();
- } catch (RuggedException re) {
- throw new RuggedExceptionWrapper(re);
- }
- }
- };
-
try {
// safety check
@@ -523,6 +506,19 @@ public class SensorMeanPlaneCrossing {
startValue = initialGuess;
}
+ // set up function evaluating to 0.0 where target matches line
+ final UnivariateFunction f = new UnivariateFunction() {
+ /** {@inheritDoc} */
+ @Override
+ public double value(final double x) throws RuggedExceptionWrapper {
+ try {
+ return targetOffset(x, targetPV).getValue();
+ } catch (RuggedException re) {
+ throw new RuggedExceptionWrapper(re);
+ }
+ }
+ };
+
final UnivariateSolver solver = new BracketingNthOrderBrentSolver(accuracy, 5);
final double crossingLine = solver.solve(maxEval, f, minLine, maxLine, startValue);
@@ -532,16 +528,39 @@ public class SensorMeanPlaneCrossing {
return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetPV.getPosition(), targetDirection);
} catch (NoBracketingException nbe) {
- final double fMinLine = f.value(minLine);
- final double fMaxLine = f.value(maxLine);
- final double expectedLine = (fMaxLine * minLine - fMinLine * maxLine) / (fMaxLine - fMinLine);
- throw new InverseLocOutOfLineRangeException(expectedLine, minLine, maxLine);
+ final DerivativeStructure fMinLine = targetOffset(minLine, targetPV);
+ final DerivativeStructure fMaxLine = targetOffset(maxLine, targetPV);
+ final double linearExpectedLine = (fMaxLine.getValue() * minLine - fMinLine.getValue() * maxLine) /
+ (fMaxLine.getValue() - fMinLine.getValue());
+ final double newtonExpectedLine;
+ if (linearExpectedLine < midLine) {
+ newtonExpectedLine = minLine - fMinLine.getPartialDerivative(1) / fMinLine.getValue();
+ } else {
+ newtonExpectedLine = maxLine - fMaxLine.getPartialDerivative(1) / fMaxLine.getValue();
+ }
+ throw new InverseLocOutOfLineRangeException(newtonExpectedLine, minLine, maxLine);
} catch (RuggedExceptionWrapper rew) {
throw rew.getException();
}
}
+ /** Target offset.
+ * @param lineNumber current line number
+ * @param targetPV target ground point
+ * @return target direction in spacecraft frame, with its first derivative
+ * with respect to line number
+ * @exception RuggedException if geometry cannot be computed
+ */
+ private DerivativeStructure targetOffset(final double lineNumber, final PVCoordinates targetPV)
+ throws RuggedException {
+ final AbsoluteDate date = sensor.getDate(lineNumber);
+ final FieldVector3D<DerivativeStructure> targetDirection =
+ evaluateLine(lineNumber, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date));
+ final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal);
+ return beta.subtract(0.5 * FastMath.PI);
+ };
+
/** Evaluate geometry for a given line number.
* @param lineNumber current line number
* @param targetPV target ground point
--
GitLab