From 81ca7b43a22cbff14803a8d090db315f6d98ec61 Mon Sep 17 00:00:00 2001
From: Luc Maisonobe <luc@orekit.org>
Date: Sat, 21 Mar 2015 15:53:39 +0100
Subject: [PATCH] Use inverse cubic interpolation when possible for inverse
location.
---
.../linesensor/SensorMeanPlaneCrossing.java | 30 +++++++++++++++----
1 file changed, 24 insertions(+), 6 deletions(-)
diff --git a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
index 0788527d..b6984f87 100644
--- a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
+++ b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
@@ -307,7 +307,8 @@ public class SensorMeanPlaneCrossing {
// as we know the solution is improved in the second stage of inverse location.
// We expect two or three evaluations only. Each new evaluation shows up quickly in
// the performances as it involves frames conversions
- final double[] searchHistory = new double[maxEval];
+ final double[] crossingLineHistory = new double[maxEval];
+ final DerivativeStructure[] betaHistory = new DerivativeStructure[maxEval];
double crossingLine = midLine;
Transform bodyToInert = midBodyToInert;
Transform scToInert = midScToInert;
@@ -315,19 +316,36 @@ public class SensorMeanPlaneCrossing {
boolean atMax = false;
for (int i = 0; i < maxEval; ++i) {
- searchHistory[i] = crossingLine;
+ crossingLineHistory[i] = crossingLine;
final FieldVector3D<DerivativeStructure> targetDirection =
evaluateLine(crossingLine, targetPV, bodyToInert, scToInert);
- final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal);
+ betaHistory[i] = FieldVector3D.angle(targetDirection, meanPlaneNormal);
- final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
+ final double deltaL;
+ if (i == 0) {
+ // simple Newton iteration
+ deltaL = (0.5 * FastMath.PI - betaHistory[i].getValue()) / betaHistory[i].getPartialDerivative(1);
+ crossingLine += deltaL;
+ } else {
+ // inverse cubic iteration
+ final double a0 = betaHistory[i - 1].getValue() - 0.5 * FastMath.PI;
+ final double l0 = crossingLineHistory[i - 1];
+ final double d0 = betaHistory[i - 1].getPartialDerivative(1);
+ final double a1 = betaHistory[i].getValue() - 0.5 * FastMath.PI;
+ final double l1 = crossingLineHistory[i];
+ final double d1 = betaHistory[i].getPartialDerivative(1);
+ final double a1Ma0 = a1 - a0;
+ crossingLine = ((l0 * (a1 - 3 * a0) - a0 * a1Ma0 / d0) * a1 * a1 +
+ (l1 * (3 * a1 - a0) - a1 * a1Ma0 / d1) * a0 * a0
+ ) / (a1Ma0 * a1Ma0 * a1Ma0);
+ deltaL = crossingLine - l1;
+ }
if (FastMath.abs(deltaL) <= accuracy) {
// return immediately, without doing any additional evaluation!
return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetDirection);
}
- crossingLine += deltaL;
for (int j = 0; j < i; ++j) {
- if (FastMath.abs(crossingLine - searchHistory[j]) <= 1.0) {
+ if (FastMath.abs(crossingLine - crossingLineHistory[j]) <= 1.0) {
// rare case: we are stuck in a loop!
// switch to a more robust (but slower) algorithm in this case
return slowFind(targetPV, crossingLine);
--
GitLab