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