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