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