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 /**
40 * @file StochasticModels.cpp
41 * Define stochastic model for measurement errors used in estimation.
42 */
43
44 //------------------------------------------------------------------------------------
45 // system includes
46
47 // GPSTk
48 #include "Vector.hpp"
49 #include "Matrix.hpp"
50 #include "Namelist.hpp"
51 #include "GNSSconstants.hpp"
52 #include "RobustStats.hpp"
53 #include "format.hpp"
54
55 // DDBase
56 #include "DDBase.hpp"
57 #include "index.hpp"
58
59 //------------------------------------------------------------------------------------
60 using namespace std;
61 using namespace gpstk;
62
63 //------------------------------------------------------------------------------------
64 // prototypes -- this module only
65 // called by ConfigureEstimation(), which is Configure(3)
66 int ConfigureStochasticModel(void);
67
68 // called by Estimation() -- inside the loop
69 void BuildStochasticModel(int count, Namelist& DNL, Matrix<double>& MCov);
70
71 // called by BuildStochasticModel(), inside the estimation loop
72 double StochasticWeight(OWid & owid);
73
74 // called by BuildStochasticModel()
75 void DecomposeName(const string& label, string& site1, string& site2,
76 GSatID& sat1, GSatID& sat2); // Estimation.cpp
77
78 // ElevationMask.cpp
79 bool ElevationMask(double elevation, double azimuth);
80
81 //------------------------------------------------------------------------------------
82 // local data
83 // simple cos squared model
84 double eps; // 'fudge' factor to avoid blowup at zenith
85 double sig0; // sigma at the minimum elevation
86 double d0; // weight at the minimum elevation
87 // other models...
88
89 //------------------------------------------------------------------------------------
90 //------------------------------------------------------------------------------------
91 // Called by Configure(3) or ConfigureEstimation(), just before Estimation loop.
92 // Configure the stochastic model.
93 // TD MinElevation here should be a separate parameter, not necessarily the mask angle
ConfigureStochasticModel(void)94 int ConfigureStochasticModel(void)
95 {
96 try {
97 if(CI.Verbose) oflog << "BEGIN ConfigureStochasticModel() with model "
98 << CI.StochasticModel
99 << " at total time " << fixed << setprecision(3)
100 << double(clock()-totaltime)/double(CLOCKS_PER_SEC) << " seconds."
101 << endl;
102
103 if(CI.StochasticModel == string("cos") || CI.StochasticModel == string("cos2")) {
104 // ----------------------------------------
105 // simple cosine or cosine squared model
106 double coselev; // eps + cosine(minimum elevation)
107 //eps = 0.0000001; seems to have no effect
108 eps = 0.001; // TD new input param?
109 // d needs to have units meters and be realistic ~ sigma(phase)
110 // =sig0(m) at min elev, smaller at higher elevation
111 sig0 = 1.0e-3; // smaller than e-2, else little effect // TD new input param?
112 coselev = eps + cos(CI.MinElevation * DEG_TO_RAD); // TD new input param?
113 if(CI.StochasticModel == string("cos2"))
114 d0 = sig0 / (coselev*coselev); // cosine squared model
115 if(CI.StochasticModel == string("cos"))
116 d0 = sig0 / coselev; // cosine model
117
118 return 0;
119 }
120 else {
121 Exception e("Unknown stochastic model identifier: " + CI.StochasticModel);
122 GPSTK_THROW(e);
123 }
124
125 return 0;
126 }
127 catch(Exception& e) { GPSTK_RETHROW(e); }
128 catch(std::exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
129 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
130 }
131
132 //------------------------------------------------------------------------------------
133 // compute the weight for a single one-way id (one site, one satellite) at count
StochasticWeight(OWid & owid,int count)134 double StochasticWeight(OWid & owid, int count)
135 {
136 try {
137 int j;
138 double cosine;
139
140 j = index(Stations[owid.site].RawDataBuffers[owid.sat].count,count);
141 if(j == -1) {
142 ostringstream oss;
143 oss << "Error -- count " << count << " not found in buffer for " << owid;
144 Exception e(oss.str());
145 GPSTK_THROW(e);
146 }
147
148 if(CI.StochasticModel == string("cos") || CI.StochasticModel == string("cos2")) {
149 // ----------------------------------------
150 // simple cosine squared model
151 cosine = eps + cos(Stations[owid.site].RawDataBuffers[owid.sat].elev[j]
152 * DEG_TO_RAD); // eps + cosine(minimum elevation)
153 if(CI.StochasticModel == string("cos2"))
154 return (d0 * cosine * cosine); // cosine squared model
155 if(CI.StochasticModel == string("cos"))
156 return (d0 * cosine); // cosine model
157 }
158
159 return 0;
160 }
161 catch(Exception& e) { GPSTK_RETHROW(e); }
162 catch(std::exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
163 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
164 }
165
166 //------------------------------------------------------------------------------------
167 // called by Estimation() - inside the data loop, inside the iteration loop
168 // Input is Namelist DNL, the double difference data Namelist (DataNL)
169 // Output is MCov, the measurement covariance matrix for this data (MeasCov).
170 // Let:
171 // d = vector of one-way data (one site, one satellite)
172 // sd = vector of single difference data (two sites, one satellite)
173 // dd = vector of double difference data (two sites, two satellites)
174 // DD and SD are matricies with elements 0,1,-1 which transform d to sd to dd:
175 // sd = SD * d
176 // dd = DD * sd
177 // dd = DD * SD * d
178 // The covariance matrix will be MC = (DD*SD)*transpose(DD*SD)
179 // = DD*SD*transpose(SD)*transpose(DD)
180 // If the one-way data has a measurement covariance, then fill the vector d with
181 // them; then MC = DD*SD* d * transpose(SD)*transpose(DD).
182 // Building DD and SD is just a matter of lists:
183 // loop through the dd namelist, keeping lists of:
184 // one-way data (site-satellite pairs) (d)
185 // single differences (site-site-satellite sets) (sd)
186 // and you have a list of double differences (DNL)
187 //
BuildStochasticModel(int count,Namelist & DNL,Matrix<double> & MCov)188 void BuildStochasticModel(int count, Namelist& DNL, Matrix<double>& MCov)
189 {
190 try {
191 unsigned int m=DNL.size();
192 if(m==0) return;
193
194 int jn,kn;
195 size_t i, in;
196 string site1,site2;
197 GSatID sat1,sat2;
198 vector<double> d; // weights of one-way data
199 vector<OWid> ld; // labels of d
200 vector<SDid> sd;
201
202 for(i=0; i<DNL.size(); i++) {
203 // break the label into site1,site2,sat1,sat2
204 DecomposeName(DNL.getName(i), site1, site2, sat1, sat2);
205 if(index(ld,OWid(site1,sat1)) == -1) ld.push_back(OWid(site1,sat1));
206 if(index(ld,OWid(site1,sat2)) == -1) ld.push_back(OWid(site1,sat2));
207 if(index(ld,OWid(site2,sat1)) == -1) ld.push_back(OWid(site2,sat1));
208 if(index(ld,OWid(site2,sat2)) == -1) ld.push_back(OWid(site2,sat2));
209 if(index(sd,SDid(site1,site2,sat1)) == -1) sd.push_back(SDid(site1,site2,sat1));
210 if(index(sd,SDid(site1,site2,sat2)) == -1) sd.push_back(SDid(site1,site2,sat2));
211 }
212
213 // fill d with the weights
214 d = vector<double>(ld.size());
215 for(i=0; i<ld.size(); i++) d[i] = StochasticWeight(ld[i], count);
216
217 // temp
218 //format f113s(11,3,2);
219 //oflog << "DDs are (" << DNL.size() << "):\n" << setw(20) << DNL << endl;
220 //oflog << "SDs are: (" << sd.size() << ")" << fixed << endl;
221 //for(i=0; i<sd.size(); i++) oflog << " / " << sd[i];
222 //oflog << endl;
223 //oflog << "OWs are: (" << ld.size() << ")" << endl;
224 //for(i=0; i<ld.size(); i++) oflog << " / " << ld[i];
225 //oflog << endl;
226 //oflog << "OW wts are: (" << d.size() << ")" << endl;
227 //for(i=0; i<d.size(); i++) oflog << " " << f113s << d[i];
228 //oflog << endl;
229
230 Matrix<double> SD(sd.size(),ld.size(),0.0);
231 Matrix<double> DD(m,sd.size(),0.0);
232 // TD need to account for signs here ... sd[.] may be site2,site1,sat1 ...
233 for(in=0; in<DNL.size(); in++) {
234 DecomposeName(DNL.getName(in), site1, site2, sat1, sat2);
235 jn = index(sd,SDid(site1,site2,sat1)); // site1-site2, sat1
236 DD(in,jn) = 1;
237 kn = index(ld,OWid(site1,sat1)); // site1, sat1
238 SD(jn,kn) = d[kn];
239 kn = index(ld,OWid(site2,sat1)); // site2, sat1
240 SD(jn,kn) = -d[kn];
241
242 jn = index(sd,SDid(site1,site2,sat2)); // site1-site2, sat2
243 DD(in,jn) = -1;
244 kn = index(ld,OWid(site1,sat2)); // site1, sat2
245 SD(jn,kn) = d[kn];
246 kn = index(ld,OWid(site2,sat2)); // site2, sat2
247 SD(jn,kn) = -d[kn];
248 }
249
250 //oflog << " SD is\n" << fixed << setw(3) << SD << endl;
251 //oflog << " DD is\n" << fixed << setw(3) << DD << endl;
252
253 Matrix<double> T;
254 T = DD * SD;
255 MCov = T * transpose(T);
256
257 static bool once=true;
258 if(once) {
259 oflog << "Measurement covariance (model " << CI.StochasticModel << ") is\n"
260 << scientific << setw(8) << setprecision(3) << MCov << endl;
261 once = false;
262 }
263
264 }
265 catch(Exception& e) { GPSTK_RETHROW(e); }
266 catch(std::exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
267 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
268 }
269
270 //------------------------------------------------------------------------------------
271 //------------------------------------------------------------------------------------
272