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