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