1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.rugged.linesensor;
18
19 import java.io.File;
20 import java.lang.reflect.InvocationTargetException;
21 import java.lang.reflect.Method;
22 import java.net.URISyntaxException;
23 import java.util.ArrayList;
24 import java.util.List;
25
26 import org.hipparchus.geometry.euclidean.threed.Line;
27 import org.hipparchus.geometry.euclidean.threed.Vector3D;
28 import org.hipparchus.random.RandomGenerator;
29 import org.hipparchus.random.Well19937a;
30 import org.hipparchus.util.FastMath;
31 import org.junit.Assert;
32 import org.junit.Before;
33 import org.junit.Test;
34 import org.orekit.attitudes.NadirPointing;
35 import org.orekit.attitudes.YawCompensation;
36 import org.orekit.bodies.BodyShape;
37 import org.orekit.bodies.GeodeticPoint;
38 import org.orekit.bodies.OneAxisEllipsoid;
39 import org.orekit.data.DataContext;
40 import org.orekit.data.DirectoryCrawler;
41 import org.orekit.frames.FramesFactory;
42 import org.orekit.frames.Transform;
43 import org.orekit.orbits.CircularOrbit;
44 import org.orekit.orbits.Orbit;
45 import org.orekit.orbits.PositionAngleType;
46 import org.orekit.propagation.Propagator;
47 import org.orekit.propagation.analytical.KeplerianPropagator;
48 import org.orekit.rugged.TestUtils;
49 import org.orekit.rugged.errors.RuggedException;
50 import org.orekit.rugged.errors.RuggedMessages;
51 import org.orekit.rugged.linesensor.SensorMeanPlaneCrossing.CrossingResult;
52 import org.orekit.rugged.los.LOSBuilder;
53 import org.orekit.rugged.utils.SpacecraftToObservedBody;
54 import org.orekit.time.AbsoluteDate;
55 import org.orekit.utils.AngularDerivativesFilter;
56 import org.orekit.utils.CartesianDerivativesFilter;
57 import org.orekit.utils.Constants;
58 import org.orekit.utils.IERSConventions;
59 import org.orekit.utils.PVCoordinates;
60 import org.orekit.utils.TimeStampedAngularCoordinates;
61 import org.orekit.utils.TimeStampedPVCoordinates;
62
63 public class SensorMeanPlaneCrossingTest {
64
65 @Test
66 public void testPerfectLine() {
67
68 final Vector3D position = new Vector3D(1.5, Vector3D.PLUS_I);
69 final Vector3D normal = Vector3D.PLUS_I;
70 final Vector3D fovCenter = Vector3D.PLUS_K;
71 final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
72
73
74 final List<Vector3D> los = new ArrayList<Vector3D>();
75 for (int i = -1000; i <= 1000; ++i) {
76 final double alpha = i * 0.17 / 1000;
77 los.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross));
78 }
79
80 final LineSensor sensor = new LineSensor("perfect line",
81 new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3),
82 position, new LOSBuilder(los).build());
83
84 Assert.assertEquals("perfect line", sensor.getName());
85 Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0));
86 Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-15);
87
88 SensorMeanPlaneCrossing mean = new SensorMeanPlaneCrossing(sensor, createInterpolator(sensor),
89 0, 2000, true, true, 50, 0.01);
90 Assert.assertEquals(0.0, Vector3D.angle(normal, mean.getMeanPlaneNormal()), 1.0e-15);
91
92 }
93
94 @Test
95 public void testNoisyLine() {
96
97 final RandomGenerator random = new Well19937a(0xf3ddb33785e12bdal);
98 final Vector3D position = new Vector3D(1.5, Vector3D.PLUS_I);
99 final Vector3D normal = Vector3D.PLUS_I;
100 final Vector3D fovCenter = Vector3D.PLUS_K;
101 final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
102
103
104 final List<Vector3D> los = new ArrayList<Vector3D>();
105 for (int i = -1000; i <= 1000; ++i) {
106 final double alpha = i * 0.17 / 10 + 1.0e-5 * random.nextDouble();
107 final double delta = 1.0e-5 * random.nextDouble();
108 final double cA = FastMath.cos(alpha);
109 final double sA = FastMath.sin(alpha);
110 final double cD = FastMath.cos(delta);
111 final double sD = FastMath.sin(delta);
112 los.add(new Vector3D(cA * cD, fovCenter, sA * cD, cross, sD, normal));
113 }
114
115 final LineSensor sensor = new LineSensor("noisy line",
116 new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3),
117 position, new LOSBuilder(los).build());
118
119 Assert.assertEquals("noisy line", sensor.getName());
120 Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0));
121 Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-5);
122
123 SensorMeanPlaneCrossing mean = new SensorMeanPlaneCrossing(sensor, createInterpolator(sensor),
124 0, 2000, true, true, 50, 0.01);
125 Assert.assertEquals(0.0, Vector3D.angle(normal, mean.getMeanPlaneNormal()), 8.0e-7);
126
127 }
128
129 @Test
130 public void testDerivativeWithoutCorrections() {
131 doTestDerivative(false, false, 3.1e-11);
132 }
133
134 @Test
135 public void testDerivativeLightTimeCorrection() {
136 doTestDerivative(true, false, 2.4e-7);
137 }
138
139 @Test
140 public void testDerivativeAberrationOfLightCorrection() {
141 doTestDerivative(false, true, 1.1e-7);
142 }
143
144 @Test
145 public void testDerivativeWithAllCorrections() {
146 doTestDerivative(true, true, 1.4e-7);
147 }
148
149 private void doTestDerivative(boolean lightTimeCorrection,
150 boolean aberrationOfLightCorrection,
151 double tol) {
152
153 final Vector3D position = new Vector3D(1.5, Vector3D.PLUS_I);
154 final Vector3D normal = Vector3D.PLUS_I;
155 final Vector3D fovCenter = Vector3D.PLUS_K;
156 final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
157
158
159 final List<Vector3D> los = new ArrayList<Vector3D>();
160 for (int i = -1000; i <= 1000; ++i) {
161 final double alpha = i * 0.17 / 1000;
162 los.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross));
163 }
164
165 final LineSensor sensor = new LineSensor("perfect line",
166 new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3),
167 position, new LOSBuilder(los).build());
168
169 Assert.assertEquals("perfect line", sensor.getName());
170 Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0));
171 Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-15);
172
173 SensorMeanPlaneCrossing mean = new SensorMeanPlaneCrossing(sensor, createInterpolator(sensor),
174 0, 2000, lightTimeCorrection,
175 aberrationOfLightCorrection, 50, 1.0e-6);
176
177 double refLine = 1200.0;
178 AbsoluteDate refDate = sensor.getDate(refLine);
179 int refPixel= 1800;
180 Transform b2i = mean.getScToBody().getBodyToInertial(refDate);
181 Transform sc2i = mean.getScToBody().getScToInertial(refDate);
182 Transform sc2b = new Transform(refDate, sc2i, b2i.getInverse());
183 Vector3D p1 = sc2b.transformPosition(position);
184 Vector3D p2 = sc2b.transformPosition(new Vector3D(1, position,
185 1.0e6, los.get(refPixel)));
186 Line line = new Line(p1, p2, 0.001);
187 BodyShape earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
188 Constants.WGS84_EARTH_FLATTENING,
189 mean.getScToBody().getBodyFrame());
190 GeodeticPoint groundPoint = earth.getIntersectionPoint(line, p1, mean.getScToBody().getBodyFrame(), refDate);
191 Vector3D gpCartesian = earth.transform(groundPoint);
192 SensorMeanPlaneCrossing.CrossingResult result = mean.find(gpCartesian);
193
194 if (lightTimeCorrection) {
195
196
197 Assert.assertTrue(result.getLine() - refLine > 0.02);
198 } else if (aberrationOfLightCorrection) {
199
200
201 Assert.assertTrue(result.getLine() - refLine > 1.9);
202 } else {
203
204 Assert.assertEquals(refLine, result.getLine(), 5.0e-11* refLine);
205 Assert.assertEquals(0.0, result.getDate().durationFrom(refDate), 1.0e-9);
206 Assert.assertEquals(0.0, Vector3D.angle(los.get(refPixel), result.getTargetDirection()), 7.6e-15);
207 }
208
209 double deltaL = 0.5;
210 Transform b2scPlus = sc2b.getInverse().shiftedBy(deltaL / sensor.getRate(refLine));
211 Vector3D dirPlus = b2scPlus.transformPosition(gpCartesian).subtract(position).normalize();
212 Transform b2scMinus = sc2b.getInverse().shiftedBy(-deltaL / sensor.getRate(refLine));
213 Vector3D dirMinus = b2scMinus.transformPosition(gpCartesian).subtract(position).normalize();
214 Vector3D dirDer = new Vector3D(1.0 / (2 * deltaL), dirPlus.subtract(dirMinus));
215 Assert.assertEquals(0.0,
216 Vector3D.distance(result.getTargetDirectionDerivative(), dirDer),
217 tol * dirDer.getNorm());
218
219 try {
220 mean.getScToBody().getBodyToInertial(refDate.shiftedBy(-Constants.JULIAN_CENTURY));
221 Assert.fail("an exception should have been thrown");
222 } catch (RuggedException re) {
223 Assert.assertEquals(RuggedMessages.OUT_OF_TIME_RANGE, re.getSpecifier());
224 }
225 try {
226 mean.getScToBody().getBodyToInertial(refDate.shiftedBy(Constants.JULIAN_CENTURY));
227 Assert.fail("an exception should have been thrown");
228 } catch (RuggedException re) {
229 Assert.assertEquals(RuggedMessages.OUT_OF_TIME_RANGE, re.getSpecifier());
230 }
231 Assert.assertNotNull(mean.getScToBody().getBodyToInertial(refDate));
232
233 }
234
235 @Test
236 public void testSlowFind()
237 throws NoSuchMethodException,
238 SecurityException, IllegalAccessException, IllegalArgumentException,
239 InvocationTargetException {
240
241 final Vector3D position = new Vector3D(1.5, Vector3D.PLUS_I);
242 final Vector3D normal = Vector3D.PLUS_I;
243 final Vector3D fovCenter = Vector3D.PLUS_K;
244 final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
245
246
247 final List<Vector3D> los = new ArrayList<Vector3D>();
248 for (int i = -1000; i <= 1000; ++i) {
249 final double alpha = i * 0.17 / 1000;
250 los.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross));
251 }
252
253 final LineSensor sensor = new LineSensor("perfect line",
254 new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3),
255 position, new LOSBuilder(los).build());
256
257 Assert.assertEquals("perfect line", sensor.getName());
258 Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0));
259 Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-15);
260
261 SensorMeanPlaneCrossing mean = new SensorMeanPlaneCrossing(sensor, createInterpolator(sensor),
262 0, 2000, true, true, 50, 1.0e-6);
263
264 double refLine = 1200.0;
265 AbsoluteDate refDate = sensor.getDate(refLine);
266 int refPixel= 1800;
267 Transform b2i = mean.getScToBody().getBodyToInertial(refDate);
268 Transform sc2i = mean.getScToBody().getScToInertial(refDate);
269 Transform sc2b = new Transform(refDate, sc2i, b2i.getInverse());
270 Vector3D p1 = sc2b.transformPosition(position);
271 Vector3D p2 = sc2b.transformPosition(new Vector3D(1, position,
272 1.0e6, los.get(refPixel)));
273 Line line = new Line(p1, p2, 0.001);
274 BodyShape earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
275 Constants.WGS84_EARTH_FLATTENING,
276 mean.getScToBody().getBodyFrame());
277 GeodeticPoint groundPoint = earth.getIntersectionPoint(line, p1, mean.getScToBody().getBodyFrame(), refDate);
278 Vector3D gpCartesian = earth.transform(groundPoint);
279 SensorMeanPlaneCrossing.CrossingResult result = mean.find(gpCartesian);
280
281 Method slowFind = SensorMeanPlaneCrossing.class.getDeclaredMethod("slowFind",
282 PVCoordinates.class,
283 Double.TYPE);
284 slowFind.setAccessible(true);
285 SensorMeanPlaneCrossing.CrossingResult slowResult =
286 (CrossingResult) slowFind.invoke(mean,
287 new PVCoordinates(gpCartesian, Vector3D.ZERO),
288 400.0);
289
290 Assert.assertEquals(result.getLine(), slowResult.getLine(), 2.0e-8);
291 Assert.assertEquals(0.0,
292 Vector3D.distance(result.getTargetDirection(),
293 slowResult.getTargetDirection()),
294 2.0e-13);
295 Assert.assertEquals(0.0,
296 Vector3D.distance(result.getTargetDirectionDerivative(),
297 slowResult.getTargetDirectionDerivative()),
298 1.0e-15);
299 }
300
301 private SpacecraftToObservedBody createInterpolator(LineSensor sensor) {
302
303 Orbit orbit = new CircularOrbit(7173352.811913891,
304 -4.029194321683225E-4, 0.0013530362644647786,
305 FastMath.toRadians(98.63218182243709),
306 FastMath.toRadians(77.55565567747836),
307 FastMath.PI, PositionAngleType.TRUE,
308 FramesFactory.getEME2000(), sensor.getDate(1000),
309 Constants.EIGEN5C_EARTH_MU);
310 BodyShape earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
311 Constants.WGS84_EARTH_FLATTENING,
312 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
313 AbsoluteDate minDate = sensor.getDate(0);
314 AbsoluteDate maxDate = sensor.getDate(2000);
315 return new SpacecraftToObservedBody(orbit.getFrame(), earth.getBodyFrame(),
316 minDate, maxDate, 0.01,
317 5.0,
318 orbitToPV(orbit, earth, minDate, maxDate, 0.25), 2,
319 CartesianDerivativesFilter.USE_P,
320 orbitToQ(orbit, earth, minDate, maxDate, 0.25), 2,
321 AngularDerivativesFilter.USE_R);
322 }
323
324 private List<TimeStampedPVCoordinates> orbitToPV(Orbit orbit, BodyShape earth,
325 AbsoluteDate minDate, AbsoluteDate maxDate,
326 double step) {
327
328 Propagator propagator = new KeplerianPropagator(orbit);
329 propagator.setAttitudeProvider(new YawCompensation(orbit.getFrame(), new NadirPointing(orbit.getFrame(), earth)));
330 propagator.propagate(minDate);
331 final List<TimeStampedPVCoordinates> list = new ArrayList<TimeStampedPVCoordinates>();
332 propagator.getMultiplexer().add(step, currentState -> list.add(new TimeStampedPVCoordinates(currentState.getDate(),
333 currentState.getPVCoordinates().getPosition(),
334 currentState.getPVCoordinates().getVelocity(),
335 Vector3D.ZERO)));
336 propagator.propagate(maxDate);
337 return list;
338 }
339
340 private List<TimeStampedAngularCoordinates> orbitToQ(Orbit orbit, BodyShape earth,
341 AbsoluteDate minDate, AbsoluteDate maxDate,
342 double step) {
343
344 Propagator propagator = new KeplerianPropagator(orbit);
345 propagator.setAttitudeProvider(new YawCompensation(orbit.getFrame(), new NadirPointing(orbit.getFrame(), earth)));
346 propagator.propagate(minDate);
347 final List<TimeStampedAngularCoordinates> list = new ArrayList<TimeStampedAngularCoordinates>();
348 propagator.getMultiplexer().add(step, currentState -> list.add(new TimeStampedAngularCoordinates(currentState.getDate(),
349 currentState.getAttitude().getRotation(),
350 Vector3D.ZERO, Vector3D.ZERO)));
351 propagator.propagate(maxDate);
352 return list;
353 }
354
355 @Before
356 public void setUp() throws URISyntaxException {
357 TestUtils.clearFactories();
358 String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
359 DataContext.getDefault().getDataProvidersManager().addProvider(new DirectoryCrawler(new File(path)));
360 }
361
362 }