1 //==============================================================================
2 //
3 // This file is part of GPSTk, the GPS Toolkit.
4 //
5 // The GPSTk is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published
7 // by the Free Software Foundation; either version 3.0 of the License, or
8 // any later version.
9 //
10 // The GPSTk is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public
16 // License along with GPSTk; if not, write to the Free Software Foundation,
17 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
18 //
19 // This software was developed by Applied Research Laboratories at the
20 // University of Texas at Austin.
21 // Copyright 2004-2020, The Board of Regents of The University of Texas System
22 //
23 //==============================================================================
24
25 //==============================================================================
26 //
27 // This software was developed by Applied Research Laboratories at the
28 // University of Texas at Austin, under contract to an agency or agencies
29 // within the U.S. Department of Defense. The U.S. Government retains all
30 // rights to use, duplicate, distribute, disclose, or release this software.
31 //
32 // Pursuant to DoD Directive 523024
33 //
34 // DISTRIBUTION STATEMENT A: This software has been approved for public
35 // release, distribution is unlimited.
36 //
37 //==============================================================================
38
39 #include "BrcKeplerOrbit.hpp"
40 #include "TestUtil.hpp"
41 #include "GPSWeekZcount.hpp"
42 #include "TimeString.hpp"
43
44 using namespace std;
45 using namespace gpstk;
46
47 /** Threshold for how much different our velocities can be between
48 * being computed directly via svXvt and computed via differencing
49 * svXvt positions over time. */
50 double velDiffThresh = 0.0008;
51
52 #ifdef _MSC_VER
53 #define LDEXP(x,y) ldexp(x,y)
54 #else
55 #define LDEXP(x,y) std::ldexp(x,y)
56 #endif
57
58 class BrcKeplerOrbit_T
59 {
60 public:
61 /// set the fields to some non-default values
62 void fill(BrcKeplerOrbit& orbit);
63 unsigned initializationTest();
64 unsigned equalityTest();
65 unsigned svXvtTest();
66 unsigned relativityTest();
67 unsigned fitIntTest();
68 };
69
70
71 void BrcKeplerOrbit_T ::
fill(BrcKeplerOrbit & orbit)72 fill(BrcKeplerOrbit& orbit)
73 {
74 ObsID oi(ObservationType::NavMsg, CarrierBand::L5, TrackingCode::Y);
75 orbit.loadData("GPS", oi, 31, GPSWeekZcount(1886, 398400),
76 GPSWeekZcount(1887, 0), GPSWeekZcount(1887, 0), 1, true,
77 // these are the same as in EngEphemeris_T.cpp
78 LDEXP(double( int16_t(0xfe17)), -29), // Cuc
79 LDEXP(double( int16_t(0x0b0e)), -29), // Cus
80 LDEXP(double( int16_t(0x22b4)), -5), // Crc
81 LDEXP(double( int16_t(0xfde4)), -5), // Crs
82 LDEXP(double( int16_t(0xffae)), -29), // Cic
83 LDEXP(double( int16_t(0x0005)), -29), // Cis
84 LDEXP(double( int32_t(0x2dbbccf8)), -31) * PI, // M0
85 LDEXP(double( int16_t(0x35bb)), -43) * PI, //dn
86 123e-12, // dndot, arbitrary, absent from GPS nav id 2
87 LDEXP(double(uint32_t(0x04473adb)), -33), // ecc
88 7.89e9, // A, should this be Ahalf * Ahalf?
89 LDEXP(double(uint32_t(0xa10dcc28)), -19), // Ahalf
90 4.56e7, // Adot, arbitrary, absent from GPS nav id 2
91 LDEXP(double( int32_t(0x3873d1d1)), -31) * PI, // OMEGA0
92 LDEXP(double( int32_t(0x2747e88f)), -31) * PI, // i0
93 LDEXP(double( int32_t(0xb078a8d5)), -31) * PI, // w
94 LDEXP(double( int32_t(0xffffa3c7)), -43) * PI, // OMEGAdot
95 LDEXP(double( int16_t(0xfdc6)), -43) * PI // idot
96 );
97 }
98
99
100 unsigned BrcKeplerOrbit_T ::
initializationTest()101 initializationTest()
102 {
103 BrcKeplerOrbit empty;
104 CommonTime emptyTime;
105 ObsID emptyObsID;
106 TUDEF("BrcKeplerOrbit", "Default Constructor");
107 TUASSERTE(bool, false, empty.dataLoaded);
108 TUASSERTE(std::string, "", empty.satSys);
109 TUASSERTE(ObsID, emptyObsID, empty.obsID);
110 TUASSERTE(short, 0, empty.PRNID);
111 TUASSERTE(CommonTime, emptyTime, empty.Toe);
112 TUASSERTE(short, 0, empty.URAoe);
113 TUASSERTE(bool, false, empty.healthy);
114 TUASSERTE(double, 0, empty.Cuc);
115 TUASSERTE(double, 0, empty.Cus);
116 TUASSERTE(double, 0, empty.Crc);
117 TUASSERTE(double, 0, empty.Crs);
118 TUASSERTE(double, 0, empty.Cic);
119 TUASSERTE(double, 0, empty.Cis);
120 TUASSERTE(double, 0, empty.M0);
121 TUASSERTE(double, 0, empty.dn);
122 TUASSERTE(double, 0, empty.dndot);
123 TUASSERTE(double, 0, empty.ecc);
124 TUASSERTE(double, 0, empty.A);
125 TUASSERTE(double, 0, empty.Ahalf);
126 TUASSERTE(double, 0, empty.Adot);
127 TUASSERTE(double, 0, empty.OMEGA0);
128 TUASSERTE(double, 0, empty.i0);
129 TUASSERTE(double, 0, empty.w);
130 TUASSERTE(double, 0, empty.OMEGAdot);
131 TUASSERTE(double, 0, empty.idot);
132 TUASSERTE(CommonTime, emptyTime, empty.beginFit);
133 TUASSERTE(CommonTime, emptyTime, empty.endFit);
134 TURETURN();
135 }
136
137
138 unsigned BrcKeplerOrbit_T ::
equalityTest()139 equalityTest()
140 {
141 TUDEF("BrcKeplerOrbit", "operator== / !=");
142 BrcKeplerOrbit orbit;
143 fill(orbit);
144 BrcKeplerOrbit orbitCopy(orbit);
145 // make sure our copy reports as being the same
146 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
147 // Tweak each data member one by one and compare.
148 // Yes the comments are fairly redundant but it helps to
149 // visually separate each batch of statements per data
150 // member.
151 orbitCopy.dataLoaded = false;
152 TUASSERT(orbitCopy != orbit);
153 TUASSERT(!(orbitCopy == orbit));
154 // satSys
155 TUCATCH(orbitCopy = orbit);
156 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
157 orbitCopy.satSys = "twaffle";
158 TUASSERT(orbitCopy != orbit);
159 TUASSERT(!(orbitCopy == orbit));
160 // obsID
161 TUCATCH(orbitCopy = orbit);
162 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
163 orbitCopy.obsID = ObsID(ObservationType::NavMsg, CarrierBand::L1, TrackingCode::P);
164 TUASSERT(orbitCopy != orbit);
165 TUASSERT(!(orbitCopy == orbit));
166 // PRNID
167 TUCATCH(orbitCopy = orbit);
168 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
169 orbitCopy.PRNID = 93;
170 TUASSERT(orbitCopy != orbit);
171 TUASSERT(!(orbitCopy == orbit));
172 // Toe
173 TUCATCH(orbitCopy = orbit);
174 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
175 orbitCopy.Toe = GPSWeekZcount(1234,56789);
176 TUASSERT(orbitCopy != orbit);
177 TUASSERT(!(orbitCopy == orbit));
178 // URAoe
179 TUCATCH(orbitCopy = orbit);
180 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
181 orbitCopy.URAoe = 943;
182 TUASSERT(orbitCopy != orbit);
183 TUASSERT(!(orbitCopy == orbit));
184 // healthy
185 TUCATCH(orbitCopy = orbit);
186 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
187 orbitCopy.healthy = false;
188 TUASSERT(orbitCopy != orbit);
189 TUASSERT(!(orbitCopy == orbit));
190 // Cuc
191 TUCATCH(orbitCopy = orbit);
192 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
193 orbitCopy.Cuc = 1.5e-12;
194 TUASSERT(orbitCopy != orbit);
195 TUASSERT(!(orbitCopy == orbit));
196 // Cus
197 TUCATCH(orbitCopy = orbit);
198 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
199 orbitCopy.Cus = 1.7e-12;
200 TUASSERT(orbitCopy != orbit);
201 TUASSERT(!(orbitCopy == orbit));
202 // Crc
203 TUCATCH(orbitCopy = orbit);
204 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
205 orbitCopy.Crc = 1.9e-12;
206 TUASSERT(orbitCopy != orbit);
207 TUASSERT(!(orbitCopy == orbit));
208 // Crs
209 TUCATCH(orbitCopy = orbit);
210 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
211 orbitCopy.Crs = 2.1e-12;
212 TUASSERT(orbitCopy != orbit);
213 TUASSERT(!(orbitCopy == orbit));
214 // Cic
215 TUCATCH(orbitCopy = orbit);
216 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
217 orbitCopy.Cic = 2.4e-12;
218 TUASSERT(orbitCopy != orbit);
219 TUASSERT(!(orbitCopy == orbit));
220 // Cis
221 TUCATCH(orbitCopy = orbit);
222 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
223 orbitCopy.Cis = 2.5e-12;
224 TUASSERT(orbitCopy != orbit);
225 TUASSERT(!(orbitCopy == orbit));
226 // M0
227 TUCATCH(orbitCopy = orbit);
228 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
229 orbitCopy.M0 = 2.6e-12;
230 TUASSERT(orbitCopy != orbit);
231 TUASSERT(!(orbitCopy == orbit));
232 // dn
233 TUCATCH(orbitCopy = orbit);
234 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
235 orbitCopy.dn = 2.7e-12;
236 TUASSERT(orbitCopy != orbit);
237 TUASSERT(!(orbitCopy == orbit));
238 // dndot
239 TUCATCH(orbitCopy = orbit);
240 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
241 orbitCopy.dndot = 2.8e-12;
242 TUASSERT(orbitCopy != orbit);
243 TUASSERT(!(orbitCopy == orbit));
244 // ecc
245 TUCATCH(orbitCopy = orbit);
246 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
247 orbitCopy.ecc = 2.9e-12;
248 TUASSERT(orbitCopy != orbit);
249 TUASSERT(!(orbitCopy == orbit));
250 // A
251 TUCATCH(orbitCopy = orbit);
252 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
253 orbitCopy.A = 3.0e-12;
254 TUASSERT(orbitCopy != orbit);
255 TUASSERT(!(orbitCopy == orbit));
256 // Ahalf
257 TUCATCH(orbitCopy = orbit);
258 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
259 orbitCopy.Ahalf = 3.1e-12;
260 TUASSERT(orbitCopy != orbit);
261 TUASSERT(!(orbitCopy == orbit));
262 // Adot
263 TUCATCH(orbitCopy = orbit);
264 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
265 orbitCopy.Adot = 3.2e-12;
266 TUASSERT(orbitCopy != orbit);
267 TUASSERT(!(orbitCopy == orbit));
268 // OMEGA0
269 TUCATCH(orbitCopy = orbit);
270 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
271 orbitCopy.OMEGA0 = 3.3e-12;
272 TUASSERT(orbitCopy != orbit);
273 TUASSERT(!(orbitCopy == orbit));
274 // i0
275 TUCATCH(orbitCopy = orbit);
276 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
277 orbitCopy.i0 = 3.4e-12;
278 TUASSERT(orbitCopy != orbit);
279 TUASSERT(!(orbitCopy == orbit));
280 // w
281 TUCATCH(orbitCopy = orbit);
282 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
283 orbitCopy.w = 3.5e-12;
284 TUASSERT(orbitCopy != orbit);
285 TUASSERT(!(orbitCopy == orbit));
286 // OMEGAdot
287 TUCATCH(orbitCopy = orbit);
288 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
289 orbitCopy.OMEGAdot = 3.6e-12;
290 TUASSERT(orbitCopy != orbit);
291 TUASSERT(!(orbitCopy == orbit));
292 // idot
293 TUCATCH(orbitCopy = orbit);
294 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
295 orbitCopy.idot = 3.7e-12;
296 TUASSERT(orbitCopy != orbit);
297 TUASSERT(!(orbitCopy == orbit));
298 // beginFit
299 TUCATCH(orbitCopy = orbit);
300 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
301 orbitCopy.beginFit = GPSWeekZcount(1234,98765);
302 TUASSERT(orbitCopy != orbit);
303 TUASSERT(!(orbitCopy == orbit));
304 // endFit
305 TUCATCH(orbitCopy = orbit);
306 TUASSERTE(BrcKeplerOrbit, orbit, orbitCopy);
307 orbitCopy.endFit = GPSWeekZcount(1267, 56533);
308 TUASSERT(orbitCopy != orbit);
309 TUASSERT(!(orbitCopy == orbit));
310 TURETURN();
311 }
312
313 unsigned BrcKeplerOrbit_T ::
svXvtTest()314 svXvtTest()
315 {
316 TUDEF("BrcKeplerOrbit", "svXvt");
317 BrcKeplerOrbit orbit;
318 ObsID oi(ObservationType::NavMsg, CarrierBand::L1, TrackingCode::Y);
319 gpstk::CommonTime toc = gpstk::CivilTime(2015,7,19,1,59,28.0,
320 gpstk::TimeSystem::GPS);
321 orbit.loadData("GPS", oi, 2, toc, toc+7200,
322 gpstk::GPSWeekSecond(1854,.716800000000e+04), 0, true,
323 -.324845314026e-05, .101532787085e-04, .168968750000e+03,
324 -.646250000000e+02, .320374965668e-06, .117346644402e-06,
325 -.136404614938e+01, .489591822036e-08, 0,
326 .146582192974e-01, .515359719276e+04 * .515359719276e+04,
327 .515359719276e+04, 0, -.296605403382e+01,
328 .941587707856e+00, -.224753761329e+01, -.804390648956e-08,
329 .789318592573e-10);
330
331 bool testFailed = false;
332 try
333 {
334 // first compute Xvt
335 static const unsigned SECONDS = 7200;
336 gpstk::Xvt zeroth_array[SECONDS];
337 for (unsigned ii = 0; ii < SECONDS; ii++)
338 {
339 zeroth_array[ii] = orbit.svXvt(orbit.getOrbitEpoch() + ii);
340 }
341 // then compute first derivative of position, i.e. velocity
342 gpstk::Triple deriv[SECONDS];
343 double h = 1; // time step size in seconds
344 for (unsigned ii = 0; ii < SECONDS; ii++)
345 {
346 if (ii == 0)
347 {
348 deriv[ii] = (1/h)*(-1.5*zeroth_array[ii].getPos() +
349 2.0*zeroth_array[ii+1].getPos() -
350 0.5*zeroth_array[ii+2].getPos());
351 }
352 else if ((ii == 1) || (ii == (SECONDS-2)))
353 {
354 deriv[ii] = (1/h)*(-0.5*zeroth_array[ii-1].getPos() +
355 0.5*zeroth_array[ii+1].getPos());
356 }
357 else if (ii == (SECONDS-1))
358 {
359 deriv[ii] = (1/h)*(0.5*zeroth_array[ii-2].getPos() -
360 2.0*zeroth_array[ii-1].getPos() +
361 1.5*zeroth_array[ii].getPos());
362 }
363 else
364 {
365 deriv[ii] = (1/h)*((1.0/12.0)*zeroth_array[ii-2].getPos() -
366 (2.0/3.0)*zeroth_array[ii-1].getPos() +
367 (2.0/3.0)*zeroth_array[ii+1].getPos() -
368 (1.0/12.0)*zeroth_array[ii+2].getPos());
369 }
370 }
371 // then check the difference between derived and computed velocity
372 for (unsigned ii = 0; ii < SECONDS; ii++)
373 {
374 double derivedMag = deriv[ii].mag();
375 double computedMag = zeroth_array[ii].getVel().mag();
376 // If you want to print the data e.g. to plot, uncomment
377 // this stream output statement and comment out tbe break
378 // statement below.
379 // Just don't check it in that way, please.
380 // cerr << ii << " " << (computedMag - derivedMag) << endl;
381 if (fabs(computedMag - derivedMag) > velDiffThresh)
382 {
383 // no sense in printing 7200 success/fail messages.
384 testFailed = true;
385 break;
386 }
387 }
388 if (testFailed)
389 {
390 TUFAIL("computed velocity is significantly different from derived"
391 " velocity");
392 }
393 else
394 {
395 TUPASS("velocity check");
396 }
397 }
398 catch (gpstk::Exception& exc)
399 {
400 cerr << exc;
401 TUFAIL("Exception");
402 }
403 catch (...)
404 {
405 TUFAIL("Exception");
406 }
407 TURETURN();
408 }
409
410
411 unsigned BrcKeplerOrbit_T ::
relativityTest()412 relativityTest()
413 {
414 TUDEF("BrcKeplerOrbit", "svRelativity");
415 BrcKeplerOrbit orbit;
416 fill(orbit);
417 TUASSERTFE(-1.7274634252517538304e-08,
418 orbit.svRelativity(GPSWeekZcount(1886, 398400)));
419 TURETURN();
420 }
421
422
423 unsigned BrcKeplerOrbit_T ::
fitIntTest()424 fitIntTest()
425 {
426 TUDEF("BrcKeplerOrbit", "getBeginningOfFitInterval");
427 BrcKeplerOrbit orbit;
428 fill(orbit);
429 CommonTime beg(GPSWeekZcount(1886, 398400));
430 CommonTime end(GPSWeekZcount(1887, 0));
431 CommonTime before(beg-1), after(end+1);
432 TUASSERTE(CommonTime, beg, orbit.getBeginningOfFitInterval());
433 TUCSM("getEndOfFitInterval");
434 TUASSERTE(CommonTime, end, orbit.getEndOfFitInterval());
435 TUCSM("withinFitInterval");
436 TUASSERT(!orbit.withinFitInterval(before));
437 TUASSERT(orbit.withinFitInterval(beg));
438 TUASSERT(orbit.withinFitInterval(end));
439 TUASSERT(!orbit.withinFitInterval(after));
440 TURETURN();
441 }
442
443
main()444 int main() //Main function to initialize and run all tests above
445 {
446 using namespace std;
447 BrcKeplerOrbit_T testClass;
448 unsigned errorTotal = 0;
449
450 errorTotal += testClass.initializationTest();
451 errorTotal += testClass.equalityTest();
452 errorTotal += testClass.svXvtTest();
453 errorTotal += testClass.relativityTest();
454 errorTotal += testClass.fitIntTest();
455
456 cout << "Total Failures for " << __FILE__ << ": " << errorTotal << endl;
457
458 return errorTotal; // Return the total number of errors
459 }
460