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

Started implementation of inverse localization (not working yet).

parent 88e35385
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/main/java/org/orekit/rugged/api/Rugged.java
+ 165
− 7
View file @ d7a4b6d4
......@@ -21,8 +21,14 @@ import java.util.HashMap;
import java.util.List;
import java.util.Map;
import org.apache.commons.math3.analysis.UnivariateFunction;
import org.apache.commons.math3.analysis.solvers.BracketingNthOrderBrentSolver;
import org.apache.commons.math3.analysis.solvers.UnivariateSolver;
import org.apache.commons.math3.exception.NoBracketingException;
import org.apache.commons.math3.exception.TooManyEvaluationsException;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath;
import org.apache.commons.math3.util.Pair;
import org.orekit.attitudes.Attitude;
import org.orekit.attitudes.AttitudeProvider;
......@@ -30,6 +36,7 @@ import org.orekit.attitudes.TabulatedProvider;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitExceptionWrapper;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.Transform;
......@@ -55,6 +62,12 @@ import org.orekit.utils.PVCoordinatesProvider;
*/
public class Rugged {
/** Absolute accuracy to use for inverse localization. */
private static final double INVERSE_LOCALIZATION_ACCURACY = 1.0e-4;
/** Maximum number of evaluations for inverse localization. */
private static final int INVERSE_LOCALIZATION_MAX_EVAL = 1000;
/** Reference date. */
private final AbsoluteDate referenceDate;
......@@ -528,8 +541,8 @@ public class Rugged {
lInert = new Vector3D(Constants.SPEED_OF_LIGHT, scToInert.transformVector(sensor.getLos(i)),
1.0, spacecraftVelocity).normalize();
} else {
lInert = scToInert.transformVector(sensor.getLos(i));
// don't apply aberration of light correction
lInert = scToInert.transformVector(sensor.getLos(i));
}
final Vector3D pBody;
......@@ -564,17 +577,162 @@ public class Rugged {
/** Inverse localization of a ground point.
* @param sensorName name of the line sensor
* @param groundPoint ground point to localize
* @return sensor pixel seeing ground point
* @param minLine minimum line number
* @param maxLine maximum line number
* @return sensor pixel seeing ground point, or null if ground point cannot
* be seen between the prescribed line numbers
* @exception RuggedException if line cannot be localized, or sensor is unknown
*/
public SensorPixel inverseLocalization(final String sensorName, final GeodeticPoint groundPoint)
public SensorPixel inverseLocalization(final String sensorName, final GeodeticPoint groundPoint,
final double minLine, final double maxLine)
throws RuggedException {
try {
checkContext();
final Sensor sensor = getSensor(sensorName);
final Vector3D target = ellipsoid.transform(groundPoint);
final UnivariateSolver solver = new BracketingNthOrderBrentSolver(INVERSE_LOCALIZATION_ACCURACY, 5);
// find the line at which ground point crosses sensor mean plane
final SensorMeanPlaneCrossing planeCrossing = new SensorMeanPlaneCrossing(sensor, target);
final double meanLine = solver.solve(INVERSE_LOCALIZATION_MAX_EVAL, planeCrossing, minLine, maxLine);
final Vector3D targetDirection = planeCrossing.targetDirection(meanLine);
// find the pixel along the line
final double meanPixel = solver.solve(INVERSE_LOCALIZATION_MAX_EVAL,
new SensorPixelCrossing(sensor, targetDirection),
0, sensor.getNbPixels());
// TODO: fix pixel offset with respect to mean sensor plane
final double fixedLine = meanLine;
final double fixedPixel = meanPixel;
return new SensorPixel(fixedLine, fixedPixel);
} catch (NoBracketingException nbe) {
// there are no solutions in the search interval
return null;
} catch (TooManyEvaluationsException tmee) {
throw new RuggedException(tmee);
} catch (OrekitExceptionWrapper oew) {
final OrekitException oe = oew.getException();
throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
}
}
/** Local class for finding when ground point crosses mean sensor plane. */
private class SensorMeanPlaneCrossing implements UnivariateFunction {
/** Line sensor. */
private final Sensor sensor;
/** Target ground point. */
private final Vector3D target;
/** Simple constructor.
* @param sensor sensor to consider
* @param target target ground point
*/
public SensorMeanPlaneCrossing(final Sensor sensor, final Vector3D target) {
this.sensor = sensor;
this.target = target;
}
/** {@inheritDoc} */
@Override
public double value(final double lineNumber) throws OrekitExceptionWrapper {
// the target crosses the mean plane if it orthogonal to the normal vector
return Vector3D.dotProduct(targetDirection(lineNumber), sensor.getMeanPlaneNormal());
}
/** Compute target point direction in spacecraft frame, at line acquisition time.
* @param lineNumber line being acquired
* @return target point direction in spacecraft frame
* @exception OrekitExceptionWrapper if some frame conversion fails
*/
public Vector3D targetDirection(final double lineNumber) throws OrekitExceptionWrapper {
try {
// compute the transform between spacecraft and observed body
final AbsoluteDate date = sensor.getDate(lineNumber);
final Transform bodyToInert = scToBody.getInertialToBody(date).getInverse();
final Transform scToInert = scToBody.getScToInertial(date);
final Vector3D meanRefInert = scToInert.transformPosition(sensor.getMeanPlaneReferencePoint());
final Transform shifted;
if (lightTimeCorrection) {
// apply light time correction
final Vector3D iT = bodyToInert.transformPosition(target);
final double deltaT = meanRefInert.distance(iT) / Constants.SPEED_OF_LIGHT;
shifted = bodyToInert.shiftedBy(-deltaT);
} else {
// don't apply light time correction
shifted = bodyToInert;
}
final Vector3D targetInert = shifted.transformPosition(target);
Vector3D lInert;
if (aberrationOfLightCorrection) {
// apply aberration of light correction
// as the spacecraft velocity is small with respect to speed of light,
// we use classical velocity addition and not relativistic velocity addition
final Vector3D spacecraftVelocity =
scToInert.transformPVCoordinates(PVCoordinates.ZERO).getVelocity();
lInert = new Vector3D(Constants.SPEED_OF_LIGHT, targetInert.subtract(meanRefInert).normalize(),
1.0, spacecraftVelocity).normalize();
} else {
// don't apply aberration of light correction
lInert = targetInert.subtract(meanRefInert).normalize();
}
// direction of the target point in spacecraft frame
return scToInert.getInverse().transformVector(lInert);
} catch (OrekitException oe) {
throw new OrekitExceptionWrapper(oe);
}
}
checkContext();
final Sensor sensor = getSensor(sensorName);
}
/** Local class for finding when ground point crosses pixel along sensor line. */
private class SensorPixelCrossing implements UnivariateFunction {
/** Line sensor. */
private final Sensor sensor;
/** Cross direction in spacecraft frame. */
private final Vector3D cross;
/** Simple constructor.
* @param sensor sensor to consider
* @param targetDirection target direction in spacecraft frame
*/
public SensorPixelCrossing(final Sensor sensor, final Vector3D targetDirection) {
this.sensor = sensor;
this.cross = Vector3D.crossProduct(sensor.getMeanPlaneNormal(), targetDirection).normalize();
}
// TODO: implement direct localization
throw RuggedException.createInternalError(null);
/** {@inheritDoc} */
@Override
public double value(final double x) {
return Vector3D.dotProduct(cross, getLOS(x));
}
/** Interpolate sensor pixels at some pixel index.
* @param x pixel index
* @return interpolated direction for specified index
*/
public Vector3D getLOS(final double x) {
// find surrounding pixels
final int iInf = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(x)));
final int iSup = iInf + 1;
// interpolate
return new Vector3D(iSup - x, sensor.getLos(iInf), x - iInf, sensor.getLos(iSup)).normalize();
}
}
......
src/test/java/org/orekit/rugged/api/RuggedTest.java
+ 56
− 0
View file @ d7a4b6d4
......@@ -456,6 +456,62 @@ public class RuggedTest {
}
@Test
public void testInverseLocalization()
throws RuggedException, OrekitException, URISyntaxException {
int dimension = 200;
String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(path)));
final BodyShape earth = createEarth();
final Orbit orbit = createOrbit(Constants.EIGEN5C_EARTH_MU);
AbsoluteDate crossing = new AbsoluteDate("2012-01-01T12:30:00.000", TimeScalesFactory.getUTC());
// one line sensor
// position: 1.5m in front (+X) and 20 cm above (-Z) of the S/C center of mass
// los: swath in the (YZ) plane, looking at 50° roll, ±1° aperture
List<PixelLOS> los = createLOS(new Vector3D(1.5, 0, -0.2),
new Rotation(Vector3D.PLUS_I, FastMath.toRadians(50.0)).applyTo(Vector3D.PLUS_K),
Vector3D.PLUS_I,
FastMath.toRadians(1.0), dimension);
// linear datation model: at reference time we get line 100, and the rate is one line every 1.5ms
LineDatation lineDatation = new LinearLineDatation(dimension / 2, 1.0 / 1.5e-3);
int firstLine = 0;
int lastLine = dimension;
Propagator propagator = new KeplerianPropagator(orbit);
propagator.setAttitudeProvider(new YawCompensation(new NadirPointing(earth)));
propagator.propagate(crossing.shiftedBy(lineDatation.getDate(firstLine) - 1.0));
propagator.setEphemerisMode();
propagator.propagate(crossing.shiftedBy(lineDatation.getDate(lastLine) + 1.0));
Propagator ephemeris = propagator.getGeneratedEphemeris();
TileUpdater updater =
new RandomLandscapeUpdater(0.0, 9000.0, 0.5, 0xf0a401650191f9f6l,
FastMath.toRadians(1.0), 257);
Rugged rugged = new Rugged(crossing, updater, 8,
AlgorithmId.DUVENHAGE,
EllipsoidId.WGS84,
InertialFrameId.EME2000,
BodyRotatingFrameId.ITRF,
ephemeris);
rugged.setLineSensor("line", los, lineDatation);
GeodeticPoint[] gp = rugged.directLocalization("line", 100.24);
GeodeticPoint target =
new GeodeticPoint(0.75 * gp[17].getLatitude() + 0.25 * gp[18].getLatitude(),
0.75 * gp[17].getLongitude() + 0.25 * gp[18].getLongitude(),
0.75 * gp[17].getAltitude() + 0.25 * gp[18].getAltitude());
SensorPixel sp = rugged.inverseLocalization("line", target, 0.0, 200.0);
Assert.assertEquals(100.24, sp.getLineNumber(), 1.0e-10);
Assert.assertEquals( 17.25, sp.getPixelNumber(), 1.0e-10);
}
private BodyShape createEarth()
throws OrekitException {
return new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
......
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