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.analysis.differentiation.GradientField;
22 import org.hipparchus.complex.ComplexField;
23 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
24 import org.hipparchus.geometry.euclidean.threed.Rotation;
25 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
26 import org.hipparchus.geometry.euclidean.threed.RotationOrder;
27 import org.hipparchus.geometry.euclidean.threed.Vector3D;
28 import org.hipparchus.util.Binary64Field;
29 import org.hipparchus.util.FastMath;
30 import org.junit.jupiter.api.AfterEach;
31 import org.junit.jupiter.api.Assertions;
32 import org.junit.jupiter.api.BeforeEach;
33 import org.junit.jupiter.api.Test;
34 import org.orekit.Utils;
35 import org.orekit.annotation.DefaultDataContext;
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.LOFType;
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.PositionAngleType;
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.IERSConventions;
58 import org.orekit.utils.PVCoordinates;
59
60
61 public class LofOffsetTest {
62
63
64 private AbsoluteDate date;
65
66
67 private double mu;
68
69
70 private Frame itrf;
71
72
73 OneAxisEllipsoid earthSpheric;
74
75
76 CircularOrbit orbit;
77 PVCoordinates pvSatEME2000;
78
79
80
81
82 @Test
83 void testGetters() {
84 final Rotation expectedRotation = new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, 0, 0, 0).revert();
85 final LofOffset lofOffset = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS);
86 Assertions.assertEquals(LOFType.LVLH_CCSDS, lofOffset.getLof());
87 Assertions.assertEquals(orbit.getFrame(), lofOffset.getInertialFrame());
88 Assertions.assertEquals(expectedRotation.getAngle(), lofOffset.getOffset().getAngle());
89 }
90
91
92
93 @Test
94 void testZero() {
95
96
97
98
99 final LofOffset lofAlignedLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS);
100 final Rotation lofOffsetRot = lofAlignedLaw.getAttitude(orbit, date, orbit.getFrame()).getRotation();
101
102
103 final Vector3D momentumEME2000 = pvSatEME2000.getMomentum();
104 final Vector3D momentumLof = lofOffsetRot.applyTo(momentumEME2000);
105 final double cosinus = FastMath.cos(Vector3D.dotProduct(momentumLof, Vector3D.PLUS_K));
106 Assertions.assertEquals(1., cosinus, Utils.epsilonAngle);
107
108 }
109
110
111 @Test
112 @DefaultDataContext
113 void testOffset() {
114
115
116 final CircularOrbit circ =
117 new CircularOrbit(7178000.0, 0.5e-4, -0.5e-4, FastMath.toRadians(0.), FastMath.toRadians(270.),
118 FastMath.toRadians(5.300), PositionAngleType.MEAN,
119 FramesFactory.getEME2000(), date, mu);
120
121
122
123
124 final OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf);
125 final GeodeticPoint geoTargetITRF = new GeodeticPoint(FastMath.toRadians(43.36), FastMath.toRadians(1.26), 600.);
126
127
128 final TargetPointing targetLaw = new TargetPointing(circ.getFrame(), geoTargetITRF, earthShape);
129 final Rotation targetRot = targetLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
130
131
132
133 final LofOffset lofAlignedLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS);
134 final Rotation lofAlignedRot = lofAlignedLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
135
136
137 Rotation rollPitchYaw = targetRot.compose(lofAlignedRot.revert(), RotationConvention.VECTOR_OPERATOR).revert();
138 final double[] angles = rollPitchYaw.getAngles(RotationOrder.ZYX, RotationConvention.VECTOR_OPERATOR);
139 final double yaw = angles[0];
140 final double pitch = angles[1];
141 final double roll = angles[2];
142
143
144
145 final LofOffset lofOffsetLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.ZYX, yaw, pitch, roll);
146 final Rotation lofOffsetRot = lofOffsetLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
147
148
149 final double angleCompo = targetRot.composeInverse(lofOffsetRot, RotationConvention.VECTOR_OPERATOR).getAngle();
150 Assertions.assertEquals(0., angleCompo, Utils.epsilonAngle);
151
152 }
153
154
155
156 @Test
157 void testTarget()
158 {
159
160
161 final GeodeticPoint targetDef = new GeodeticPoint(FastMath.toRadians(5.), FastMath.toRadians(-40.), 0.);
162 final TargetPointing targetLaw = new TargetPointing(orbit.getFrame(), targetDef, earthSpheric);
163
164
165 final LofOffset lofAlignedLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS);
166 final Rotation lofAlignedRot = lofAlignedLaw.getAttitude(orbit, date, orbit.getFrame()).getRotation();
167 final Attitude targetAttitude = targetLaw.getAttitude(orbit, date, orbit.getFrame());
168 final Rotation rollPitchYaw = targetAttitude.getRotation().compose(lofAlignedRot.revert(), RotationConvention.VECTOR_OPERATOR).revert();
169 final double[] angles = rollPitchYaw.getAngles(RotationOrder.ZYX, RotationConvention.VECTOR_OPERATOR);
170 final double yaw = angles[0];
171 final double pitch = angles[1];
172 final double roll = angles[2];
173
174
175 final LofOffset lofOffsetLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.ZYX, yaw, pitch, roll);
176 final LofOffsetPointing lofOffsetPtLaw = new LofOffsetPointing(orbit.getFrame(), earthSpheric, lofOffsetLaw, Vector3D.PLUS_K);
177
178
179 final Vector3D pTargetRes =
180 lofOffsetPtLaw.getTargetPV(orbit, date, earthSpheric.getBodyFrame()).getPosition();
181 final GeodeticPoint targetRes = earthSpheric.transform(pTargetRes, earthSpheric.getBodyFrame(), date);
182
183 Assertions.assertEquals(targetDef.getLongitude(), targetRes.getLongitude(), Utils.epsilonAngle);
184 Assertions.assertEquals(targetDef.getLongitude(), targetRes.getLongitude(), Utils.epsilonAngle);
185
186 }
187
188 @Test
189 @DefaultDataContext
190 void testSpin() {
191
192 final AttitudeProvider law = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.XYX, 0.1, 0.2, 0.3);
193
194 AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 1, 1),
195 new TimeComponents(3, 25, 45.6789),
196 TimeScalesFactory.getUTC());
197 KeplerianOrbit orbit =
198 new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
199 FastMath.toRadians(10.), FastMath.toRadians(20.),
200 FastMath.toRadians(30.), PositionAngleType.MEAN,
201 FramesFactory.getEME2000(), date, 3.986004415e14);
202
203 Propagator propagator = new KeplerianPropagator(orbit, law);
204
205 double h = 0.01;
206 SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h));
207 SpacecraftState s0 = propagator.propagate(date);
208 SpacecraftState sPlus = propagator.propagate(date.shiftedBy(h));
209
210
211 double errorAngleMinus = Rotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(),
212 s0.getAttitude().getRotation());
213 double evolutionAngleMinus = Rotation.distance(sMinus.getAttitude().getRotation(),
214 s0.getAttitude().getRotation());
215 Assertions.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus);
216 double errorAnglePlus = Rotation.distance(s0.getAttitude().getRotation(),
217 sPlus.shiftedBy(-h).getAttitude().getRotation());
218 double evolutionAnglePlus = Rotation.distance(s0.getAttitude().getRotation(),
219 sPlus.getAttitude().getRotation());
220 Assertions.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus);
221
222 Vector3D spin0 = s0.getAttitude().getSpin();
223 Vector3D reference = AngularCoordinates.estimateRate(sMinus.getAttitude().getRotation(),
224 sPlus.getAttitude().getRotation(),
225 2 * h);
226 Assertions.assertEquals(0.0, spin0.subtract(reference).getNorm(), 1.0e-10);
227
228 }
229
230 @Test
231 @DefaultDataContext
232 void testAnglesSign() {
233
234 AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 1, 1),
235 new TimeComponents(3, 25, 45.6789),
236 TimeScalesFactory.getUTC());
237 KeplerianOrbit orbit =
238 new KeplerianOrbit(7178000.0, 1.e-8, FastMath.toRadians(50.),
239 FastMath.toRadians(10.), FastMath.toRadians(20.),
240 FastMath.toRadians(0.), PositionAngleType.MEAN,
241 FramesFactory.getEME2000(), date, 3.986004415e14);
242
243 double alpha = 0.1;
244 double cos = FastMath.cos(alpha);
245 double sin = FastMath.sin(alpha);
246
247
248 Attitude attitude = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.XYZ, alpha, 0.0, 0.0).getAttitude(orbit, date, orbit.getFrame());
249 checkSatVector(orbit, attitude, Vector3D.PLUS_I, 1.0, 0.0, 0.0, 1.0e-8);
250 checkSatVector(orbit, attitude, Vector3D.PLUS_J, 0.0, cos, sin, 1.0e-8);
251 checkSatVector(orbit, attitude, Vector3D.PLUS_K, 0.0, -sin, cos, 1.0e-8);
252
253
254 attitude = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.XYZ, 0.0, alpha, 0.0).getAttitude(orbit, date, orbit.getFrame());
255 checkSatVector(orbit, attitude, Vector3D.PLUS_I, cos, 0.0, -sin, 1.0e-8);
256 checkSatVector(orbit, attitude, Vector3D.PLUS_J, 0.0, 1.0, 0.0, 1.0e-8);
257 checkSatVector(orbit, attitude, Vector3D.PLUS_K, sin, 0.0, cos, 1.0e-8);
258
259
260 attitude = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.XYZ, 0.0, 0.0, alpha).getAttitude(orbit, date, orbit.getFrame());
261 checkSatVector(orbit, attitude, Vector3D.PLUS_I, cos, sin, 0.0, 1.0e-8);
262 checkSatVector(orbit, attitude, Vector3D.PLUS_J, -sin, cos, 0.0, 1.0e-8);
263 checkSatVector(orbit, attitude, Vector3D.PLUS_K, 0.0, 0.0, 1.0, 1.0e-8);
264
265 }
266
267 @Test
268 @DefaultDataContext
269 void testRetrieveAngles() {
270 AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 1, 1),
271 new TimeComponents(3, 25, 45.6789),
272 TimeScalesFactory.getUTC());
273 KeplerianOrbit orbit =
274 new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
275 FastMath.toRadians(10.), FastMath.toRadians(20.),
276 FastMath.toRadians(30.), PositionAngleType.MEAN,
277 FramesFactory.getEME2000(), date, 3.986004415e14);
278
279 RotationOrder order = RotationOrder.ZXY;
280 double alpha1 = 0.123;
281 double alpha2 = 0.456;
282 double alpha3 = 0.789;
283 LofOffset law = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, order, alpha1, alpha2, alpha3);
284 Rotation offsetAtt = law.getAttitude(orbit, date, orbit.getFrame()).getRotation();
285 Rotation alignedAtt = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS).getAttitude(orbit, date, orbit.getFrame()).getRotation();
286 Rotation offsetProper = offsetAtt.compose(alignedAtt.revert(), RotationConvention.VECTOR_OPERATOR);
287 double[] anglesV = offsetProper.revert().getAngles(order, RotationConvention.VECTOR_OPERATOR);
288 Assertions.assertEquals(alpha1, anglesV[0], 1.0e-11);
289 Assertions.assertEquals(alpha2, anglesV[1], 1.0e-11);
290 Assertions.assertEquals(alpha3, anglesV[2], 1.0e-11);
291 double[] anglesF = offsetProper.getAngles(order, RotationConvention.FRAME_TRANSFORM);
292 Assertions.assertEquals(alpha1, anglesF[0], 1.0e-11);
293 Assertions.assertEquals(alpha2, anglesF[1], 1.0e-11);
294 Assertions.assertEquals(alpha3, anglesF[2], 1.0e-11);
295 }
296
297 @Test
298 @DefaultDataContext
299 void testTypesField() {
300 AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 1, 1),
301 new TimeComponents(3, 25, 45.6789),
302 TimeScalesFactory.getUTC());
303 KeplerianOrbit orbit =
304 new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
305 FastMath.toRadians(10.), FastMath.toRadians(20.),
306 FastMath.toRadians(30.), PositionAngleType.MEAN,
307 FramesFactory.getEME2000(), date, 3.986004415e14);
308
309 for (final LOFType type : LOFType.values()) {
310 RotationOrder order = RotationOrder.ZXY;
311 double alpha1 = 0.123;
312 double alpha2 = 0.456;
313 double alpha3 = 0.789;
314 LofOffset law = new LofOffset(orbit.getFrame(), type, order, alpha1, alpha2, alpha3);
315 checkField(Binary64Field.getInstance(), law, orbit, date, orbit.getFrame());
316 }
317 }
318
319 private void checkSatVector(Orbit o, Attitude a, Vector3D satVector,
320 double expectedX, double expectedY, double expectedZ,
321 double threshold) {
322 Vector3D zLof = o.getPosition().normalize().negate();
323 Vector3D yLof = o.getPVCoordinates().getMomentum().normalize().negate();
324 Vector3D xLof = Vector3D.crossProduct(yLof, zLof);
325 Assertions.assertTrue(Vector3D.dotProduct(xLof, o.getPVCoordinates().getVelocity()) > 0);
326 Vector3D v = a.getRotation().applyInverseTo(satVector);
327 Assertions.assertEquals(expectedX, Vector3D.dotProduct(v, xLof), threshold);
328 Assertions.assertEquals(expectedY, Vector3D.dotProduct(v, yLof), threshold);
329 Assertions.assertEquals(expectedZ, Vector3D.dotProduct(v, zLof), threshold);
330 }
331
332 private <T extends CalculusFieldElement<T>> void checkField(final Field<T> field, final AttitudeProvider provider,
333 final Orbit orbit, final AbsoluteDate date,
334 final Frame frame)
335 {
336 Attitude attitudeD = provider.getAttitude(orbit, date, frame);
337 final FieldOrbit<T> orbitF = new FieldSpacecraftState<>(field, new SpacecraftState(orbit)).getOrbit();
338 final FieldAbsoluteDate<T> dateF = new FieldAbsoluteDate<>(field, date);
339 FieldAttitude<T> attitudeF = provider.getAttitude(orbitF, dateF, frame);
340 Assertions.assertEquals(0.0, Rotation.distance(attitudeD.getRotation(), attitudeF.getRotation().toRotation()), 1.0e-15);
341 Assertions.assertEquals(0.0, Vector3D.distance(attitudeD.getSpin(), attitudeF.getSpin().toVector3D()), 1.0e-15);
342 Assertions.assertEquals(0.0, Vector3D.distance(attitudeD.getRotationAcceleration(), attitudeF.getRotationAcceleration().toVector3D()), 1.0e-15);
343 }
344
345 @Test
346 void testGetAttitudeRotation() {
347
348 final AbsoluteDate date = orbit.getDate();
349 final LofOffset lofOffset = new LofOffset(orbit.getFrame(), LOFType.QSW);
350
351 final Rotation actualRotation = lofOffset.getAttitudeRotation(orbit, date, itrf);
352
353 final Rotation expectedRotation = lofOffset.getAttitude(orbit, date, itrf).getRotation();
354 Assertions.assertEquals(0., Rotation.distance(expectedRotation, actualRotation));
355 }
356
357 @Test
358 void testGetAttitudeRotationFieldComplex() {
359 final ComplexField complexField = ComplexField.getInstance();
360 templateTestGetRotationField(complexField);
361 }
362
363 @Test
364 void testGetAttitudeRotationFieldGradient() {
365 final GradientField gradientField = GradientField.getField(1);
366 templateTestGetRotationField(gradientField);
367 }
368
369 <T extends CalculusFieldElement<T>> void templateTestGetRotationField(final Field<T> field) {
370
371 final LofOffset lofOffset = new LofOffset(orbit.getFrame(), LOFType.QSW);
372 final SpacecraftState state = new SpacecraftState(orbit);
373 final FieldSpacecraftState<T> fieldState = new FieldSpacecraftState<>(field, state);
374
375 final FieldRotation<T> actualRotation = lofOffset.getAttitudeRotation(fieldState.getOrbit(), fieldState.getDate(), itrf);
376
377 final FieldRotation<T> expectedRotation = lofOffset.getAttitude(fieldState.getOrbit(), fieldState.getDate(), itrf).getRotation();
378 Assertions.assertEquals(0., Rotation.distance(expectedRotation.toRotation(), actualRotation.toRotation()));
379 }
380
381 @BeforeEach
382 @DefaultDataContext
383 public void setUp() {
384 try {
385
386 Utils.setDataRoot("regular-data");
387
388
389 date = new AbsoluteDate(new DateComponents(2008, 4, 7),
390 TimeComponents.H00,
391 TimeScalesFactory.getUTC());
392
393
394 mu = 3.9860047e14;
395
396
397 itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
398
399
400 earthSpheric =
401 new OneAxisEllipsoid(6378136.460, 0., itrf);
402
403
404 orbit =
405 new CircularOrbit(7178000.0, 0.5e-8, -0.5e-8, FastMath.toRadians(50.), FastMath.toRadians(150.),
406 FastMath.toRadians(5.300), PositionAngleType.MEAN,
407 FramesFactory.getEME2000(), date, mu);
408 pvSatEME2000 = orbit.getPVCoordinates();
409
410
411 } catch (OrekitException oe) {
412 Assertions.fail(oe.getMessage());
413 }
414
415 }
416
417 @AfterEach
418 public void tearDown() {
419 date = null;
420 itrf = null;
421 earthSpheric = null;
422 }
423
424 }
425