1 |
|
---|
2 | #include <cmath>
|
---|
3 |
|
---|
4 | #include "bncconst.h"
|
---|
5 | #include "windup.h"
|
---|
6 | #include "bncutils.h"
|
---|
7 | #include "bnctides.h"
|
---|
8 |
|
---|
9 | using namespace std;
|
---|
10 | using namespace BNC;
|
---|
11 |
|
---|
12 | // Constructor
|
---|
13 | ///////////////////////////////////////////////////////////////////////////
|
---|
14 | t_windUp::t_windUp() {
|
---|
15 | for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) {
|
---|
16 | sumWind[ii] = 0.0;
|
---|
17 | lastEtime[ii] = 0.0;
|
---|
18 | }
|
---|
19 | }
|
---|
20 |
|
---|
21 | // Phase Wind-Up Correction
|
---|
22 | ///////////////////////////////////////////////////////////////////////////
|
---|
23 | double t_windUp::value(const bncTime& etime, const ColumnVector& rRec,
|
---|
24 | t_prn prn, const ColumnVector& rSat) {
|
---|
25 |
|
---|
26 | if (etime.mjddec() != lastEtime[prn.toInt()]) {
|
---|
27 |
|
---|
28 | // Unit Vector GPS Satellite --> Receiver
|
---|
29 | // --------------------------------------
|
---|
30 | ColumnVector rho = rRec - rSat;
|
---|
31 | rho /= rho.norm_Frobenius();
|
---|
32 |
|
---|
33 | // GPS Satellite unit Vectors sz, sy, sx
|
---|
34 | // -------------------------------------
|
---|
35 | ColumnVector sz = -rSat / rSat.norm_Frobenius();
|
---|
36 |
|
---|
37 | ColumnVector xSun = Sun(etime.mjddec());
|
---|
38 | xSun /= xSun.norm_Frobenius();
|
---|
39 |
|
---|
40 | ColumnVector sy = crossproduct(sz, xSun);
|
---|
41 | ColumnVector sx = crossproduct(sy, sz);
|
---|
42 |
|
---|
43 | // Effective Dipole of the GPS Satellite Antenna
|
---|
44 | // ---------------------------------------------
|
---|
45 | ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
|
---|
46 | - crossproduct(rho, sy);
|
---|
47 |
|
---|
48 | // Receiver unit Vectors rx, ry
|
---|
49 | // ----------------------------
|
---|
50 | ColumnVector rx(3);
|
---|
51 | ColumnVector ry(3);
|
---|
52 |
|
---|
53 | double recEll[3]; xyz2ell(rRec.data(), recEll) ;
|
---|
54 | double neu[3];
|
---|
55 |
|
---|
56 | neu[0] = 1.0;
|
---|
57 | neu[1] = 0.0;
|
---|
58 | neu[2] = 0.0;
|
---|
59 | neu2xyz(recEll, neu, rx.data());
|
---|
60 |
|
---|
61 | neu[0] = 0.0;
|
---|
62 | neu[1] = -1.0;
|
---|
63 | neu[2] = 0.0;
|
---|
64 | neu2xyz(recEll, neu, ry.data());
|
---|
65 |
|
---|
66 | // Effective Dipole of the Receiver Antenna
|
---|
67 | // ----------------------------------------
|
---|
68 | ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
|
---|
69 | + crossproduct(rho, ry);
|
---|
70 |
|
---|
71 | // Resulting Effect
|
---|
72 | // ----------------
|
---|
73 | double alpha = DotProduct(dipSat,dipRec) /
|
---|
74 | (dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
|
---|
75 |
|
---|
76 | if (alpha > 1.0) alpha = 1.0;
|
---|
77 | if (alpha < -1.0) alpha = -1.0;
|
---|
78 |
|
---|
79 | double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
|
---|
80 |
|
---|
81 | if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
|
---|
82 | dphi = -dphi;
|
---|
83 | }
|
---|
84 |
|
---|
85 | if (lastEtime[prn.toInt()] == 0.0) {
|
---|
86 | sumWind[prn.toInt()] = dphi;
|
---|
87 | }
|
---|
88 | else {
|
---|
89 | sumWind[prn.toInt()] = nint(sumWind[prn.toInt()] - dphi) + dphi;
|
---|
90 | }
|
---|
91 |
|
---|
92 | lastEtime[prn.toInt()] = etime.mjddec();
|
---|
93 | }
|
---|
94 |
|
---|
95 | return sumWind[prn.toInt()];
|
---|
96 | }
|
---|