source: ntrip/trunk/BNC/src/PPP/ambres.cpp@ 10791

Last change on this file since 10791 was 10791, checked in by mervart, 3 days ago

BNC Multifrequency and PPPAR Client (initial version)
File size: 7.7 KB
Line 
1#include <set>
2#include <algorithm>
3#include <math.h>
4#include <newmatio.h>
5
6#include "ambres.h"
7#include "bncutils.h"
8#include "lambda.h"
9#include "pppClient.h"
10
11using namespace std;
12using namespace BNC_PPP;
13
14//
15//////////////////////////////////////////////////////////////////////////////////////////
16AmbRes::AmbRes() {
17 reset();
18}
19
20//
21//////////////////////////////////////////////////////////////////////////////////////////
22AmbRes::~AmbRes() {
23}
24
25//
26//////////////////////////////////////////////////////////////////////////////////////////
27bool AmbRes::isResolvable(const t_pppParam* amb) {
28 if ( amb->ambNumEpo() >= OPT->_ar._minNumEpo && amb->ambEleSat() >= OPT->_ar._minEle) {
29 return true;
30 }
31 else {
32 return false;
33 }
34}
35
36//
37//////////////////////////////////////////////////////////////////////////////////////////
38bool AmbRes::isFixable(double xx, double sigma, bool* checked) {
39 if (OPT->_ar._maxFrac <= 0.0 || OPT->_ar._maxSig <= 0.0) {
40 if (checked) {
41 *checked = false;
42 }
43 return true;
44 }
45 else {
46 if (checked) {
47 *checked = true;
48 }
49 return ( fabs(xx - nint(xx)) <= OPT->_ar._maxFrac && sigma <= OPT->_ar._maxSig );
50 }
51}
52
53//
54//////////////////////////////////////////////////////////////////////////////////////////
55void AmbRes::addToGroup(const t_pppParam* amb) {
56 t_lc LC = amb->LC();
57 char sys = amb->prn().system();
58 AmbGroup* p_ambGroup = 0;
59 for (AmbGroup& ambGroup : _ambGroups) {
60 if (ambGroup._system == sys && ambGroup._LC == LC) {
61 p_ambGroup = &ambGroup;
62 break;
63 }
64 }
65 if (!p_ambGroup) {
66 _ambGroups.emplace_back(AmbGroup(sys, LC));
67 p_ambGroup = &_ambGroups.back();
68 }
69 p_ambGroup->_zdAmbs.push_back(ZdAmb(amb));
70
71 _numZdAmbs += 1;
72}
73
74// Select Reference Ambiguity
75////////////////////////////////////////////////////////////////////////////////////////////
76void AmbRes::selectReference(AmbGroup& ambGroup) {
77
78 // Compute sum of variances of all double-difference ambiguities
79 // -------------------------------------------------------------
80 double minVarSum = 0.0;
81 for (const ZdAmb& zdAmb1 : ambGroup._zdAmbs) {
82 int i1 = zdAmb1.index();
83 double varSum = 0.0;
84 for (const ZdAmb& zdAmb2 : ambGroup._zdAmbs) {
85 int i2 = zdAmb2.index();
86 varSum += _QQ[i1][i1] - 2.0 * _QQ(i1+1,i2+1) + _QQ[i2][i2];
87 }
88 if (ambGroup._zdAmbRef == 0 || minVarSum > varSum) {
89 ambGroup._zdAmbRef = &zdAmb1;
90 minVarSum = varSum;
91 }
92 }
93}
94
95//
96//////////////////////////////////////////////////////////////////////////////////////////
97t_irc AmbRes::run(const bncTime& epoTime,
98 const std::vector<t_pppParam*>& params,
99 SymmetricMatrix& QFinal,
100 ColumnVector& xFinal,
101 double& fixRatio,
102 std::ostream& msg) {
103 reset();
104 fixRatio = 0.0;
105
106 msg << string(epoTime) << " Ambiguity Resolution (BIE)" << endl;
107
108 // Resolvable ambiguities
109 // ----------------------
110 map<char, set<t_prn>> usedPrns;
111 for (char system : OPT->_ar._systems) {
112 for (const auto& par : params) {
113 if (par->type() == t_pppParam::amb && par->prn().system() == system) {
114 if (isResolvable(par)) {
115 addToGroup(par);
116 }
117 }
118 }
119 }
120
121 // Remove Groups with less than 2 ambiguities
122 // ------------------------------------------
123 auto iGrp = _ambGroups.begin();
124 while (iGrp != _ambGroups.end()) {
125 if (iGrp->_zdAmbs.size() < 2) {
126 _numZdAmbs -= iGrp->_zdAmbs.size();
127 iGrp = _ambGroups.erase(iGrp);
128 }
129 else {
130 ++iGrp;
131 }
132 }
133
134 // Check number of zero-difference ambiguities
135 // -------------------------------------------
136 bool hasEnoughAmbs = false;
137 for (const auto& ambGroup : _ambGroups) {
138 if (int(ambGroup._zdAmbs.size()) >= OPT->_ar._minNumSat) {
139 hasEnoughAmbs = true;
140 break;
141 }
142 }
143 if (!hasEnoughAmbs) {
144 msg << " not enough resolvable ambiguities" << endl;
145 return t_irc::failure;
146 }
147
148 // Construct the variance-covariance matrix of ambiguities
149 // -------------------------------------------------------
150 Matrix ZD(_numZdAmbs, params.size()); ZD = 0.0;
151 int idx = -1;
152 for (auto& ambGroup : _ambGroups) {
153 for (auto& zdAmb : ambGroup._zdAmbs) {
154 idx += 1;
155 zdAmb.setIndex(idx);
156 ZD[idx][zdAmb.indexGlobal()] = 1.0;
157 }
158 }
159 _xx = ZD * xFinal;
160 _QQ << ZD * QFinal * ZD.t();
161
162 // Select and Constrain Reference Ambiguities
163 // ------------------------------------------
164 Matrix HH(_ambGroups.size(), _numZdAmbs); HH = 0.0;
165 ColumnVector hh(_ambGroups.size());
166 for (unsigned iGrp = 0; iGrp < _ambGroups.size(); ++iGrp) {
167 AmbGroup& ambGroup = _ambGroups[iGrp];
168 selectReference(ambGroup);
169 HH[iGrp][ambGroup._zdAmbRef->index()] = 1.0;
170 hh[iGrp] = nint(_xx[ambGroup._zdAmbRef->index()]);
171 }
172 DiagonalMatrix PP(HH.nrows()); PP = 1.0 / (sigCon * sigCon);
173 kalman(HH, hh, PP, _QQ, _xx);
174
175 // Perform the Lambda (BIE) Search
176 // -------------------------------
177 ColumnVector xBie;
178 SymmetricMatrix covBie;
179 Lambda::search(_xx, _QQ, xBie, covBie);
180 if (xBie.Nrows() == 0) {
181 msg << " BIE search: failed" << endl;
182 return t_irc::failure;
183 }
184
185 // Print BIE Results
186 // -----------------
187 printBieResults(xBie, covBie, msg);
188
189 // Constrain ambiguities
190 // ---------------------
191 setConstraints(QFinal, xFinal, xBie, covBie, fixRatio, msg);
192
193 return t_irc::success;
194}
195
196// Print single-difference ambiguities and AR results
197////////////////////////////////////////////////////////////////////////////////////////////
198void AmbRes::printBieResults(const ColumnVector& xBie, const SymmetricMatrix& covBie,
199 ostream& msg) const {
200 msg.setf(ios::fixed);
201 for (const AmbGroup& ambGroup : _ambGroups) {
202 msg << " Group " << ambGroup._LC.toString() << endl;
203
204 for (const ZdAmb& zdAmb: ambGroup._zdAmbs) {
205 int idx = zdAmb.index();
206 msg << " " << zdAmb.prn().toString() << ' ' << setw(7) << setprecision(2) << _xx[idx];
207 if (&zdAmb == ambGroup._zdAmbRef) {
208 msg << " ref";
209 }
210 else {
211 msg << " +- ";
212 }
213 msg << setw(6) << setprecision(2) << sqrtMod(_QQ[idx][idx]) << ' '
214 << setw(7) << setprecision(2) << xBie[idx] << ' '
215 << setw(9) << setprecision(6) << sqrtMod(covBie[idx][idx]);
216 if (isFixable(xBie[idx], sqrtMod(covBie[idx][idx]))) {
217 msg << " fix ";
218 }
219 msg << endl;
220 }
221 }
222}
223
224//
225////////////////////////////////////////////////////////////////////////////////////////////
226void AmbRes::setConstraints(SymmetricMatrix& QFinal, ColumnVector& xFinal,
227 const ColumnVector& xBie, const SymmetricMatrix& covBie,
228 double& fixRatio, ostream& /*msg*/) {
229
230 fixRatio = 0.0;
231
232 vector<unique_ptr<const RowVector>> CC;
233 vector<double> cc;
234 vector<double> Sc;
235 int numSdAmbs = 0;
236 int numFixSdAll = 0;
237 for (const AmbGroup& ambGroup : _ambGroups) {
238 numSdAmbs += ambGroup._zdAmbs.size() - 1;
239 for (const ZdAmb& zdAmb : ambGroup._zdAmbs) {
240 int idx = zdAmb.index();
241 if (isFixable(xBie[idx], sqrtMod(covBie[idx][idx]))) {
242 if (ambGroup._zdAmbRef->indexGlobal() != zdAmb.indexGlobal()) {
243 numFixSdAll += 1;
244 }
245 RowVector* pCC;
246 CC.emplace_back(pCC = new RowVector(xFinal.Nrows()));
247 cc.push_back(xBie[idx]);
248 Sc.push_back(sigCon);
249 (*pCC) = 0.0;
250 (*pCC)[zdAmb.indexGlobal()] = 1.0;
251 }
252 }
253 }
254 if (CC.size() > 0) {
255 kalman(CC, cc, Sc, QFinal, xFinal);
256 }
257 if (numSdAmbs > 0) {
258 fixRatio = double(numFixSdAll) / double(numSdAmbs);
259 }
260}
Note: See TracBrowser for help on using the repository browser.