1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.gnss.attitude;
18
19 import java.io.BufferedReader;
20 import java.io.IOException;
21 import java.io.InputStream;
22 import java.io.InputStreamReader;
23 import java.io.Reader;
24 import java.nio.charset.StandardCharsets;
25 import java.util.ArrayList;
26 import java.util.List;
27 import java.util.function.BiFunction;
28 import java.util.stream.Collectors;
29 import java.util.stream.Stream;
30
31 import org.hipparchus.CalculusFieldElement;
32 import org.hipparchus.Field;
33 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
34 import org.hipparchus.geometry.euclidean.threed.Vector3D;
35 import org.hipparchus.util.Binary64;
36 import org.hipparchus.util.Binary64Field;
37 import org.hipparchus.util.FastMath;
38 import org.junit.jupiter.api.Assertions;
39 import org.junit.jupiter.api.BeforeEach;
40 import org.orekit.Utils;
41 import org.orekit.attitudes.Attitude;
42 import org.orekit.attitudes.FieldAttitude;
43 import org.orekit.frames.Frame;
44 import org.orekit.frames.FramesFactory;
45 import org.orekit.frames.StaticTransform;
46 import org.orekit.gnss.SatelliteSystem;
47 import org.orekit.gnss.antenna.SatelliteType;
48 import org.orekit.orbits.CartesianOrbit;
49 import org.orekit.orbits.FieldCartesianOrbit;
50 import org.orekit.orbits.Orbit;
51 import org.orekit.time.AbsoluteDate;
52 import org.orekit.time.FieldAbsoluteDate;
53 import org.orekit.time.FieldTimeInterpolator;
54 import org.orekit.time.GNSSDate;
55 import org.orekit.time.TimeInterpolator;
56 import org.orekit.utils.CartesianDerivativesFilter;
57 import org.orekit.utils.Constants;
58 import org.orekit.utils.ExtendedPositionProvider;
59 import org.orekit.utils.FieldPVCoordinates;
60 import org.orekit.utils.IERSConventions;
61 import org.orekit.utils.PVCoordinates;
62 import org.orekit.utils.TimeStampedFieldPVCoordinates;
63 import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;
64 import org.orekit.utils.TimeStampedPVCoordinates;
65 import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;
66
67 public abstract class AbstractGNSSAttitudeProviderTest {
68
69 @BeforeEach
70 public void setUp() {
71 Utils.setDataRoot("regular-data:gnss");
72 }
73
74 protected enum CheckAxis {
75
76 X_AXIS(Vector3D.PLUS_I, (x, z) -> x),
77 Y_AXIS(Vector3D.PLUS_J, (x, z) -> Vector3D.crossProduct(z, x)),
78 Z_AXIS(Vector3D.PLUS_K, (x, z) -> z);
79
80 final Vector3D canonical;
81 final BiFunction<Vector3D, Vector3D, Vector3D> getRefAxis;
82
83 CheckAxis(final Vector3D canonical, final BiFunction<Vector3D, Vector3D, Vector3D> getRefAxis) {
84 this.canonical = canonical;
85 this.getRefAxis = getRefAxis;
86 }
87
88 double error(final Attitude attitude, final Vector3D x, final Vector3D z) {
89 final Vector3D computedAxis = attitude.getRotation().applyInverseTo(canonical);
90 final Vector3D referenceAxis = getRefAxis.apply(x, z);
91 return Vector3D.angle(computedAxis, referenceAxis);
92 }
93
94 }
95
96 protected void doTestAxes(final String fileName, final double tolXY, double tolZ, boolean useGenericAttitude) {
97
98 if (getClass().getResource("/gnss/attitude/" + fileName) == null) {
99 Assertions.fail("file not found: " + fileName);
100 }
101
102
103
104
105
106
107
108
109 final Frame eme2000 = FramesFactory.getEME2000();
110 final Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, false);
111 final List<List<ParsedLine>> dataBlocks = parseFile(fileName, eme2000, itrf);
112 double maxErrorX = 0;
113 double maxErrorY = 0;
114 double maxErrorZ = 0;
115 for (final List<ParsedLine> dataBlock : dataBlocks) {
116 final AbsoluteDate validityStart = dataBlock.get(0).gpsDate.getDate();
117 final AbsoluteDate validityEnd = dataBlock.get(dataBlock.size() - 1).gpsDate.getDate();
118 final int prnNumber = dataBlock.get(0).prnNumber;
119 final ExtendedPositionProvider fakedSun = new FakedSun(dataBlock);
120 final GNSSAttitudeProvider attitudeProvider = useGenericAttitude ?
121 new GenericGNSS(validityStart, validityEnd, fakedSun, eme2000) :
122 dataBlock.get(0).satType.buildAttitudeProvider(validityStart, validityEnd,
123 fakedSun, eme2000, prnNumber);
124 Assertions.assertEquals(attitudeProvider.validityStart(), dataBlock.get(0).gpsDate.getDate());
125 Assertions.assertEquals(attitudeProvider.validityEnd(), dataBlock.get(dataBlock.size() - 1).gpsDate.getDate());
126
127 for (final ParsedLine parsedLine : dataBlock) {
128
129
130 final Attitude attitude1 = attitudeProvider.getAttitude(parsedLine.orbit, parsedLine.gpsDate.getDate(), parsedLine.orbit.getFrame());
131 final Vector3D x = parsedLine.eclipsX;
132 final PVCoordinates pv = parsedLine.orbit.getPVCoordinates();
133 final Vector3D z = pv.getPosition().normalize().negate();
134 maxErrorX = FastMath.max(maxErrorX, CheckAxis.X_AXIS.error(attitude1, x, z));
135 maxErrorY = FastMath.max(maxErrorY, CheckAxis.Y_AXIS.error(attitude1, x, z));
136 maxErrorZ = FastMath.max(maxErrorZ, CheckAxis.Z_AXIS.error(attitude1, x, z));
137
138
139 final Field<Binary64> field = Binary64Field.getInstance();
140 final FieldPVCoordinates<Binary64> pv64 = new FieldPVCoordinates<>(field, parsedLine.orbit.getPVCoordinates());
141 final FieldAbsoluteDate<Binary64> date64 = new FieldAbsoluteDate<>(field, parsedLine.gpsDate.getDate());
142 final FieldCartesianOrbit<Binary64> orbit64 = new FieldCartesianOrbit<>(pv64,
143 parsedLine.orbit.getFrame(),
144 date64,
145 field.getZero().add(parsedLine.orbit.getMu()));
146 final FieldAttitude<Binary64> attitude64 =
147 attitudeProvider.getAttitude(orbit64, orbit64.getDate(), parsedLine.orbit.getFrame());
148 final Attitude attitude2 = attitude64.toAttitude();
149 maxErrorX = FastMath.max(maxErrorX, CheckAxis.X_AXIS.error(attitude2, x, z));
150 maxErrorY = FastMath.max(maxErrorY, CheckAxis.Y_AXIS.error(attitude2, x, z));
151 maxErrorZ = FastMath.max(maxErrorZ, CheckAxis.Z_AXIS.error(attitude2, x, z));
152
153 }
154
155 }
156
157 Assertions.assertEquals(0, maxErrorX, tolXY);
158 Assertions.assertEquals(0, maxErrorY, tolXY);
159 Assertions.assertEquals(0, maxErrorZ, tolZ);
160
161 }
162
163 private List<List<ParsedLine>> parseFile(final String fileName, final Frame eme2000, final Frame itrf)
164 {
165 final List<List<ParsedLine>> dataBlocks = new ArrayList<>();
166 try (InputStream is = getClass().getResourceAsStream("/gnss/attitude/" + fileName);
167 Reader reader = new InputStreamReader(is, StandardCharsets.UTF_8);
168 BufferedReader br = new BufferedReader(reader)) {
169
170
171 dataBlocks.add(new ArrayList<>());
172 ParsedLine parsedLine = null;
173 for (String line = br.readLine(); line != null; line = br.readLine()) {
174 line = line.trim();
175 if (line.startsWith("#")) {
176 continue;
177 }
178 final ParsedLine previous = parsedLine;
179 parsedLine = new ParsedLine(line, eme2000, itrf);
180 if (previous != null &&
181 (parsedLine.prnNumber != previous.prnNumber ||
182 parsedLine.gpsDate.getDate().durationFrom(previous.gpsDate.getDate()) > 14400)) {
183 dataBlocks.add(new ArrayList<>());
184 }
185 dataBlocks.get(dataBlocks.size() - 1).add(parsedLine);
186 }
187
188 } catch (IOException ioe) {
189 Assertions.fail(ioe.getLocalizedMessage());
190 }
191
192 return dataBlocks;
193
194 }
195
196 private static class FakedSun implements ExtendedPositionProvider {
197
198 final int SAMPLE_SIZE = 5;
199 final List<ParsedLine> parsedLines;
200
201 FakedSun(final List<ParsedLine> parsedLines) {
202 this.parsedLines = parsedLines;
203 }
204
205 private Stream<ParsedLine> getCloseLines(final AbsoluteDate date) {
206 final int margin = SAMPLE_SIZE / 2;
207 int closest = margin;
208 for (int i = closest + 1; i < parsedLines.size() - margin; ++i) {
209 if (FastMath.abs(date.durationFrom(parsedLines.get(i).gpsDate.getDate())) <
210 FastMath.abs(date.durationFrom(parsedLines.get(closest).gpsDate.getDate()))) {
211 closest = i;
212 }
213 }
214 return parsedLines.subList(closest - margin, closest + margin + 1).stream();
215 }
216
217 @Override
218 public Vector3D getPosition(AbsoluteDate date, Frame frame) {
219
220 final List<TimeStampedPVCoordinates> sample = getCloseLines(date).
221 map(parsedLine ->
222 new TimeStampedPVCoordinates(
223 parsedLine.gpsDate.getDate(),
224 parsedLine.sunP,
225 Vector3D.ZERO,
226 Vector3D.ZERO)).collect(Collectors.toList());
227
228
229 final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
230 new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), 1000000, CartesianDerivativesFilter.USE_P);
231
232 return interpolator.interpolate(date, sample).getPosition();
233 }
234
235 @Override
236 public <T extends CalculusFieldElement<T>> FieldVector3D<T>
237 getPosition(FieldAbsoluteDate<T> date, Frame frame) {
238 final Field<T> field = date.getField();
239
240
241 final List<TimeStampedFieldPVCoordinates<T>> sample = getCloseLines(date.toAbsoluteDate()).
242 map(parsedLine ->
243 new TimeStampedFieldPVCoordinates<>(parsedLine.gpsDate.getDate(),
244 new FieldVector3D<>(field, parsedLine.sunP),
245 FieldVector3D.getZero(field),
246 FieldVector3D.getZero(field))).collect(
247 Collectors.toList());
248
249 final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> interpolator =
250 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(sample.size(), 1.025 * Constants.JULIAN_DAY,
251 CartesianDerivativesFilter.USE_P);
252
253 return interpolator.interpolate(date, sample).getPosition();
254 }
255
256 }
257
258 private static class ParsedLine {
259
260
261 private static final double MS_TO_S = 1.0e-3;
262
263 final GNSSDate gpsDate;
264 final int prnNumber;
265 final SatelliteType satType;
266 final Orbit orbit;
267 final Vector3D sunP;
268 final Vector3D eclipsX;
269
270 ParsedLine(final String line, final Frame eme2000, final Frame itrf) {
271 final String[] fields = line.split("\\s+");
272 gpsDate = new GNSSDate(Integer.parseInt(fields[1]), Double.parseDouble(fields[2]) * MS_TO_S, SatelliteSystem.GPS);
273 final StaticTransform t = itrf.getStaticTransformTo(eme2000, gpsDate.getDate());
274 prnNumber = Integer.parseInt(fields[3].substring(1));
275 satType = SatelliteType.parseSatelliteType(fields[4].replaceAll("[-_ ]", ""));
276 orbit = new CartesianOrbit(new TimeStampedPVCoordinates(gpsDate.getDate(),
277 t.transformPosition(new Vector3D(Double.parseDouble(fields[ 6]),
278 Double.parseDouble(fields[ 7]),
279 Double.parseDouble(fields[ 8]))),
280 t.transformVector(new Vector3D(Double.parseDouble(fields[ 9]),
281 Double.parseDouble(fields[10]),
282 Double.parseDouble(fields[11])))),
283 eme2000, Constants.EIGEN5C_EARTH_MU);
284 sunP = t.transformPosition(new Vector3D(Double.parseDouble(fields[12]),
285 Double.parseDouble(fields[13]),
286 Double.parseDouble(fields[14])));
287
288
289
290
291
292
293 eclipsX = t.transformVector(new Vector3D(Double.parseDouble(fields[21]),
294 Double.parseDouble(fields[22]),
295 Double.parseDouble(fields[23])));
296
297 }
298
299 }
300
301 }