1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.attitudes;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.geometry.euclidean.threed.Line;
22 import org.hipparchus.geometry.euclidean.threed.Rotation;
23 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.util.Binary64Field;
26 import org.hipparchus.util.FastMath;
27 import org.junit.jupiter.api.AfterEach;
28 import org.junit.jupiter.api.Assertions;
29 import org.junit.jupiter.api.BeforeEach;
30 import org.junit.jupiter.api.Test;
31 import org.orekit.Utils;
32 import org.orekit.bodies.GeodeticPoint;
33 import org.orekit.bodies.OneAxisEllipsoid;
34 import org.orekit.errors.OrekitException;
35 import org.orekit.frames.Frame;
36 import org.orekit.frames.FramesFactory;
37 import org.orekit.frames.Transform;
38 import org.orekit.orbits.CircularOrbit;
39 import org.orekit.orbits.FieldOrbit;
40 import org.orekit.orbits.KeplerianOrbit;
41 import org.orekit.orbits.Orbit;
42 import org.orekit.orbits.PositionAngleType;
43 import org.orekit.propagation.FieldSpacecraftState;
44 import org.orekit.propagation.Propagator;
45 import org.orekit.propagation.SpacecraftState;
46 import org.orekit.propagation.analytical.KeplerianPropagator;
47 import org.orekit.time.AbsoluteDate;
48 import org.orekit.time.DateComponents;
49 import org.orekit.time.FieldAbsoluteDate;
50 import org.orekit.time.TimeComponents;
51 import org.orekit.time.TimeInterpolator;
52 import org.orekit.time.TimeScalesFactory;
53 import org.orekit.utils.AngularCoordinates;
54 import org.orekit.utils.CartesianDerivativesFilter;
55 import org.orekit.utils.Constants;
56 import org.orekit.utils.IERSConventions;
57 import org.orekit.utils.PVCoordinates;
58 import org.orekit.utils.TimeStampedFieldPVCoordinates;
59 import org.orekit.utils.TimeStampedPVCoordinates;
60 import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;
61
62 import java.util.ArrayList;
63 import java.util.List;
64
65
66 class YawCompensationTest {
67
68
69 private AbsoluteDate date;
70
71
72 private Frame itrf;
73
74
75 CircularOrbit circOrbit;
76
77
78 OneAxisEllipsoid earthShape;
79
80
81
82 @Test
83 void testTarget() {
84
85
86
87
88 NadirPointing nadirLaw = new NadirPointing(circOrbit.getFrame(), earthShape);
89
90
91 YawCompensation yawCompensLaw = new YawCompensation(circOrbit.getFrame(), nadirLaw);
92
93
94
95
96 TimeStampedPVCoordinates noYawObserved = nadirLaw.getTargetPV(circOrbit, date, itrf);
97
98
99 TimeStampedPVCoordinates yawObserved = yawCompensLaw.getTargetPV(circOrbit, date, itrf);
100
101
102 PVCoordinates observedDiff = new PVCoordinates(yawObserved, noYawObserved);
103
104 Assertions.assertEquals(0.0, observedDiff.getPosition().getNorm(), Utils.epsilonTest);
105 Assertions.assertEquals(0.0, observedDiff.getVelocity().getNorm(), Utils.epsilonTest);
106 Assertions.assertEquals(0.0, observedDiff.getAcceleration().getNorm(), Utils.epsilonTest);
107 Assertions.assertSame(nadirLaw, yawCompensLaw.getUnderlyingAttitudeProvider());
108
109 }
110
111
112
113 @Test
114 void testSlidingDerivatives() {
115
116 GroundPointing law =
117 new YawCompensation(circOrbit.getFrame(), new NadirPointing(circOrbit.getFrame(), earthShape));
118
119 List<TimeStampedPVCoordinates> sample = new ArrayList<>();
120 for (double dt = -0.1; dt < 0.1; dt += 0.01) {
121 Orbit o = circOrbit.shiftedBy(dt);
122 sample.add(law.getTargetPV(o, o.getDate(), o.getFrame()));
123 }
124
125
126 final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
127 new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_P);
128
129 TimeStampedPVCoordinates reference = interpolator.interpolate(circOrbit.getDate(), sample);
130
131 TimeStampedPVCoordinates target =
132 law.getTargetPV(circOrbit, circOrbit.getDate(), circOrbit.getFrame());
133
134 Assertions.assertEquals(0.0,
135 Vector3D.distance(reference.getPosition(), target.getPosition()),
136 1.0e-15 * reference.getPosition().getNorm());
137 Assertions.assertEquals(0.0,
138 Vector3D.distance(reference.getVelocity(), target.getVelocity()),
139 3.0e-11 * reference.getVelocity().getNorm());
140 Assertions.assertEquals(0.0,
141 Vector3D.distance(reference.getAcceleration(), target.getAcceleration()),
142 1.0e-5 * reference.getAcceleration().getNorm());
143
144 }
145
146
147
148 @Test
149 void testAlignment() {
150
151 GroundPointing notCompensated = new NadirPointing(circOrbit.getFrame(), earthShape);
152 YawCompensation compensated = new YawCompensation(circOrbit.getFrame(), notCompensated);
153 Attitude att0 = compensated.getAttitude(circOrbit, circOrbit.getDate(), circOrbit.getFrame());
154
155
156 Vector3D satInert = circOrbit.getPosition();
157 Vector3D zInert = att0.getRotation().applyInverseTo(Vector3D.PLUS_K);
158 GeodeticPoint gp = earthShape.getIntersectionPoint(new Line(satInert,
159 satInert.add(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, zInert),
160 1.0e-10),
161 satInert,
162 circOrbit.getFrame(), circOrbit.getDate());
163 PVCoordinates pEarth = new PVCoordinates(earthShape.transform(gp), Vector3D.ZERO, Vector3D.ZERO);
164
165 double minYWithoutCompensation = Double.POSITIVE_INFINITY;
166 double maxYWithoutCompensation = Double.NEGATIVE_INFINITY;
167 double minYDotWithoutCompensation = Double.POSITIVE_INFINITY;
168 double maxYDotWithoutCompensation = Double.NEGATIVE_INFINITY;
169 double minYWithCompensation = Double.POSITIVE_INFINITY;
170 double maxYWithCompensation = Double.NEGATIVE_INFINITY;
171 double minYDotWithCompensation = Double.POSITIVE_INFINITY;
172 double maxYDotWithCompensation = Double.NEGATIVE_INFINITY;
173 for (double dt = -0.2; dt < 0.2; dt += 0.002) {
174
175 PVCoordinates withoutCompensation = toSpacecraft(pEarth, circOrbit.shiftedBy(dt), notCompensated);
176 if (FastMath.abs(withoutCompensation.getPosition().getX()) <= 1000.0) {
177 minYWithoutCompensation = FastMath.min(minYWithoutCompensation, withoutCompensation.getPosition().getY());
178 maxYWithoutCompensation = FastMath.max(maxYWithoutCompensation, withoutCompensation.getPosition().getY());
179 minYDotWithoutCompensation = FastMath.min(minYDotWithoutCompensation, withoutCompensation.getVelocity().getY());
180 maxYDotWithoutCompensation = FastMath.max(maxYDotWithoutCompensation, withoutCompensation.getVelocity().getY());
181 }
182
183 PVCoordinates withCompensation = toSpacecraft(pEarth, circOrbit.shiftedBy(dt), compensated);
184 if (FastMath.abs(withCompensation.getPosition().getX()) <= 1000.0) {
185 minYWithCompensation = FastMath.min(minYWithCompensation, withCompensation.getPosition().getY());
186 maxYWithCompensation = FastMath.max(maxYWithCompensation, withCompensation.getPosition().getY());
187 minYDotWithCompensation = FastMath.min(minYDotWithCompensation, withCompensation.getVelocity().getY());
188 maxYDotWithCompensation = FastMath.max(maxYDotWithCompensation, withCompensation.getVelocity().getY());
189 }
190
191 }
192
193
194
195
196
197
198 Assertions.assertEquals(-55.7056, minYWithoutCompensation, 0.0001);
199 Assertions.assertEquals(+55.7056, maxYWithoutCompensation, 0.0001);
200 Assertions.assertEquals(352.5667, minYDotWithoutCompensation, 0.0001);
201 Assertions.assertEquals(352.5677, maxYDotWithoutCompensation, 0.0001);
202 Assertions.assertEquals( 0.0000, minYWithCompensation, 0.0001);
203 Assertions.assertEquals( 0.0008, maxYWithCompensation, 0.0001);
204 Assertions.assertEquals( -0.0101, minYDotWithCompensation, 0.0001);
205 Assertions.assertEquals( 0.0102, maxYDotWithCompensation, 0.0001);
206
207 }
208
209 PVCoordinates toSpacecraft(PVCoordinates groundPoint, Orbit orbit, AttitudeProvider attitudeProvider)
210 {
211 SpacecraftState state =
212 new SpacecraftState(orbit, attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame()));
213 Transform earthToSc =
214 new Transform(orbit.getDate(),
215 earthShape.getBodyFrame().getTransformTo(orbit.getFrame(), orbit.getDate()),
216 state.toTransform());
217 return earthToSc.transformPVCoordinates(groundPoint);
218 }
219
220
221
222
223 @Test
224 void testCompensMinMax() {
225
226
227
228
229 NadirPointing nadirLaw = new NadirPointing(circOrbit.getFrame(), earthShape);
230
231
232 YawCompensation yawCompensLaw = new YawCompensation(circOrbit.getFrame(), nadirLaw);
233
234
235
236 double duration = circOrbit.getKeplerianPeriod();
237 KeplerianPropagator extrapolator = new KeplerianPropagator(circOrbit);
238
239
240 double delta_t = 15.0;
241 AbsoluteDate extrapDate = date;
242
243
244 double yawMin = 1.e+12;
245 double latMin = 0.;
246
247 while (extrapDate.durationFrom(date) < duration) {
248 extrapDate = extrapDate.shiftedBy(delta_t);
249
250
251 Orbit extrapOrbit = extrapolator.propagate(extrapDate).getOrbit();
252 PVCoordinates extrapPvSatEME2000 = extrapOrbit.getPVCoordinates();
253
254
255 double extrapLat =
256 earthShape.transform(extrapPvSatEME2000.getPosition(), FramesFactory.getEME2000(), extrapDate).getLatitude();
257
258
259 double yawAngle = yawCompensLaw.getYawAngle(extrapOrbit, extrapDate, extrapOrbit.getFrame());
260
261
262 if (FastMath.abs(yawAngle) <= yawMin) {
263 yawMin = FastMath.abs(yawAngle);
264 latMin = extrapLat;
265 }
266
267
268
269
270
271 if ((FastMath.abs(extrapLat) < FastMath.toRadians(2.)) &&
272 (extrapPvSatEME2000.getVelocity().getZ() >= 0. )) {
273 Assertions.assertEquals(-3.206, FastMath.toDegrees(yawAngle), 0.003);
274 }
275
276
277 if ( extrapLat > FastMath.toRadians(50.15) ) {
278 Assertions.assertEquals(0, FastMath.toDegrees(yawAngle), 0.15);
279 }
280
281
282 if ( (FastMath.abs(extrapLat) < FastMath.toRadians(2.))
283 && (extrapPvSatEME2000.getVelocity().getZ() <= 0. ) ) {
284 Assertions.assertEquals(3.206, FastMath.toDegrees(yawAngle), 0.003);
285 }
286
287
288 if ( extrapLat < FastMath.toRadians(-50.15) ) {
289 Assertions.assertEquals(0, FastMath.toDegrees(yawAngle), 0.15);
290 }
291
292 }
293
294
295 Assertions.assertEquals( 0.0, FastMath.toDegrees(yawMin), 0.004);
296 Assertions.assertEquals(50.0, FastMath.toDegrees(latMin), 0.22);
297
298 }
299
300
301
302 @Test
303 void testCompensAxis() {
304
305
306
307
308 NadirPointing nadirLaw = new NadirPointing(circOrbit.getFrame(), earthShape);
309
310
311 YawCompensation yawCompensLaw = new YawCompensation(circOrbit.getFrame(), nadirLaw);
312
313
314 Rotation rotNoYaw = nadirLaw.getAttitude(circOrbit, date, circOrbit.getFrame()).getRotation();
315 Rotation rotYaw = yawCompensLaw.getAttitude(circOrbit, date, circOrbit.getFrame()).getRotation();
316
317 checkField(Binary64Field.getInstance(), yawCompensLaw, circOrbit, circOrbit.getDate(), circOrbit.getFrame());
318
319
320 Rotation compoRot = rotYaw.compose(rotNoYaw.revert(), RotationConvention.VECTOR_OPERATOR);
321 Vector3D yawAxis = compoRot.getAxis(RotationConvention.VECTOR_OPERATOR);
322
323
324 Assertions.assertEquals(0., yawAxis.subtract(Vector3D.PLUS_K).getNorm(), Utils.epsilonTest);
325
326 }
327
328 @Test
329 void testSpin() {
330
331 NadirPointing nadirLaw = new NadirPointing(circOrbit.getFrame(), earthShape);
332
333
334 YawCompensation law = new YawCompensation(circOrbit.getFrame(), nadirLaw);
335
336 KeplerianOrbit orbit =
337 new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
338 FastMath.toRadians(10.), FastMath.toRadians(20.),
339 FastMath.toRadians(30.), PositionAngleType.MEAN,
340 FramesFactory.getEME2000(),
341 date.shiftedBy(-300.0), 3.986004415e14);
342
343 Propagator propagator = new KeplerianPropagator(orbit, law);
344
345 double h = 0.01;
346 SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h));
347 SpacecraftState s0 = propagator.propagate(date);
348 SpacecraftState sPlus = propagator.propagate(date.shiftedBy(h));
349
350
351 double errorAngleMinus = Rotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(),
352 s0.getAttitude().getRotation());
353 double evolutionAngleMinus = Rotation.distance(sMinus.getAttitude().getRotation(),
354 s0.getAttitude().getRotation());
355 Assertions.assertEquals(0.0, errorAngleMinus, 8.5e-6 * evolutionAngleMinus);
356 double errorAnglePlus = Rotation.distance(s0.getAttitude().getRotation(),
357 sPlus.shiftedBy(-h).getAttitude().getRotation());
358 double evolutionAnglePlus = Rotation.distance(s0.getAttitude().getRotation(),
359 sPlus.getAttitude().getRotation());
360 Assertions.assertEquals(0.0, errorAnglePlus, 2.0e-5 * evolutionAnglePlus);
361
362 Vector3D spin0 = s0.getAttitude().getSpin();
363 Vector3D reference = AngularCoordinates.estimateRate(sMinus.getAttitude().getRotation(),
364 sPlus.getAttitude().getRotation(),
365 2 * h);
366 Assertions.assertTrue(spin0.getNorm() > 1.0e-3);
367 Assertions.assertEquals(0.0, spin0.subtract(reference).getNorm(), 2.0e-8);
368
369 }
370
371 private <T extends CalculusFieldElement<T>> void checkField(final Field<T> field, final YawCompensation provider,
372 final Orbit orbit, final AbsoluteDate date,
373 final Frame frame) {
374
375 final Attitude attitudeD = provider.getAttitude(orbit, date, frame);
376 final FieldOrbit<T> orbitF = new FieldSpacecraftState<>(field, new SpacecraftState(orbit)).getOrbit();
377 final FieldAbsoluteDate<T> dateF = new FieldAbsoluteDate<>(field, date);
378 final FieldAttitude<T> attitudeF = provider.getAttitude(orbitF, dateF, frame);
379 Assertions.assertEquals(0.0, Rotation.distance(attitudeD.getRotation(), attitudeF.getRotation().toRotation()), 2.0e-13);
380 Assertions.assertEquals(0.0, Vector3D.distance(attitudeD.getSpin(), attitudeF.getSpin().toVector3D()), 2.0e-11);
381 Assertions.assertEquals(0.0, Vector3D.distance(attitudeD.getRotationAcceleration(), attitudeF.getRotationAcceleration().toVector3D()), 2.0e-13);
382
383 final TimeStampedPVCoordinates pvD = provider.getTargetPV(orbit, date, frame);
384 final TimeStampedFieldPVCoordinates<T> pvF = provider.getTargetPV(orbitF, dateF, frame);
385 Assertions.assertEquals(0.0, Vector3D.distance(pvD.getPosition(), pvF.getPosition().toVector3D()), 1.0e-15);
386 Assertions.assertEquals(0.0, Vector3D.distance(pvD.getVelocity(), pvF.getVelocity().toVector3D()), 9.0e-9);
387 Assertions.assertEquals(0.0, Vector3D.distance(pvD.getAcceleration(), pvF.getAcceleration().toVector3D()), 8.0e-7);
388
389 Assertions.assertEquals(provider.getYawAngle(orbit, date, frame),
390 provider.getYawAngle(orbitF, dateF, frame).getReal(),
391 1.0e-12);
392 }
393
394 @BeforeEach
395 public void setUp() {
396 try {
397 Utils.setDataRoot("regular-data");
398
399
400 date = new AbsoluteDate(new DateComponents(2008, 4, 7),
401 TimeComponents.H00,
402 TimeScalesFactory.getUTC());
403
404
405 final double mu = 3.9860047e14;
406
407
408 itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
409
410
411 circOrbit =
412 new CircularOrbit(7178000.0, 0.5e-4, -0.5e-4, FastMath.toRadians(50.), FastMath.toRadians(270.),
413 FastMath.toRadians(5.300), PositionAngleType.MEAN,
414 FramesFactory.getEME2000(), date, mu);
415
416
417 earthShape =
418 new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf);
419
420 } catch (OrekitException oe) {
421 Assertions.fail(oe.getMessage());
422 }
423
424 }
425
426 @AfterEach
427 public void tearDown() {
428 date = null;
429 itrf = null;
430 circOrbit = null;
431 earthShape = null;
432 }
433
434 }
435