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 <iostream>
40 #include "TestUtil.hpp"
41 #include "BDSEphemeris.hpp"
42 #include "CivilTime.hpp"
43 #include "BDSWeekSecond.hpp"
44 #include "TimeString.hpp"
45
46 using namespace std;
47
48 /** Threshold for how much different our velocities can be between
49 * being computed directly via svXvt and computed via differencing
50 * svXvt positions over time. */
51 double velDiffThresh = 0.0008;
52
53
54 class BDSEphemeris_T
55 {
56 public:
57 unsigned testSvXvtMEO();
58 unsigned testSvXvtGEO();
59 // call writeVel for a variety of ephemerides
60 // unsigned wut();
61 /** Write to a file the difference between the magnitude of the
62 * velocity vector as computed by svXvt and the magnitude of the
63 * position derivative as computed by this function. For
64 * plotting. */
65 // void writeVel(const gpstk::BDSEphemeris& oe);
66 };
67
68
69 unsigned BDSEphemeris_T ::
testSvXvtMEO()70 testSvXvtMEO()
71 {
72 TUDEF("BDSEphemeris", "svXvt");
73 // Hard code orbital parameters mostly so we can copy and paste
74 // the data into other similar tests with minimal changes.
75 gpstk::BDSEphemeris oe;
76 oe.Cuc = -.324845314026e-05;
77 oe.Cus = .101532787085e-04;
78 oe.Crc = .168968750000e+03;
79 oe.Crs = -.646250000000e+02;
80 oe.Cic = .320374965668e-06;
81 oe.Cis = .117346644402e-06;
82 oe.M0 = -.136404614938e+01;
83 oe.dn = .489591822036e-08;
84 oe.dndot = 0;
85 oe.ecc = .146582192974e-01;
86 oe.A = .515359719276e+04 * .515359719276e+04;
87 oe.Adot = 0;
88 oe.OMEGA0 = -.296605403382e+01;
89 oe.i0 = .941587707856e+00;
90 oe.w = -.224753761329e+01;
91 oe.OMEGAdot = -.804390648956e-08;
92 oe.idot = .789318592573e-10;
93 oe.ctToc = gpstk::CivilTime(2015,7,19,1,59,28.0,gpstk::TimeSystem::BDT);
94 oe.af0 = .579084269702e-03;
95 oe.af1 = .227373675443e-11;
96 oe.af2 = .000000000000e+00;
97 oe.dataLoadedFlag = true;
98 oe.satID = gpstk::SatID(2, gpstk::SatelliteSystem::BeiDou);
99 oe.ctToe = gpstk::BDSWeekSecond(498,.716800000000e+04);
100 // iode .700000000000e+01
101 // codes on L2 .100000000000e+01
102 // L2 P data .000000000000e+00
103 // sv accuracy .240000000000e+01
104 // sv health .000000000000e+00
105 // tgd -.204890966415e-07
106 // iodc .700000000000e+01
107 // xmit time .360000000000e+04
108 // fit int .400000000000e+01
109 bool testFailed = false;
110 try
111 {
112 // first compute Xvt
113 static const unsigned SECONDS = 7200;
114 gpstk::Xvt zeroth_array[SECONDS];
115 for (unsigned ii = 0; ii < SECONDS; ii++)
116 {
117 zeroth_array[ii] = oe.svXvt(oe.ctToc + ii);
118 }
119 // then compute first derivative of position, i.e. velocity
120 gpstk::Triple deriv[SECONDS];
121 double h = 1; // time step size in seconds
122 for (unsigned ii = 0; ii < SECONDS; ii++)
123 {
124 if (ii == 0)
125 {
126 deriv[ii] = (1/h)*(-1.5*zeroth_array[ii].getPos() +
127 2.0*zeroth_array[ii+1].getPos() -
128 0.5*zeroth_array[ii+2].getPos());
129 }
130 else if ((ii == 1) || (ii == (SECONDS-2)))
131 {
132 deriv[ii] = (1/h)*(-0.5*zeroth_array[ii-1].getPos() +
133 0.5*zeroth_array[ii+1].getPos());
134 }
135 else if (ii == (SECONDS-1))
136 {
137 deriv[ii] = (1/h)*(0.5*zeroth_array[ii-2].getPos() -
138 2.0*zeroth_array[ii-1].getPos() +
139 1.5*zeroth_array[ii].getPos());
140 }
141 else
142 {
143 deriv[ii] = (1/h)*((1.0/12.0)*zeroth_array[ii-2].getPos() -
144 (2.0/3.0)*zeroth_array[ii-1].getPos() +
145 (2.0/3.0)*zeroth_array[ii+1].getPos() -
146 (1.0/12.0)*zeroth_array[ii+2].getPos());
147 }
148 }
149 // then check the difference between derived and computed velocity
150 for (unsigned ii = 0; ii < SECONDS; ii++)
151 {
152 double derivedMag = deriv[ii].mag();
153 double computedMag = zeroth_array[ii].getVel().mag();
154 // If you want to print the data e.g. to plot, uncomment
155 // this stream output statement and comment out tbe break
156 // statement below.
157 // Just don't check it in that way, please.
158 // cerr << ii << " " << (computedMag - derivedMag) << endl;
159 if (fabs(computedMag - derivedMag) > velDiffThresh)
160 {
161 // no sense in printing 7200 success/fail messages.
162 testFailed = true;
163 break;
164 }
165 }
166 if (testFailed)
167 {
168 TUFAIL("computed velocity is significantly different from derived"
169 " velocity");
170 }
171 else
172 {
173 TUPASS("velocity check");
174 }
175 }
176 catch (gpstk::Exception& exc)
177 {
178 cerr << exc;
179 TUFAIL("Exception");
180 }
181 catch (...)
182 {
183 TUFAIL("Exception");
184 }
185 TURETURN();
186 }
187
188
189 unsigned BDSEphemeris_T ::
testSvXvtGEO()190 testSvXvtGEO()
191 {
192 TUDEF("BDSEphemeris", "svXvt");
193 // Hard code orbital parameters mostly so we can copy and paste
194 // the data into other similar tests with minimal changes.
195 gpstk::BDSEphemeris oe;
196 oe.Cuc = -1.08121894E-05;
197 oe.Cus = -1.25728548E-06;
198 oe.Crc = 3.97031250E+01;
199 oe.Crs = -3.23656250E+02;
200 oe.Cic = -2.02562660E-07;
201 oe.Cis = -2.00234354E-08;
202 oe.M0 = 2.81324357E+00;
203 oe.dn = -1.00075597E-09;
204 oe.dndot = 0.00000000E+00;
205 oe.ecc = 2.62024812E-04;
206 oe.A = 4.21651139E+07;
207 oe.Adot = 0.00000000E+00;
208 oe.OMEGA0 = -2.99944238E+00;
209 oe.i0 = 1.06909427E-01;
210 oe.w = 2.63078773E+00;
211 oe.OMEGAdot = 2.13687472E-09;
212 oe.idot = 1.45363198E-10;
213 oe.ctToc = gpstk::CivilTime(2019,3,1,0,0,0,gpstk::TimeSystem::BDT);
214 oe.af0 = 2.59640510E-04;
215 oe.af1 = 4.48929782E-11;
216 oe.af2 = 0.00000000E+00;
217 oe.dataLoadedFlag = true;
218 oe.satID = gpstk::SatID(1, gpstk::SatelliteSystem::BeiDou);
219 oe.ctToe = oe.ctToc;
220 bool testFailed = false;
221 try
222 {
223 // first compute Xvt
224 static const unsigned SECONDS = 7200;
225 gpstk::Xvt zeroth_array[SECONDS];
226 for (unsigned ii = 0; ii < SECONDS; ii++)
227 {
228 zeroth_array[ii] = oe.svXvt(oe.ctToc + ii);
229 }
230 // then compute first derivative of position, i.e. velocity
231 gpstk::Triple deriv[SECONDS];
232 double h = 1; // time step size in seconds
233 for (unsigned ii = 0; ii < SECONDS; ii++)
234 {
235 if (ii == 0)
236 {
237 deriv[ii] = (1/h)*(-1.5*zeroth_array[ii].getPos() +
238 2.0*zeroth_array[ii+1].getPos() -
239 0.5*zeroth_array[ii+2].getPos());
240 }
241 else if ((ii == 1) || (ii == (SECONDS-2)))
242 {
243 deriv[ii] = (1/h)*(-0.5*zeroth_array[ii-1].getPos() +
244 0.5*zeroth_array[ii+1].getPos());
245 }
246 else if (ii == (SECONDS-1))
247 {
248 deriv[ii] = (1/h)*(0.5*zeroth_array[ii-2].getPos() -
249 2.0*zeroth_array[ii-1].getPos() +
250 1.5*zeroth_array[ii].getPos());
251 }
252 else
253 {
254 deriv[ii] = (1/h)*((1.0/12.0)*zeroth_array[ii-2].getPos() -
255 (2.0/3.0)*zeroth_array[ii-1].getPos() +
256 (2.0/3.0)*zeroth_array[ii+1].getPos() -
257 (1.0/12.0)*zeroth_array[ii+2].getPos());
258 }
259 }
260 // then check the difference between derived and computed velocity
261 for (unsigned ii = 0; ii < SECONDS; ii++)
262 {
263 double derivedMag = deriv[ii].mag();
264 double computedMag = zeroth_array[ii].getVel().mag();
265 // If you want to print the data e.g. to plot, uncomment
266 // this stream output statement and comment out tbe break
267 // statement below.
268 // Just don't check it in that way, please.
269 // cerr << ii << " " << (computedMag - derivedMag) << endl;
270 if (fabs(computedMag - derivedMag) > velDiffThresh)
271 {
272 // no sense in printing 7200 success/fail messages.
273 testFailed = true;
274 break;
275 }
276 }
277 if (testFailed)
278 {
279 TUFAIL("computed velocity is significantly different from derived"
280 " velocity");
281 }
282 else
283 {
284 TUPASS("velocity check");
285 }
286 }
287 catch (gpstk::Exception& exc)
288 {
289 cerr << exc;
290 TUFAIL("Exception");
291 }
292 catch (...)
293 {
294 TUFAIL("Exception");
295 }
296 TURETURN();
297 }
298
299
300 // I'm not saving all the code generated for wut().
301 // Look at dump2code.pl if you need to do this again.
302 // For now it's all being left in the file but #if'd out.
303 #if 0
304 unsigned BDSEphemeris_T ::
305 wut()
306 {
307 try
308 {
309 gpstk::BDSEphemeris oe;
310 oe.Cuc = -1.08121894E-05;
311 oe.Cus = -1.25728548E-06;
312 oe.Crc = 3.97031250E+01;
313 oe.Crs = -3.23656250E+02;
314 oe.Cic = -2.02562660E-07;
315 oe.Cis = -2.00234354E-08;
316 oe.M0 = 2.81324357E+00;
317 oe.dn = -1.00075597E-09;
318 oe.dndot = 0.00000000E+00;
319 oe.ecc = 2.62024812E-04;
320 oe.A = 4.21651139E+07;
321 oe.Adot = 0.00000000E+00;
322 oe.OMEGA0 = -2.99944238E+00;
323 oe.i0 = 1.06909427E-01;
324 oe.w = 2.63078773E+00;
325 oe.OMEGAdot = 2.13687472E-09;
326 oe.idot = 1.45363198E-10;
327 oe.ctToc = gpstk::CivilTime(2020,3,1,0,0,0,gpstk::TimeSystem::BDT);
328 oe.af0 = 2.59640510E-04;
329 oe.af1 = 4.48929782E-11;
330 oe.af2 = 0.00000000E+00;
331 oe.dataLoadedFlag = true;
332 oe.satID = gpstk::SatID(1, gpstk::SatelliteSystem::BeiDou);
333 oe.ctToe = gpstk::CivilTime(2020,3,1,0,0,0,gpstk::TimeSystem::BDT);
334 writeVel(oe);
335 }
336 catch(...)
337 {
338 cerr << "exception" << endl;
339 }
340 }
341
342
343 void BDSEphemeris_T ::
344 writeVel(const gpstk::BDSEphemeris& oe)
345 {
346 ostringstream ss;
347 ss << setw(2) << setfill('0') << oe.satID.id << "_"
348 << gpstk::printTime(oe.ctToc, "%04Y%02m%02d_%02H%02M%02S.dat");
349 ofstream s(ss.str().c_str());
350 try
351 {
352 // first compute Xvt
353 static const unsigned SECONDS = 7200;
354 gpstk::Xvt zeroth_array[SECONDS];
355 for (unsigned ii = 0; ii < SECONDS; ii++)
356 {
357 zeroth_array[ii] = oe.svXvt(oe.ctToc + ii);
358 }
359 // then compute first derivative of position, i.e. velocity
360 gpstk::Triple deriv[SECONDS];
361 double h = 1; // time step size in seconds
362 for (unsigned ii = 0; ii < SECONDS; ii++)
363 {
364 if (ii == 0)
365 {
366 deriv[ii] = (1/h)*(-1.5*zeroth_array[ii].getPos() +
367 2.0*zeroth_array[ii+1].getPos() -
368 0.5*zeroth_array[ii+2].getPos());
369 }
370 else if ((ii == 1) || (ii == (SECONDS-2)))
371 {
372 deriv[ii] = (1/h)*(-0.5*zeroth_array[ii-1].getPos() +
373 0.5*zeroth_array[ii+1].getPos());
374 }
375 else if (ii == (SECONDS-1))
376 {
377 deriv[ii] = (1/h)*(0.5*zeroth_array[ii-2].getPos() -
378 2.0*zeroth_array[ii-1].getPos() +
379 1.5*zeroth_array[ii].getPos());
380 }
381 else
382 {
383 deriv[ii] = (1/h)*((1.0/12.0)*zeroth_array[ii-2].getPos() -
384 (2.0/3.0)*zeroth_array[ii-1].getPos() +
385 (2.0/3.0)*zeroth_array[ii+1].getPos() -
386 (1.0/12.0)*zeroth_array[ii+2].getPos());
387 }
388 }
389 // then write the difference between derived and computed velocity
390 for (unsigned ii = 0; ii < SECONDS; ii++)
391 {
392 double derivedMag = deriv[ii].mag();
393 double computedMag = zeroth_array[ii].getVel().mag();
394 s << ii << " " << (computedMag - derivedMag) << endl;
395 }
396 }
397 catch (gpstk::Exception& exc)
398 {
399 cerr << exc;
400 }
401 catch (...)
402 {
403 cerr << "exception" << endl;
404 }
405 }
406 #endif
407
408
main()409 int main()
410 {
411 unsigned total = 0;
412 BDSEphemeris_T testClass;
413 total += testClass.testSvXvtMEO();
414 total += testClass.testSvXvtGEO();
415 //testClass.wut();
416
417 cout << "Total Failures for " << __FILE__ << ": " << total << endl;
418 return total;
419 }
420