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.Field;
20 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
21 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
22 import org.hipparchus.util.Binary64;
23 import org.hipparchus.util.Binary64Field;
24 import org.hipparchus.util.FastMath;
25 import org.junit.jupiter.api.Assertions;
26 import org.junit.jupiter.api.DisplayName;
27 import org.junit.jupiter.api.Test;
28 import org.mockito.Mockito;
29 import org.orekit.Utils;
30 import org.orekit.annotation.DefaultDataContext;
31 import org.orekit.bodies.OneAxisEllipsoid;
32 import org.orekit.errors.OrekitIllegalArgumentException;
33 import org.orekit.errors.OrekitMessages;
34 import org.orekit.frames.Frame;
35 import org.orekit.frames.FramesFactory;
36 import org.orekit.orbits.FieldCircularOrbit;
37 import org.orekit.propagation.FieldPropagator;
38 import org.orekit.propagation.analytical.FieldEcksteinHechlerPropagator;
39 import org.orekit.time.AbsoluteDate;
40 import org.orekit.time.FieldAbsoluteDate;
41 import org.orekit.time.FieldTimeInterpolator;
42 import org.orekit.time.FieldTimeStamped;
43 import org.orekit.time.TimeInterpolator;
44 import org.orekit.utils.AngularDerivativesFilter;
45 import org.orekit.utils.Constants;
46 import org.orekit.utils.FieldPVCoordinates;
47 import org.orekit.utils.IERSConventions;
48 import org.orekit.utils.TimeStampedAngularCoordinates;
49 import org.orekit.utils.TimeStampedAngularCoordinatesHermiteInterpolator;
50 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
51 import org.orekit.utils.TimeStampedFieldAngularCoordinatesHermiteInterpolator;
52
53 import java.util.ArrayList;
54 import java.util.List;
55
56 import static org.mockito.Mockito.mock;
57
58 public class FieldAttitudeInterpolatorTest {
59
60 final Field<Binary64> field = Binary64Field.getInstance();
61
62 @Test
63 @DefaultDataContext
64 void testInterpolation() {
65
66
67 Utils.setDataRoot("regular-data");
68 final Binary64 ehMu = new Binary64(3.9860047e14);
69 final double ae = 6.378137e6;
70 final double c20 = -1.08263e-3;
71 final double c30 = 2.54e-6;
72 final double c40 = 1.62e-6;
73 final double c50 = 2.3e-7;
74 final double c60 = -5.5e-7;
75
76 final FieldAbsoluteDate<Binary64> date = new FieldAbsoluteDate<>(field, new AbsoluteDate()).shiftedBy(584.);
77 final FieldVector3D<Binary64> position = new FieldVector3D<>(new Binary64(3220103.),
78 new Binary64(69623.),
79 new Binary64(6449822.));
80 final FieldVector3D<Binary64> velocity = new FieldVector3D<>(new Binary64(6414.7),
81 new Binary64(-2006.),
82 new Binary64(-3180.));
83 final FieldCircularOrbit<Binary64> initialOrbit =
84 new FieldCircularOrbit<>(new FieldPVCoordinates<>(position, velocity),
85 FramesFactory.getEME2000(), date, ehMu);
86
87 FieldPropagator<Binary64> propagator =
88 new FieldEcksteinHechlerPropagator<>(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);
89 OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
90 Constants.WGS84_EARTH_FLATTENING,
91 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
92 propagator.setAttitudeProvider(new BodyCenterPointing(initialOrbit.getFrame(), earth));
93 final FieldAttitude<Binary64> initialAttitude = propagator.propagate(initialOrbit.getDate()).getAttitude();
94
95
96 List<FieldAttitude<Binary64>> sample = new ArrayList<>();
97 for (double dt = 0; dt < 251.0; dt += 60.0) {
98 sample.add(propagator.propagate(date.shiftedBy(dt)).getAttitude());
99 }
100
101
102 final double extrapolationThreshold = 59;
103 final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<Binary64>, Binary64> angularInterpolator =
104 new TimeStampedFieldAngularCoordinatesHermiteInterpolator<>(sample.size(), extrapolationThreshold,
105 AngularDerivativesFilter.USE_RR);
106
107 final FieldAttitudeInterpolator<Binary64> attitudeInterpolator =
108 new FieldAttitudeInterpolator<>(initialOrbit.getFrame(), angularInterpolator);
109
110
111 double maxShiftAngleError = 0;
112 double maxInterpolationAngleError = 0;
113 double maxShiftRateError = 0;
114 double maxInterpolationRateError = 0;
115 for (double dt = 0; dt < 240.0; dt += 1.0) {
116 FieldAbsoluteDate<Binary64> t = initialOrbit.getDate().shiftedBy(dt);
117 FieldAttitude<Binary64> propagated = propagator.propagate(t).getAttitude();
118 Binary64 shiftAngleError =
119 FieldRotation.distance(propagated.getRotation(), initialAttitude.shiftedBy(dt).getRotation());
120 Binary64 interpolationAngleError = FieldRotation.distance(propagated.getRotation(),
121 attitudeInterpolator.interpolate(t, sample)
122 .getRotation());
123 Binary64 shiftRateError = FieldVector3D.distance(propagated.getSpin(),
124 initialAttitude.shiftedBy(dt).getSpin());
125 Binary64 interpolationRateError = FieldVector3D.distance(propagated.getSpin(),
126 attitudeInterpolator.interpolate(t, sample).getSpin());
127 maxShiftAngleError = FastMath.max(maxShiftAngleError, shiftAngleError.getReal());
128 maxInterpolationAngleError = FastMath.max(maxInterpolationAngleError, interpolationAngleError.getReal());
129 maxShiftRateError = FastMath.max(maxShiftRateError, shiftRateError.getReal());
130 maxInterpolationRateError = FastMath.max(maxInterpolationRateError, interpolationRateError.getReal());
131 }
132 Assertions.assertEquals(0.0, maxShiftAngleError, 6.9e-6);
133 Assertions.assertEquals(0.0, maxInterpolationAngleError, 8.8e-15);
134 Assertions.assertEquals(0.0, maxShiftRateError, 7.6e-8);
135 Assertions.assertEquals(0.0, maxInterpolationRateError, 2.0e-16);
136
137
138 maxShiftAngleError = 0;
139 maxInterpolationAngleError = 0;
140 maxShiftRateError = 0;
141 maxInterpolationRateError = 0;
142 for (double dt = 250.0; dt < 300.0; dt += 1.0) {
143 FieldAbsoluteDate<Binary64> t = initialOrbit.getDate().shiftedBy(dt);
144 FieldAttitude<Binary64> propagated = propagator.propagate(t).getAttitude();
145 Binary64 shiftAngleError = FieldRotation.distance(propagated.getRotation(),
146 initialAttitude.shiftedBy(dt).getRotation());
147 Binary64 interpolationAngleError = FieldRotation.distance(propagated.getRotation(),
148 attitudeInterpolator.interpolate(t, sample)
149 .getRotation());
150 Binary64 shiftRateError = FieldVector3D.distance(propagated.getSpin(),
151 initialAttitude.shiftedBy(dt).getSpin());
152 Binary64 interpolationRateError = FieldVector3D.distance(propagated.getSpin(),
153 attitudeInterpolator.interpolate(t, sample).getSpin());
154 maxShiftAngleError = FastMath.max(maxShiftAngleError, shiftAngleError.getReal());
155 maxInterpolationAngleError = FastMath.max(maxInterpolationAngleError, interpolationAngleError.getReal());
156 maxShiftRateError = FastMath.max(maxShiftRateError, shiftRateError.getReal());
157 maxInterpolationRateError = FastMath.max(maxInterpolationRateError, interpolationRateError.getReal());
158 }
159 Assertions.assertEquals(0.0, maxShiftAngleError, 1.3e-5);
160 Assertions.assertEquals(0.0, maxInterpolationAngleError, 1.2e-12);
161 Assertions.assertEquals(0.0, maxShiftRateError, 1.2e-7);
162 Assertions.assertEquals(0.0, maxInterpolationRateError, 8.4e-14);
163
164 Assertions.assertEquals(initialOrbit.getFrame(), attitudeInterpolator.getReferenceFrame());
165 Assertions.assertEquals(angularInterpolator, attitudeInterpolator.getAngularInterpolator());
166
167 }
168
169 @Test
170 @DisplayName("Test constructor")
171 void testConstructor() {
172
173 final Frame frameMock = Mockito.mock(Frame.class);
174
175 @SuppressWarnings("unchecked")
176 final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<Binary64>, Binary64> angularInterpolatorMock =
177 Mockito.mock(FieldTimeInterpolator.class);
178
179
180 final FieldAttitudeInterpolator<Binary64> attitudeInterpolator =
181 new FieldAttitudeInterpolator<>(frameMock, angularInterpolatorMock);
182
183
184 Assertions.assertEquals(frameMock, attitudeInterpolator.getReferenceFrame());
185 Assertions.assertEquals(angularInterpolatorMock, attitudeInterpolator.getAngularInterpolator());
186 }
187
188 @Test
189 @DisplayName("test error thrown when sample is too small")
190 @DefaultDataContext
191 void testErrorThrownWhenSampleIsTooSmall() {
192
193 final AbsoluteDate interpolationDate = new AbsoluteDate();
194
195 final List<Attitude> attitudes = new ArrayList<>();
196 attitudes.add(Mockito.mock(Attitude.class));
197
198 final TimeInterpolator<TimeStampedAngularCoordinates> angularInterpolator =
199 new TimeStampedAngularCoordinatesHermiteInterpolator(2, AngularDerivativesFilter.USE_R);
200
201 final TimeInterpolator<Attitude> attitudeInterpolator =
202 new AttitudeInterpolator(FramesFactory.getGCRF(), angularInterpolator);
203
204
205 OrekitIllegalArgumentException thrown = Assertions.assertThrows(OrekitIllegalArgumentException.class,
206 () -> attitudeInterpolator.interpolate(interpolationDate, attitudes));
207
208 Assertions.assertEquals(OrekitMessages.NOT_ENOUGH_CACHED_NEIGHBORS, thrown.getSpecifier());
209 Assertions.assertEquals(1, ((Integer) thrown.getParts()[0]).intValue());
210 Assertions.assertEquals(2, ((Integer) thrown.getParts()[1]).intValue());
211
212 }
213
214
215
216
217
218
219 @Test
220 @SuppressWarnings("unchecked")
221 void testGetSubInterpolator() {
222
223
224 final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<Binary64>, Binary64> mockTimeInterpolator =
225 mock(FieldTimeInterpolator.class);
226
227
228 final FieldAttitudeInterpolator<Binary64> attitudeInterpolator =
229 new FieldAttitudeInterpolator<>(FramesFactory.getGCRF(), mockTimeInterpolator);
230
231
232 final List<FieldTimeInterpolator<? extends FieldTimeStamped<Binary64>, Binary64>> actualSubInterpolators =
233 attitudeInterpolator.getSubInterpolators();
234
235
236 Assertions.assertEquals(1, actualSubInterpolators.size());
237 Assertions.assertSame(mockTimeInterpolator, actualSubInterpolators.get(0));
238 }
239
240 }