diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index aaf4043e7eb94ee09e2acb2d7a23318718ee8a7e..e2c38d281a935deb460e5db5f44c958937de7a6a 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -626,10 +626,10 @@ public class Rugged {
final Vector3D sBLocal = sensorB.getPosition(); // S_b
// Get sensors's position and LOS into inertial frame
- final Vector3D sA = scToInertA.transformPosition(sALocal).normalize();
- final Vector3D vA = scToInertA.transformVector(vALocal).normalize();
- final Vector3D sB = scToInertB.transformPosition(sBLocal).normalize();
- final Vector3D vB = scToInertB.transformVector(vBLocal).normalize();
+ final Vector3D sA = scToInertA.transformPosition(sALocal);
+ final Vector3D vA = scToInertA.transformVector(vALocal);
+ final Vector3D sB = scToInertB.transformPosition(sBLocal);
+ final Vector3D vB = scToInertB.transformVector(vBLocal);
final Vector3D vBase= sB.subtract(sA); // S_b - S_a
final double svA = Vector3D.dotProduct(vBase, vA); // SV_a = (S_b - S_a).V_a
@@ -637,8 +637,8 @@ public class Rugged {
final double vAvB = Vector3D.dotProduct(vA, vB); // V_a.V_b
- // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 + (V_a.V_b)²)
- final double lambdaB = (svA * vAvB - svB) / (1 + FastMath.pow(vAvB,2));
+ // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 - (V_a.V_b)²)
+ final double lambdaB = (svA * vAvB - svB) / (1 - vAvB * vAvB);
// Compute lambda_a = SV_a + lambdaB * V_a.V_b
final double lambdaA = svA + lambdaB * vAvB;
@@ -648,7 +648,7 @@ public class Rugged {
final Vector3D vDistance = mB.subtract(mA); // M_b - M_a
//System.out.format("vDistance %f %f %f ",vDistance.getX(),vDistance.getY(),vDistance.getZ());
- // Get square of the euclidean norm
+ // Get the euclidean norm
final double d = vDistance.getNorm();
return d;
}
@@ -693,8 +693,8 @@ public class Rugged {
sensorB.getLOSDerivatives(dateB, iSup, generator)).normalize();
// Get sensors's LOS into inertial frame
- final FieldVector3D<DerivativeStructure> vA = scToInertA.transformVector(vALocal).normalize(); // V_a : line of sight's vectorA
- final FieldVector3D<DerivativeStructure> vB = scToInertB.transformVector(vBLocal).normalize(); // V_b
+ final FieldVector3D<DerivativeStructure> vA = scToInertA.transformVector(vALocal); // V_a : line of sight's vectorA
+ final FieldVector3D<DerivativeStructure> vB = scToInertB.transformVector(vBLocal); // V_b
// Get sensors's position into local frame TODO: check if we have to implement getPositionDerivatives() method & CO
final Vector3D sAtmp = sensorA.getPosition(); // S_a : sensorA 's position
@@ -706,8 +706,8 @@ public class Rugged {
final FieldVector3D<DerivativeStructure> sBLocal = new FieldVector3D<DerivativeStructure>(scaleFactor, sBtmp);
// Get sensors's position into inertial frame
- final FieldVector3D<DerivativeStructure> sA = scToInertA.transformPosition(sALocal).normalize();
- final FieldVector3D<DerivativeStructure> sB = scToInertB.transformPosition(sBLocal).normalize();
+ final FieldVector3D<DerivativeStructure> sA = scToInertA.transformPosition(sALocal);
+ final FieldVector3D<DerivativeStructure> sB = scToInertB.transformPosition(sBLocal);
// final FieldVector3D<DerivativeStructure> sA = sensorA.getPositionDerivatives(); // S_a : sensorA 's position
// final FieldVector3D<DerivativeStructure> sB = sensorB.getPositionDerivatives(); // S_b
@@ -718,8 +718,8 @@ public class Rugged {
final DerivativeStructure vAvB = FieldVector3D.dotProduct(vA, vB); // V_a.V_b
- // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 + (V_a.V_b)²)
- final DerivativeStructure lambdaB = (svA.multiply(vAvB).subtract(svB)).divide(vAvB.pow(2).add(1));
+ // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 - (V_a.V_b)²)
+ final DerivativeStructure lambdaB = (svA.multiply(vAvB).subtract(svB)).divide(vAvB.multiply(vAvB).subtract(1).negate());
// Compute lambda_a = SV_a + lambdaB * V_a.V_b
final DerivativeStructure lambdaA = vAvB.multiply(lambdaB).add(svA);
@@ -729,7 +729,7 @@ public class Rugged {
final FieldVector3D<DerivativeStructure> vDistance = mB.subtract(mA); // M_b - M_a
- // Get square of the euclidean norm
+ // Get the euclidean norm
final DerivativeStructure d = vDistance.getNorm();
return new DerivativeStructure[] {d};
}
diff --git a/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java b/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
index 688b401822073b74b7850682f4cfd85d8c7f0ca8..16b11db82487ab3f22ce7fcf49c1442fc030e7b5 100644
--- a/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
+++ b/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
@@ -309,36 +309,36 @@ public class AffinageRuggedLiaison {
});
// initialize ruggedB with selected flag set to true
-// System.out.format(" **** Select parameters drivers (ruggedB) **** %n");
-// ruggedB.
-// getLineSensor(pleiadesViewingModelB.getSensorName()).
-// getParametersDrivers().
-// filter(driver -> driver.getName().equals(SensorNameB+"_roll") || driver.getName().equals(SensorNameB+"_pitch")).
-// forEach(driver -> {
-// try {
-// driver.setSelected(true);
-// driver.setValue(0.0);
-// } catch (OrekitException e) {
-// throw new OrekitExceptionWrapper(e);
-// }
-// });
-// ruggedB.
-// getLineSensor(pleiadesViewingModelB.getSensorName()).
-// getParametersDrivers().
-// filter(driver -> driver.getName().equals(SensorNameB+"_factor")).
-// forEach(driver -> {
-// try {
-// driver.setSelected(true);
-// driver.setValue(1.0);
-// } catch (OrekitException e) {
-// throw new OrekitExceptionWrapper(e);
-// }
-// });
+ System.out.format(" **** Select parameters drivers (ruggedB) **** %n");
+ ruggedB.
+ getLineSensor(pleiadesViewingModelB.getSensorName()).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals(SensorNameB+"_roll") || driver.getName().equals(SensorNameB+"_pitch")).
+ forEach(driver -> {
+ try {
+ driver.setSelected(true);
+ driver.setValue(0.0);
+ } catch (OrekitException e) {
+ throw new OrekitExceptionWrapper(e);
+ }
+ });
+ ruggedB.
+ getLineSensor(pleiadesViewingModelB.getSensorName()).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals(SensorNameB+"_factor")).
+ forEach(driver -> {
+ try {
+ driver.setSelected(true);
+ driver.setValue(1.0);
+ } catch (OrekitException e) {
+ throw new OrekitExceptionWrapper(e);
+ }
+ });
System.out.format(" **** Start optimization **** %n");
// perform parameters estimation
int maxIterations = 100;
- double convergenceThreshold = 1e-14;//1e-14;
+ double convergenceThreshold = 1e-4;//1e-14;
System.out.format("iterations max %d convergence threshold %3.6e \n",maxIterations, convergenceThreshold);
@@ -378,6 +378,7 @@ public class AffinageRuggedLiaison {
// Viewing model B
double rollValueB = FastMath.toRadians(-0.00);
double pitchValueB = FastMath.toRadians(0.00);
+ double factorValueB = 1.00;
double estRollB = ruggedB.getLineSensor(pleiadesViewingModelB.getSensorName()).
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameB+"_roll")).
@@ -396,7 +397,7 @@ public class AffinageRuggedLiaison {
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameB+"_factor")).
findFirst().get().getValue();
- double factorErrorB = (estFactorB - factorValue);
+ double factorErrorB = (estFactorB - factorValueB);
System.out.format("Estimated factor B %3.5f factor error %3.6e %n ", estFactorB, factorErrorB);
diff --git a/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
index d958141961eb8b67ed207b2a379df71c37c7a06e..e2f8f4c3d6f18e2c0df975b3dbc2016509d3df1c 100644
--- a/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
+++ b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
@@ -148,7 +148,7 @@ public class SensorToSensorMeasureGenerator {
System.out.format(Locale.US,"distance %f %n",distance);
mapping.addMapping(new SensorPixel(line, pixelA),
- sensorPixelB, 0.0);
+ sensorPixelB, distance);
measureCount++;
} else {
// System.out