Skip to content
Snippets Groups Projects
Commit 3bb6c730 authored by Jonathan Guinet's avatar Jonathan Guinet
Browse files

add noise in SensorToSensorMeasureGenerator, choice of Attitude Law

parent 0ce7bc13
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
+ 112
− 77
View file @ 3bb6c730
...@@ -16,17 +16,14 @@ ...@@ -16,17 +16,14 @@
*/ */
package AffinagePleiades; package AffinagePleiades;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath; import org.hipparchus.util.FastMath;
import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum; import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
import java.io.File; import java.io.File;
import java.util.Locale; import java.util.Locale;
import java.util.Collection;
import java.util.Collections; import java.util.Collections;
import java.io.FileWriter;
import java.io.StringWriter;
import java.io.PrintWriter;
import java.io.IOException;
import org.orekit.bodies.BodyShape; import org.orekit.bodies.BodyShape;
import org.orekit.bodies.GeodeticPoint; import org.orekit.bodies.GeodeticPoint;
...@@ -43,15 +40,15 @@ import org.orekit.rugged.api.EllipsoidId; ...@@ -43,15 +40,15 @@ import org.orekit.rugged.api.EllipsoidId;
import org.orekit.rugged.api.InertialFrameId; import org.orekit.rugged.api.InertialFrameId;
import org.orekit.rugged.api.Rugged; import org.orekit.rugged.api.Rugged;
import org.orekit.rugged.api.RuggedBuilder; import org.orekit.rugged.api.RuggedBuilder;
import org.orekit.rugged.api.SensorToSensorMapping;
import org.orekit.rugged.errors.RuggedException; import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.linesensor.LineSensor; import org.orekit.rugged.linesensor.LineSensor;
import org.orekit.rugged.linesensor.SensorPixel;
import org.orekit.time.AbsoluteDate; import org.orekit.time.AbsoluteDate;
import org.orekit.utils.AngularDerivativesFilter; import org.orekit.utils.AngularDerivativesFilter;
import org.orekit.utils.CartesianDerivativesFilter; import org.orekit.utils.CartesianDerivativesFilter;
import org.orekit.utils.PVCoordinates; import org.orekit.utils.PVCoordinates;
import org.orekit.utils.ParameterDriver;
/** /**
...@@ -78,7 +75,8 @@ public class AffinageRuggedLiaison { ...@@ -78,7 +75,8 @@ public class AffinageRuggedLiaison {
//create 2 Pleiades Viewing Model //create 2 Pleiades Viewing Model
final String SensorNameA = "SensorA"; final String SensorNameA = "SensorA";
final double incidenceAngleA = -0.0; final double incidenceAngleA = -5.0;
//final String dateA = "2016-01-01T11:59:50.0";
final String dateA = "2016-01-01T11:59:50.0"; final String dateA = "2016-01-01T11:59:50.0";
PleiadesViewingModel pleiadesViewingModelA = new PleiadesViewingModel(SensorNameA,incidenceAngleA,dateA); PleiadesViewingModel pleiadesViewingModelA = new PleiadesViewingModel(SensorNameA,incidenceAngleA,dateA);
final AbsoluteDate minDateA = pleiadesViewingModelA.getMinDate(); final AbsoluteDate minDateA = pleiadesViewingModelA.getMinDate();
...@@ -92,8 +90,10 @@ public class AffinageRuggedLiaison { ...@@ -92,8 +90,10 @@ public class AffinageRuggedLiaison {
NormalizedSphericalHarmonicsProvider gravityFieldA = orbitmodelA.createGravityField(); NormalizedSphericalHarmonicsProvider gravityFieldA = orbitmodelA.createGravityField();
Orbit orbitA = orbitmodelA.createOrbit(gravityFieldA.getMu(), refDateA); Orbit orbitA = orbitmodelA.createOrbit(gravityFieldA.getMu(), refDateA);
// if no LOF Transform Attitude Provider is Nadir Pointing Yaw Compensation
final double [] rollPoly = {0.0,0.0,0.0}; final double [] rollPoly = {0.0,0.0,0.0};
double[] pitchPoly = {0.0,0.0}; double[] pitchPoly = {0.025,0.0};
double[] yawPoly = {0.0,0.0,0.0}; double[] yawPoly = {0.0,0.0,0.0};
orbitmodelA.setLOFTransform(rollPoly, pitchPoly, yawPoly, minDateA); orbitmodelA.setLOFTransform(rollPoly, pitchPoly, yawPoly, minDateA);
...@@ -118,7 +118,9 @@ public class AffinageRuggedLiaison { ...@@ -118,7 +118,9 @@ public class AffinageRuggedLiaison {
orbitmodelA.orbitToPV(orbitA, earthA, minDateA.shiftedBy(-0.0), maxDateA.shiftedBy(+0.0), 0.25), orbitmodelA.orbitToPV(orbitA, earthA, minDateA.shiftedBy(-0.0), maxDateA.shiftedBy(+0.0), 0.25),
8, CartesianDerivativesFilter.USE_PV, 8, CartesianDerivativesFilter.USE_PV,
orbitmodelA.orbitToQ(orbitA, earthA, minDateA.shiftedBy(-0.0), maxDateA.shiftedBy(+0.0), 0.25), orbitmodelA.orbitToQ(orbitA, earthA, minDateA.shiftedBy(-0.0), maxDateA.shiftedBy(+0.0), 0.25),
2, AngularDerivativesFilter.USE_R); 2, AngularDerivativesFilter.USE_R)
.setLightTimeCorrection(false)
.setAberrationOfLightCorrection(false);
Rugged ruggedA = ruggedBuilderA.build(); Rugged ruggedA = ruggedBuilderA.build();
...@@ -126,8 +128,9 @@ public class AffinageRuggedLiaison { ...@@ -126,8 +128,9 @@ public class AffinageRuggedLiaison {
//create 2 Pleiades Viewing Model //create 2 Pleiades Viewing Model
final String SensorNameB = "SensorB"; final String SensorNameB = "SensorB";
final double incidenceAngleB = +0.0; final double incidenceAngleB = 0.0;
final String dateB = "2016-01-01T12:00:10.0"; //final String dateB = "2016-01-01T12:00:10.0";
final String dateB = "2016-01-01T12:02:50.0";
PleiadesViewingModel pleiadesViewingModelB = new PleiadesViewingModel(SensorNameB,incidenceAngleB,dateB); PleiadesViewingModel pleiadesViewingModelB = new PleiadesViewingModel(SensorNameB,incidenceAngleB,dateB);
final AbsoluteDate minDateB = pleiadesViewingModelB.getMinDate(); final AbsoluteDate minDateB = pleiadesViewingModelB.getMinDate();
final AbsoluteDate maxDateB = pleiadesViewingModelB.getMaxDate(); final AbsoluteDate maxDateB = pleiadesViewingModelB.getMaxDate();
...@@ -141,7 +144,7 @@ public class AffinageRuggedLiaison { ...@@ -141,7 +144,7 @@ public class AffinageRuggedLiaison {
Orbit orbitB = orbitmodelB.createOrbit(gravityFieldB.getMu(), refDateB); Orbit orbitB = orbitmodelB.createOrbit(gravityFieldB.getMu(), refDateB);
orbitmodelB.setLOFTransform(rollPoly, pitchPoly, yawPoly, minDateB); //orbitmodelB.setLOFTransform(rollPoly, pitchPoly, yawPoly, minDateB);
PVCoordinates PVB = orbitB.getPVCoordinates(earthB.getBodyFrame()); PVCoordinates PVB = orbitB.getPVCoordinates(earthB.getBodyFrame());
GeodeticPoint gpB = earthB.transform(PVB.getPosition(), earthB.getBodyFrame(), orbitB.getDate()); GeodeticPoint gpB = earthB.transform(PVB.getPosition(), earthB.getBodyFrame(), orbitB.getDate());
...@@ -165,23 +168,25 @@ public class AffinageRuggedLiaison { ...@@ -165,23 +168,25 @@ public class AffinageRuggedLiaison {
orbitmodelB.orbitToPV(orbitB, earthB, minDateB.shiftedBy(-0.0), maxDateB.shiftedBy(+0.0), 0.25), orbitmodelB.orbitToPV(orbitB, earthB, minDateB.shiftedBy(-0.0), maxDateB.shiftedBy(+0.0), 0.25),
8, CartesianDerivativesFilter.USE_PV, 8, CartesianDerivativesFilter.USE_PV,
orbitmodelB.orbitToQ(orbitB, earthB, minDateB.shiftedBy(-0.0), maxDateB.shiftedBy(+0.0), 0.25), orbitmodelB.orbitToQ(orbitB, earthB, minDateB.shiftedBy(-0.0), maxDateB.shiftedBy(+0.0), 0.25),
2, AngularDerivativesFilter.USE_R); 2, AngularDerivativesFilter.USE_R)
.setLightTimeCorrection(false)
.setAberrationOfLightCorrection(false);
Rugged ruggedB = ruggedBuilderB.build(); Rugged ruggedB = ruggedBuilderB.build();
/*
Vector3D positionA = lineSensorA.getPosition(); // This returns a zero vector since we set the relative position of the sensor w.r.T the satellite to 0. Vector3D positionA = lineSensorA.getPosition(); // This returns a zero vector since we set the relative position of the sensor w.r.T the satellite to 0.
AbsoluteDate lineDateA = lineSensorA.getDate(pleiadesViewingModelA.dimension/2); AbsoluteDate lineDateA = lineSensorA.getDate(pleiadesViewingModelA.dimension/2);
Vector3D losA = lineSensorA.getLOS(lineDateA,pleiadesViewingModelA.dimension/2); Vector3D losA = lineSensorA.getLOS(lineDateA,pleiadesViewingModelA.dimension/2);
GeodeticPoint centerPointA = ruggedA.directLocation(lineDateA, positionA, losA); GeodeticPoint centerPointA = ruggedA.directLocation(lineDateA, positionA, losA);
System.out.format(Locale.US, "center geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n", System.out.format(Locale.US, "center geodetic position A : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
FastMath.toDegrees(centerPointA.getLatitude()), FastMath.toDegrees(centerPointA.getLatitude()),
FastMath.toDegrees(centerPointA.getLongitude()),centerPointA.getAltitude()); FastMath.toDegrees(centerPointA.getLongitude()),centerPointA.getAltitude());
...@@ -191,7 +196,7 @@ public class AffinageRuggedLiaison { ...@@ -191,7 +196,7 @@ public class AffinageRuggedLiaison {
AbsoluteDate lineDateB = lineSensorB.getDate(pleiadesViewingModelB.dimension/2); AbsoluteDate lineDateB = lineSensorB.getDate(pleiadesViewingModelB.dimension/2);
Vector3D losB = lineSensorB.getLOS(lineDateB,pleiadesViewingModelB.dimension/2); Vector3D losB = lineSensorB.getLOS(lineDateB,pleiadesViewingModelB.dimension/2);
GeodeticPoint centerPointB = ruggedB.directLocation(lineDateB, positionB, losB); GeodeticPoint centerPointB = ruggedB.directLocation(lineDateB, positionB, losB);
System.out.format(Locale.US, "center geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n", System.out.format(Locale.US, "center geodetic position B : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
FastMath.toDegrees(centerPointB.getLatitude()), FastMath.toDegrees(centerPointB.getLatitude()),
FastMath.toDegrees(centerPointB.getLongitude()),centerPointB.getAltitude()); FastMath.toDegrees(centerPointB.getLongitude()),centerPointB.getAltitude());
...@@ -199,14 +204,14 @@ public class AffinageRuggedLiaison { ...@@ -199,14 +204,14 @@ public class AffinageRuggedLiaison {
lineDateB = lineSensorB.getDate(0); lineDateB = lineSensorB.getDate(0);
losB = lineSensorB.getLOS(lineDateB,0); losB = lineSensorB.getLOS(lineDateB,0);
GeodeticPoint firstPointB = ruggedB.directLocation(lineDateB, positionB, losB); GeodeticPoint firstPointB = ruggedB.directLocation(lineDateB, positionB, losB);
System.out.format(Locale.US, "first geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n", System.out.format(Locale.US, "first geodetic position B : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
FastMath.toDegrees(firstPointB.getLatitude()), FastMath.toDegrees(firstPointB.getLatitude()),
FastMath.toDegrees(firstPointB.getLongitude()),firstPointB.getAltitude()); FastMath.toDegrees(firstPointB.getLongitude()),firstPointB.getAltitude());
lineDateB = lineSensorB.getDate(pleiadesViewingModelB.dimension-1); lineDateB = lineSensorB.getDate(pleiadesViewingModelB.dimension-1);
losB = lineSensorB.getLOS(lineDateB,pleiadesViewingModelB.dimension-1); losB = lineSensorB.getLOS(lineDateB,pleiadesViewingModelB.dimension-1);
GeodeticPoint lastPointB = ruggedB.directLocation(lineDateB, positionB, losB); GeodeticPoint lastPointB = ruggedB.directLocation(lineDateB, positionB, losB);
System.out.format(Locale.US, "last geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n", System.out.format(Locale.US, "last geodetic position B : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
FastMath.toDegrees(lastPointB.getLatitude()), FastMath.toDegrees(lastPointB.getLatitude()),
FastMath.toDegrees(lastPointB.getLongitude()),lastPointB.getAltitude()); FastMath.toDegrees(lastPointB.getLongitude()),lastPointB.getAltitude());
...@@ -214,26 +219,23 @@ public class AffinageRuggedLiaison { ...@@ -214,26 +219,23 @@ public class AffinageRuggedLiaison {
double distance = DistanceTools.computeDistanceRad(centerPointA.getLongitude(), centerPointA.getLatitude(), double distance = DistanceTools.computeDistanceRad(centerPointA.getLongitude(), centerPointA.getLatitude(),
centerPointB.getLongitude(), centerPointB.getLatitude()); centerPointB.getLongitude(), centerPointB.getLatitude());
System.out.format("distance %f meters %n",distance); System.out.format("distance %f meters %n",distance);*/
System.out.format(" **** Add roll / pitch / factor values **** %n"); System.out.format(" **** Add roll / pitch / factor values **** %n");
double rollValueA = FastMath.toRadians(0.00);//0.6 double rollValueA = FastMath.toRadians(0.004);//0.6
double pitchValueA = FastMath.toRadians(0.00);//0.02 double pitchValueA = FastMath.toRadians(0.0008);//0.02
double factorValue = 1.0; double factorValue = 1.01;
//final String SensorNameAFactor = SensorNameA+"_factor";
//final String SensorNameBFactor = SensorNameB+"_factor";
final String SensorNameAFactor = "factor"; final String SensorNameAFactor = "factor";
final String SensorNameBFactor = "factor"; final String SensorNameBFactor = "factor";
System.out.format("roll : %3.5f pitch : %3.5f factor : %3.5f \n",rollValueA,pitchValueA,factorValue); System.out.format("Acquisition A roll : %3.5f pitch : %3.5f factor : %3.5f \n",rollValueA,pitchValueA,factorValue);
ruggedA. ruggedA.
getLineSensor(SensorNameA). getLineSensor(SensorNameA).
...@@ -253,19 +255,19 @@ public class AffinageRuggedLiaison { ...@@ -253,19 +255,19 @@ public class AffinageRuggedLiaison {
filter(driver -> driver.getName().equals(SensorNameAFactor)). filter(driver -> driver.getName().equals(SensorNameAFactor)).
findFirst().get().setValue(factorValue); findFirst().get().setValue(factorValue);
/*
lineDateA = lineSensorA.getDate(pleiadesViewingModelA.dimension/2); lineDateA = lineSensorA.getDate(pleiadesViewingModelA.dimension/2);
losA = lineSensorA.getLOS(lineDateA,(int) pleiadesViewingModelA.dimension/2); losA = lineSensorA.getLOS(lineDateA,(int) pleiadesViewingModelA.dimension/2);*/
//Vector3D losADouble = lineSensorA.getLOS(lineDateA,(double) pleiadesViewingModelA.dimension/2); //Vector3D losADouble = lineSensorA.getLOS(lineDateA,(double) pleiadesViewingModelA.dimension/2);
//final Vector3D vDistance = losA.subtract(losADouble); //final Vector3D vDistance = losA.subtract(losADouble);
//final double d = vDistance.getNorm(); //final double d = vDistance.getNorm();
//System.out.format("distance double/int %f ",d); //System.out.format("distance double/int %f ",d);
double rollValueB = FastMath.toRadians(0.02);//0.6 double rollValueB = FastMath.toRadians(-0.004);//0.6
double pitchValueB = FastMath.toRadians(0.01);//0.02 double pitchValueB = FastMath.toRadians(0.0008);//0.02
factorValue = 1.00;
System.out.format("roll : %3.5f pitch : %3.5f factor : %3.5f \n",rollValueB,pitchValueB,factorValue); System.out.format(" Acquisition B roll : %3.5f pitch : %3.5f factor : %3.5f \n",rollValueB,pitchValueB,factorValue);
ruggedB. ruggedB.
getLineSensor(SensorNameB). getLineSensor(SensorNameB).
...@@ -286,7 +288,7 @@ public class AffinageRuggedLiaison { ...@@ -286,7 +288,7 @@ public class AffinageRuggedLiaison {
findFirst().get().setValue(factorValue); findFirst().get().setValue(factorValue);
centerPointA = ruggedA.directLocation(lineDateA, positionA, losA); /*centerPointA = ruggedA.directLocation(lineDateA, positionA, losA);
System.out.format(Locale.US, "center geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n", System.out.format(Locale.US, "center geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
FastMath.toDegrees(centerPointA.getLatitude()), FastMath.toDegrees(centerPointA.getLatitude()),
FastMath.toDegrees(centerPointA.getLongitude()),centerPointA.getAltitude()); FastMath.toDegrees(centerPointA.getLongitude()),centerPointA.getAltitude());
...@@ -294,63 +296,66 @@ public class AffinageRuggedLiaison { ...@@ -294,63 +296,66 @@ public class AffinageRuggedLiaison {
distance = DistanceTools.computeDistanceRad(centerPointA.getLongitude(), centerPointA.getLatitude(), distance = DistanceTools.computeDistanceRad(centerPointA.getLongitude(), centerPointA.getLatitude(),
centerPointB.getLongitude(), centerPointB.getLatitude()); centerPointB.getLongitude(), centerPointB.getLatitude());
System.out.format("distance %f meters %n",distance); */
// Search the sensor pixel seeing point //add mapping
int minLine = 0;
int maxLine = pleiadesViewingModelB.dimension-1;
SensorPixel sensorPixelB = ruggedB.inverseLocation(SensorNameB, centerPointA, minLine, maxLine);
// we need to test if the sensor pixel is found in the prescribed lines otherwise the sensor pixel is null
if (sensorPixelB != null){
System.out.format(Locale.US, "Sensor Pixel found : line = %5.3f, pixel = %5.3f %n", sensorPixelB.getLineNumber(), sensorPixelB.getPixelNumber());
AbsoluteDate date = lineSensorB.getDate(sensorPixelB.getLineNumber());
losB = lineSensorB.getLOS(date,sensorPixelB.getPixelNumber());
GeodeticPoint pointB = ruggedB.directLocation(date, positionB, losB);
distance = DistanceTools.computeDistanceRad(centerPointA.getLongitude(), pointB.getLatitude(),
pointB.getLongitude(), pointB.getLatitude());
System.out.format("distance %f meters %n",distance);
} else { final double[] pixErr = new double[4];
System.out.println("Sensor Pixel is null: point cannot be seen between the prescribed line numbers\n"); pixErr[0] = 0.0; // SensorA mean
} pixErr[1] = 0.0; // SensorA std
pixErr[2] = 0; // SensorB mean
pixErr[3] = 0.0; //SensorB std
System.out.format(" **** Generate Measures **** %n");
System.out.format(" **** Sensor A mean %f std %f **** %n",pixErr[0],pixErr[1]);
System.out.format(" **** Sensor B mean %f std %f **** %n",pixErr[2],pixErr[3]);
//add mapping
System.out.format(" **** Generate Measures **** %n");
SensorToSensorMeasureGenerator measure = new SensorToSensorMeasureGenerator(pleiadesViewingModelA,ruggedA,pleiadesViewingModelB,ruggedB); SensorToSensorMeasureGenerator measure = new SensorToSensorMeasureGenerator(pleiadesViewingModelA,ruggedA,pleiadesViewingModelB,ruggedB);
int lineSampling = 1000; int lineSampling = 1000;
int pixelSampling = 1000; int pixelSampling = 1000;
measure.CreateMeasure(lineSampling, pixelSampling); measure.CreateMeasure(lineSampling, pixelSampling,pixErr);
System.out.format("nb TiePoints %d %n", measure.getMeasureCount()); System.out.format("nb TiePoints %d %n", measure.getMeasureCount());
SensorToSensorLocalisationMetrics gtResiduals = new SensorToSensorLocalisationMetrics(measure.getMapping(),ruggedA,ruggedB, false); SensorToSensorLocalisationMetrics gtResiduals = new SensorToSensorLocalisationMetrics(measure.getMapping(),ruggedA,ruggedB, false);
System.out.format("gt residuals max : %3.4f mean %3.4f meters %n",gtResiduals.getMaxResidual(),gtResiduals.getMeanResidual()); System.out.format("gt residuals max : %3.4f mean %3.4f meters %n",gtResiduals.getMaxResidual(),gtResiduals.getMeanResidual());
System.out.format("gt los distance max : %3.4f mean %3.4f meters %n",gtResiduals.getLosMaxDistance(),gtResiduals.getLosMeanDistance()); System.out.format("gt los distance max : %1.4e mean %1.4e meters %n",gtResiduals.getLosMaxDistance(),gtResiduals.getLosMeanDistance());
// initialize ruggedA without perturbation // initialize ruggedA without perturbation
System.out.format(" **** Reset Roll/Pitch/Factor (ruggedA) **** %n"); System.out.format(" **** Reset Roll/Pitch/Factor (ruggedA) **** %n");
ruggedA. ruggedA.
getLineSensor(pleiadesViewingModelA.getSensorName()). getLineSensor(pleiadesViewingModelA.getSensorName()).
getParametersDrivers(). getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameA+"_roll") || driver.getName().equals(SensorNameA+"_pitch")). filter(driver -> driver.getName().equals(SensorNameA+"_roll")).
forEach(driver -> { forEach(driver -> {
try { try {
driver.setSelected(false); driver.setSelected(true);
driver.setValue(0.0); driver.setValue(0.0);
} catch (OrekitException e) { } catch (OrekitException e) {
throw new OrekitExceptionWrapper(e); throw new OrekitExceptionWrapper(e);
} }
}); });
ruggedA.
getLineSensor(pleiadesViewingModelA.getSensorName()).
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameA+"_pitch")).
forEach(driver -> {
try {
driver.setSelected(true);
driver.setValue(0.0);
} catch (OrekitException e) {
throw new OrekitExceptionWrapper(e);
}
});
ruggedA. ruggedA.
getLineSensor(pleiadesViewingModelA.getSensorName()). getLineSensor(pleiadesViewingModelA.getSensorName()).
getParametersDrivers(). getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameAFactor)). filter(driver -> driver.getName().equals(SensorNameAFactor)).
forEach(driver -> { forEach(driver -> {
try { try {
driver.setSelected(false); driver.setSelected(true);
driver.setValue(1.0); // default value: no Z scale factor applied driver.setValue(1.0); // default value: no Z scale factor applied
} catch (OrekitException e) { } catch (OrekitException e) {
throw new OrekitExceptionWrapper(e); throw new OrekitExceptionWrapper(e);
...@@ -359,7 +364,7 @@ public class AffinageRuggedLiaison { ...@@ -359,7 +364,7 @@ public class AffinageRuggedLiaison {
// initialize ruggedB with selected flag set to true // initialize ruggedB with selected flag set to true
System.out.format(" **** Select parameters drivers (ruggedB) **** %n"); System.out.format(" **** Select parameters drivers (ruggedB) **** %n");
ruggedB. /*ruggedB.
getLineSensor(pleiadesViewingModelB.getSensorName()). getLineSensor(pleiadesViewingModelB.getSensorName()).
getParametersDrivers(). getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameB+"_roll") || driver.getName().equals(SensorNameB+"_pitch")). filter(driver -> driver.getName().equals(SensorNameB+"_roll") || driver.getName().equals(SensorNameB+"_pitch")).
...@@ -377,25 +382,58 @@ public class AffinageRuggedLiaison { ...@@ -377,25 +382,58 @@ public class AffinageRuggedLiaison {
filter(driver -> driver.getName().equals(SensorNameBFactor)). filter(driver -> driver.getName().equals(SensorNameBFactor)).
forEach(driver -> { forEach(driver -> {
try { try {
driver.setSelected(false); driver.setSelected(true);
driver.setValue(1.0); driver.setValue(1.0);
} catch (OrekitException e) { } catch (OrekitException e) {
throw new OrekitExceptionWrapper(e); throw new OrekitExceptionWrapper(e);
} }
}); });*/
ruggedB.
getLineSensor(SensorNameB).
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameB+"_roll")).
findFirst().get().setValue(0.0);
ruggedB.
getLineSensor(SensorNameB).
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameB+"_roll")).
findFirst().get().setSelected(true);
ruggedB.
getLineSensor(SensorNameB).
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameB+"_pitch")).
findFirst().get().setValue(0.0);
ruggedB.
getLineSensor(SensorNameB).
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameB+"_pitch")).
findFirst().get().setSelected(true);
ruggedB.
getLineSensor(SensorNameB).
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameBFactor)).
findFirst().get().setValue(1.0);
ruggedB.
getLineSensor(SensorNameB).
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameBFactor)).
findFirst().get().setSelected(true);
SensorToSensorLocalisationMetrics initialResiduals = new SensorToSensorLocalisationMetrics(measure.getMapping(),ruggedA,ruggedB, false); SensorToSensorLocalisationMetrics initialResiduals = new SensorToSensorLocalisationMetrics(measure.getMapping(),ruggedA,ruggedB, false);
System.out.format("initial residuals max : %3.4f mean %3.4f meters %n",initialResiduals.getMaxResidual(),initialResiduals.getMeanResidual()); System.out.format("initial residuals max : %3.4f mean %3.4f meters %n",initialResiduals.getMaxResidual(),initialResiduals.getMeanResidual());
System.out.format("initial los distance max : %3.4f mean %3.4f meters %n",initialResiduals.getLosMaxDistance(),initialResiduals.getLosMeanDistance()); System.out.format("initial los distance max : %1.4e mean %1.4e meters %n",initialResiduals.getLosMaxDistance(),initialResiduals.getLosMeanDistance());
System.out.format(" **** Start optimization **** %n"); System.out.format(" **** Start optimization **** %n");
// perform parameters estimation // perform parameters estimation
int maxIterations = 100; int maxIterations = 100;
double convergenceThreshold = 1e-6;//1e-14; double convergenceThreshold = 1e-10;//1e-14;
System.out.format("iterations max %d convergence threshold %3.6e \n",maxIterations, convergenceThreshold); System.out.format("iterations max %d convergence threshold %3.6e \n",maxIterations, convergenceThreshold);
...@@ -460,12 +498,9 @@ public class AffinageRuggedLiaison { ...@@ -460,12 +498,9 @@ public class AffinageRuggedLiaison {
SensorToSensorLocalisationMetrics finalResiduals = new SensorToSensorLocalisationMetrics(measure.getMapping(),ruggedA,ruggedB, false); SensorToSensorLocalisationMetrics finalResiduals = new SensorToSensorLocalisationMetrics(measure.getMapping(),ruggedA,ruggedB, false);
System.out.format("final residuals max : %3.4f mean %3.4f meters %n",finalResiduals.getMaxResidual(),finalResiduals.getMeanResidual()); System.out.format("final residuals max : %3.4f mean %3.4f meters %n",finalResiduals.getMaxResidual(),finalResiduals.getMeanResidual());
System.out.format("final los distance max : %3.4f mean %3.4f meters %n",finalResiduals.getLosMaxDistance(),finalResiduals.getLosMeanDistance()); System.out.format("final los distance max : %1.4e mean %1.4e meters %n",finalResiduals.getLosMaxDistance(),finalResiduals.getLosMeanDistance());
} catch (OrekitException oe) { } catch (OrekitException oe) {
System.err.println(oe.getLocalizedMessage()); System.err.println(oe.getLocalizedMessage());
System.exit(1); System.exit(1);
......
src/tutorials/java/AffinagePleiades/OrbitModel.java
+ 0
− 1
View file @ 3bb6c730
...@@ -21,7 +21,6 @@ import org.hipparchus.ode.nonstiff.DormandPrince853Integrator; ...@@ -21,7 +21,6 @@ import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
import org.hipparchus.util.FastMath; import org.hipparchus.util.FastMath;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.List; import java.util.List;
import java.util.Arrays;
import org.orekit.attitudes.AttitudeProvider; import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.NadirPointing; import org.orekit.attitudes.NadirPointing;
import org.orekit.attitudes.YawCompensation; import org.orekit.attitudes.YawCompensation;
......
src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
+ 60
− 20
View file @ 3bb6c730
...@@ -14,6 +14,7 @@ ...@@ -14,6 +14,7 @@
package AffinagePleiades; package AffinagePleiades;
import org.orekit.rugged.api.SensorToSensorMapping; import org.orekit.rugged.api.SensorToSensorMapping;
import org.orekit.rugged.api.Rugged; import org.orekit.rugged.api.Rugged;
import org.orekit.rugged.linesensor.LineSensor; import org.orekit.rugged.linesensor.LineSensor;
import org.orekit.rugged.linesensor.SensorPixel; import org.orekit.rugged.linesensor.SensorPixel;
...@@ -22,17 +23,13 @@ import org.orekit.rugged.errors.RuggedException; ...@@ -22,17 +23,13 @@ import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.errors.RuggedExceptionWrapper; import org.orekit.rugged.errors.RuggedExceptionWrapper;
import org.orekit.rugged.errors.RuggedMessages; import org.orekit.rugged.errors.RuggedMessages;
import org.orekit.time.AbsoluteDate; import org.orekit.time.AbsoluteDate;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.random.UncorrelatedRandomVectorGenerator; import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
import org.hipparchus.random.GaussianRandomGenerator; import org.hipparchus.random.GaussianRandomGenerator;
import org.hipparchus.random.Well19937a; import org.hipparchus.random.Well19937a;
import org.hipparchus.util.FastMath;
import org.orekit.bodies.GeodeticPoint; import org.orekit.bodies.GeodeticPoint;
import org.orekit.rugged.utils.DSGenerator;
import org.orekit.rugged.utils.SpacecraftToObservedBody;
import java.util.Locale;
/** /**
* class for measure generation * class for measure generation
...@@ -51,7 +48,7 @@ public class SensorToSensorMeasureGenerator { ...@@ -51,7 +48,7 @@ public class SensorToSensorMeasureGenerator {
private LineSensor sensorA; private LineSensor sensorA;
private LineSensor sensorB; //private LineSensor sensorB;
private PleiadesViewingModel viewingModelA; private PleiadesViewingModel viewingModelA;
...@@ -95,7 +92,7 @@ public class SensorToSensorMeasureGenerator { ...@@ -95,7 +92,7 @@ public class SensorToSensorMeasureGenerator {
this.viewingModelB = viewingModelB; this.viewingModelB = viewingModelB;
sensorA = ruggedA.getLineSensor(sensorNameA); sensorA = ruggedA.getLineSensor(sensorNameA);
sensorB = ruggedB.getLineSensor(sensorNameB); //sensorB = ruggedB.getLineSensor(sensorNameB);
measureCount = 0; measureCount = 0;
} }
...@@ -116,16 +113,32 @@ public class SensorToSensorMeasureGenerator { ...@@ -116,16 +113,32 @@ public class SensorToSensorMeasureGenerator {
* @param pixelSampling sampling along columns * @param pixelSampling sampling along columns
* @throws RuggedException * @throws RuggedException
*/ */
public void CreateMeasure(final int lineSampling, final int pixelSampling) public void CreateMeasure(final int lineSampling, final int pixelSampling, double[]err)
throws RuggedException { throws RuggedException {
// Search the sensor pixel seeing point // Search the sensor pixel seeing point
final int minLine = 0; final int minLine = 0;
final int maxLine = viewingModelB.dimension - 1; final int maxLine = viewingModelB.dimension - 1;
double meanA[] = {err[0],err[0]};
double stdA[] = {err[1],err[1]};
double meanB[] = {err[2],err[2]};
double stdB[] = {err[3],err[3]};
GaussianRandomGenerator rngA = new GaussianRandomGenerator(new Well19937a(0xefac03d9be4d24b9l));
UncorrelatedRandomVectorGenerator rvgA = new UncorrelatedRandomVectorGenerator(meanA, stdA, rngA);
GaussianRandomGenerator rngB = new GaussianRandomGenerator(new Well19937a(0xdf1c03d9be0b34b9l));
UncorrelatedRandomVectorGenerator rvgB = new UncorrelatedRandomVectorGenerator(meanB, stdB, rngB);
for (double line = 0; line < viewingModelA.dimension; line += lineSampling) { for (double line = 0; line < viewingModelA.dimension; line += lineSampling) {
AbsoluteDate dateA = sensorA.getDate(line); AbsoluteDate dateA = sensorA.getDate(line);
for (int pixelA = 0; pixelA < sensorA for (double pixelA = 0; pixelA < sensorA
.getNbPixels(); pixelA += pixelSampling) { .getNbPixels(); pixelA += pixelSampling) {
GeodeticPoint gpA = ruggedA GeodeticPoint gpA = ruggedA
...@@ -137,24 +150,51 @@ public class SensorToSensorMeasureGenerator { ...@@ -137,24 +150,51 @@ public class SensorToSensorMeasureGenerator {
// we need to test if the sensor pixel is found in the // we need to test if the sensor pixel is found in the
// prescribed lines otherwise the sensor pixel is null // prescribed lines otherwise the sensor pixel is null
if (sensorPixelB != null) { if (sensorPixelB != null) {
//System.out.format(" %n");
/*final AbsoluteDate dateB = sensorB /*final AbsoluteDate dateB = sensorB
.getDate(sensorPixelB.getLineNumber()); .getDate(sensorPixelB.getLineNumber());
double pixelB = sensorPixelB.getPixelNumber();*/ double pixelB = sensorPixelB.getPixelNumber();*/
// Get spacecraft to body transform of rugged instance A // Get spacecraft to body transform of rugged instance A
//final SpacecraftToObservedBody scToBodyA = ruggedA /*
// .getScToBody(); final SpacecraftToObservedBody scToBodyA = ruggedA
//final SpacecraftToObservedBody scToBodyB = ruggedB .getScToBody();
// .getScToBody(); final SpacecraftToObservedBody scToBodyB = ruggedB
.getScToBody();
/*double distanceB = ruggedB double distanceLOSB = ruggedB
.distanceBetweenLOS(sensorA, dateA, pixelA, scToBodyA, .distanceBetweenLOS(sensorA, dateA, pixelA, scToBodyA,
sensorB, dateB, pixelB);*/ sensorB, dateB, pixelB);
double distanceLOSA = ruggedA
.distanceBetweenLOS(sensorB, dateB, pixelB, scToBodyB,
sensorA, dateA, pixelA);
*/
//System.out.format("distance LOS %1.8e %1.8e %n",distanceLOSB,distanceLOSA);
/*
GeodeticPoint gpB = ruggedB
.directLocation(dateB, sensorB.getPosition(),
sensorB.getLOS(dateB, pixelB));
double GEOdistance = DistanceTools.computeDistanceRad(gpA.getLongitude(), gpA.getLatitude(),
gpB.getLongitude(), gpB.getLatitude());
*/
//System.out.format("distance %1.8e meters %n",GEOdistance);
final double distance = 0.0; final double distance = 0.0;
mapping.addMapping(new SensorPixel(line, pixelA),
sensorPixelB, distance); //
final double[] vecRandomA = rvgA.nextVector();
final double[] vecRandomB = rvgB.nextVector();
SensorPixel RealPixelA = new SensorPixel(line + vecRandomA[0], pixelA+ vecRandomA[1]);
SensorPixel RealPixelB = new SensorPixel(sensorPixelB.getLineNumber() + vecRandomB[0], sensorPixelB.getPixelNumber()+ vecRandomB[1]);
//System.out.format("pix A %f %f Real %f %f %n", line, pixelA, RealPixelA.getLineNumber(), RealPixelA.getPixelNumber());
//System.out.format("pix B %f %f Real %f %f %n", sensorPixelB.getLineNumber(), sensorPixelB.getPixelNumber(), RealPixelB.getLineNumber(), RealPixelB.getPixelNumber());
mapping.addMapping(RealPixelA,
RealPixelB, distance);
measureCount++; measureCount++;
} }
......
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