source: ntrip/trunk/BNC/src/PPP/lambda.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: 15.4 KB
Line 
1
2#include <iostream>
3#include <iomanip>
4#include <newmatio.h>
5#include <cmath>
6
7#include "lambda.h"
8
9using namespace BNC_PPP;
10using namespace std;
11
12// Gauss Error Function
13/////////////////////////////////////////////////////////////////////////////////////////
14double Lambda::erf(double xx) {
15 static const double a1 = 0.254829592;
16 static const double a2 = -0.284496736;
17 static const double a3 = 1.421413741;
18 static const double a4 = -1.453152027;
19 static const double a5 = 1.061405429;
20 static const double pp = 0.3275911;
21
22 int sign = (xx < 0) ? -1 : 1;
23 xx = fabs(xx);
24
25 double tt = 1.0/(1.0 + pp*xx);
26 double yy = 1.0 - (((((a5*tt + a4)*tt) + a3)*tt + a2)*tt + a1)*tt*exp(-xx*xx);
27
28 return sign * yy;
29}
30
31// Cumulative Distribution Function (Normal Distribution)
32/////////////////////////////////////////////////////////////////////////////////////////
33double Lambda::normcdf(double x, double mu, double sigma) {
34 static const double root_two = sqrt(2.0);
35 return 0.5 * ( 1.0 + erf( (x-mu) / (sigma*root_two) ) );
36}
37
38// Auxiliary functions
39/////////////////////////////////////////////////////////////////////////////////////////
40void Lambda::swap(double& a, double& b) {
41 double t(a); a = b; b = t;
42}
43double Lambda::sign(double a) {
44 if (a < 0.0) {
45 return -1.0;
46 }
47 else if (a > 0.0) {
48 return 1.0;
49 }
50 else {
51 return 0.0;
52 }
53}
54double Lambda::nint(double val) {
55 return ((val < 0.0) ? -floor(fabs(val)+0.5) : floor(val+0.5));
56}
57
58// LAMBDA/BIE Search
59/////////////////////////////////////////////////////////////////////////////////////////
60void Lambda::search(ColumnVector aFlt, const SymmetricMatrix& QQ,
61 ColumnVector& aFix, SymmetricMatrix& covBie) {
62
63 int nn = QQ.Nrows();
64
65 // Remove integer numbers from float solution (for computational convenience only)
66 // -------------------------------------------------------------------------------
67 ColumnVector incr(nn);
68 for (int ii = 0; ii < nn; ii++) {
69 incr[ii] = nint(aFlt[ii]);
70 aFlt[ii] = aFlt[ii] - incr[ii];
71 }
72
73 // Compute ZZ matrix based on the decomposition Q=LL^T*D*LL; The transformed
74 // float solution: zFlt = ZZ^T *aFlt, QzFlt = ZZ^T * QQ * ZZ
75 // -----------------------------------------------------------------------
76 SymmetricMatrix QzFlt;
77 Matrix ZZ;
78 LowerTriangularMatrix LL;
79 DiagonalMatrix DD;
80 ColumnVector zFlt;
81 Matrix iZt;
82 decorrel(QQ, aFlt, QzFlt, ZZ, LL, DD, zFlt, iZt);
83
84 // Perform the search
85 // ------------------
86 Info info;
87 BIE(zFlt, LL, DD, info);
88
89 // Perform the back-transformation and add the increments
90 // ------------------------------------------------------
91 aFix = iZt * info.zBie + incr;
92 info.zBie = ZZ.t() * aFix;
93 info.aFix = iZt * info.zFix;
94 for (int iCand = 1; iCand <= info.zFix.Ncols(); iCand++) {
95 info.aFix.column(iCand) += incr;
96 }
97 info.zFix = ZZ.t() * info.aFix;
98
99#ifdef LAMBDA_MAIN_TEST
100 cout.setf(ios::fixed);
101 cout << "aFix(3 cand)= \n" << setw(10) << setprecision(2) << info.aFix.columns(1,3) << endl;
102#endif
103
104 // Variances of ambiguities
105 // ------------------------
106 static const double sigCon = 1e-6;
107 DiagonalMatrix covZ(info.zFix.Nrows()); covZ = 0.0;
108 for (int ia = 0; ia < info.zFix.Nrows(); ia++) { // loop over all ambiguities
109 for (int ic = 0; ic < info.zFix.Ncols(); ic++) { // loop over all candidates
110 double dZ = info.zBie[ia] - info.zFix[ia][ic];
111 covZ[ia] += info.wgt[ic] * dZ * dZ;
112 }
113 if (covZ[ia] < sigCon*sigCon) { // make the matrix positive-definite
114 covZ[ia] = sigCon*sigCon;
115 }
116 }
117 Matrix invZ = ZZ.i();
118 covBie << invZ.t() * covZ * invZ;
119}
120
121// Best Integer Equivariant Estimator
122/////////////////////////////////////////////////////////////////////////////////////////
123void Lambda::BIE(const ColumnVector& zFlt, // Original ambiguities
124 const LowerTriangularMatrix& LL, // L matrix from L'DL-decomposition of QzFlt
125 const DiagonalMatrix& DD, // D matrix from L'DL-decomposition of QzFlt
126 Info& info) {
127
128 int nn = zFlt.Nrows();
129 int ncands = 100;
130 ColumnVector sqnorm;
131 info.wgt.ReSize(ncands); info.wgt = 0.0;
132 info.zBie.ReSize(nn); info.zBie = 0.0;
133
134 ssearch(zFlt, LL, DD, ncands, info.zFix, sqnorm);
135
136 LowerTriangularMatrix Li = LL.i();
137 SymmetricMatrix QzFltInv; QzFltInv << Li * DD.i() * Li.t();
138
139 info.zBie.ReSize(nn); info.zBie = 0.0;
140 double wgtSum = 0.0;
141 double norm1 = 0.0;
142 for (int ic = 1; ic <= ncands; ic++) {
143
144 ColumnVector da = zFlt - info.zFix.column(ic);
145 double daNorm = DotProduct(da, QzFltInv * da);
146
147 if (ic == 1) {
148 norm1 = daNorm;
149 info.wgt(ic) = 1.0;
150 }
151 else {
152 info.wgt(ic) = exp(-0.5 * (daNorm-norm1)); // weights scaled by exp(0.5 * norm1)
153 }
154
155 wgtSum += info.wgt(ic);
156
157 for (int ia = 1; ia <= info.zFix.Nrows(); ia++) {
158 info.zBie(ia) += info.wgt(ic) * info.zFix(ia,ic);
159 }
160 }
161
162 info.zBie /= wgtSum;
163 info.wgt /= wgtSum;
164}
165
166// Decomposition Q = L'DL (L is lower triangular)
167/////////////////////////////////////////////////////////////////////////////////////////
168void Lambda::ldldecom(const SymmetricMatrix& QQ, LowerTriangularMatrix& LL, DiagonalMatrix& DD) {
169
170 const int n = QQ.Nrows();
171
172 Matrix QC = QQ;
173
174 LL.ReSize(n); LL = 0.0;
175 DD.resize(n); DD = 0.0;
176
177 for (int i = n-1; i >= 0; i--) {
178 DD[i] = QC[i][i];
179 if ( DD[i] <= 0.0 ) {
180 throw "ldldecom problem";
181 }
182 double temp = sqrt(DD[i]);
183 for (int j = 0; j <= i; j++) {
184 LL[i][j] = QC[i][j]/temp;
185 }
186 for (int j = 0; j <= i-1; j++) {
187 for(int k = 0; k <= j; k++) {
188 QC[j][k] -= LL[i][k] * LL[i][j];
189 }
190 }
191 for (int j = 0; j <= i; j++) {
192 LL[i][j] /= LL[i][i];
193 }
194 }
195}
196
197//
198/////////////////////////////////////////////////////////////////////////////////////////
199void Lambda::decorrel(const SymmetricMatrix& QQ, // Variance-covariance matrix of ambiguities
200 const ColumnVector& aFlt, // Original ambiguities
201 SymmetricMatrix& QzFlt, // Cov. matrix of decorrelated ambiguities
202 Matrix& ZZ, // ZZ-transformation matrix
203 LowerTriangularMatrix& LL, // L matrix from L'DL-decomposition of QzFlt
204 DiagonalMatrix& DD, // D matrix from L'DL-decomposition of QzFlt
205 ColumnVector& zFlt, // Transformed ambiguities
206 Matrix& iZt) { // ZZ.t().i() transformation matrix
207
208 // L'DL Decomposition
209 // ------------------
210 ldldecom(QQ, LL, DD);
211
212 // Reduction
213 // ---------
214 int n = DD.Nrows();
215
216 iZt.ReSize(n,n); iZt = 0.0;
217 for (int i = 0; i < n; i++) {
218 iZt[i][i] = 1.0;
219 }
220
221 int i1 = n - 1;
222 bool sw = true;
223 while (sw) {
224
225 int i = n; // loop for column from n to 1
226 sw = false;
227
228 while ( !sw && i > 1) {
229
230 i = i - 1; // the ith column
231 if (i <= i1) {
232 for (int j = i+1; j <= n; j++) {
233 double mu = nint(LL(j,i));
234 if (mu != 0.0) {
235 for (int k = j; k <= n; k++) {
236 LL(k,i) = LL(k,i) - mu * LL(k,j);
237 }
238 for (int k = 1; k <= n; k++) {
239 iZt(k,j) = iZt(k,j) + mu * iZt(k,i);
240 }
241 }
242 }
243 }
244
245 double delta = DD(i) + LL(i+1,i) * LL(i+1,i) * DD(i+1);
246 if (delta < DD(i+1)) {
247 double lambda = DD(i+1) * LL(i+1,i) / delta;
248 double eta = DD(i) / delta;
249 DD(i) = eta * DD(i+1);
250 DD(i+1) = delta;
251
252 Matrix hlp(2,2); hlp << -LL(i+1,i) << 1.0
253 << eta << lambda;
254
255 LL.submatrix(i,i+1,1,i-1) = hlp * LL.submatrix(i,i+1,1,i-1);
256
257 LL(i+1,i) = lambda;
258
259 for (int k = i+2; k <= n; k++) swap( LL(k,i), LL(k,i+1));
260 for (int k = 1; k <= n; k++) swap(iZt(k,i), iZt(k,i+1));
261 i1 = i;
262 sw = true;
263 }
264 }
265 }
266
267 // Transformed Q-matrix, transformation-matrix, and decorrelated ambiguities
268 // -------------------------------------------------------------------------
269 ZZ = iZt.i().t();
270 for (int i = 0; i < ZZ.Nrows(); i++) {
271 for (int j = 0; j < ZZ.Nrows(); j++) {
272 ZZ[i][j] = nint(ZZ[i][j]);
273 }
274 }
275
276 QzFlt << ZZ.t() * QQ * ZZ; // it is also L'DL
277 zFlt = ZZ.t() * aFlt;
278}
279
280// Integer ambiguity vector search by employing the search-and-shrink technique
281/////////////////////////////////////////////////////////////////////////////////////////
282void Lambda::ssearch(const ColumnVector& zFlt, // Original ambiguities
283 const LowerTriangularMatrix& LL, // L matrix from L'DL-decomposition of QzFlt
284 const DiagonalMatrix& DD, // D matrix from L'DL-decomposition of QzFlt
285 int ncands, // Number of requested candidates
286 Matrix& zFix, // estimated integers (n x ncands )
287 ColumnVector& sqnorm) { // squared norms (ascendantly sorted)
288
289 // Initialize outputs
290 // ------------------
291 int n = zFlt.Nrows();
292 zFix.ReSize(n, ncands); zFix = 0.0;
293 sqnorm.ReSize(ncands); sqnorm = 0.0;
294
295 // Initializing the variables for searching
296 // ----------------------------------------
297 double Chi2 = 1.0e+18; // start search with an infinite chi^2
298 ColumnVector dist(n); dist(n) = 0.0; // dist(k)=sum_{j=k+1}^{n}(a_j-acond_j)^2/d_j
299 bool endsearch = false;
300 int count = 0; // the number of candidates
301
302 ColumnVector acond(n); acond(n) = zFlt(n);
303 ColumnVector zcond(n); zcond(n) = nint(acond(n));
304 double left = acond(n) - zcond(n);
305 ColumnVector step(n); step(n) = sign(left);
306
307 // For a very occasional case when the value of float solution zFlt(n) == 0, we
308 // compusively give a positive step to continue.
309 if (step(n) == 0.0) {
310 step(n) = 1;
311 }
312
313 int imax = ncands; // initially, the maximum F(z) is at ncands
314 Matrix SS(n, n); SS = 0.0; // used to compute conditional ambiguities
315
316 int k = n;
317
318 // Start the main search-loop
319 // --------------------------
320 while (!endsearch) {
321 double newdist = dist(k) + left*left / DD(k);
322 if (newdist < Chi2) {
323 if (k != 1) { // Case 1: move down
324 k = k - 1;
325 dist(k) = newdist;
326 for (int j = 1; j <= k; j++) {
327 SS(k,j) = SS(k+1,j) + (zcond(k+1)-acond(k+1)) * LL(k+1,j);
328 }
329
330 acond(k) = zFlt(k) + SS(k, k);
331 zcond(k) = round(acond(k));
332 left = acond(k) - zcond(k);
333 step(k) = sign(left);
334
335 if (step(k) == 0) {
336 step(k) = 1.0;
337 }
338 }
339 else { // Case 2: store the found candidate and try next valid integer
340 if (count < ncands - 1) {
341 count = count + 1;
342 zFix.column(count) = zcond;
343 sqnorm(count) = newdist;
344 }
345 else {
346 zFix.column(imax) = zcond;
347 sqnorm(imax) = newdist;
348 Chi2 = sqnorm.maximum1(imax);
349 }
350 zcond(1) = zcond(1) + step(1); // next valid integer
351 left = acond(1) - zcond(1);
352 step(1) = -step(1) - sign(step(1));
353 }
354 }
355 else { // Case 3: exit or move up
356 if (k == n) {
357 endsearch = true;
358 }
359 else {
360 k = k + 1; // move up
361 zcond(k) = zcond(k) + step(k); // next valid integer
362 left = acond(k) - zcond(k);
363 step(k) = -step(k) - sign(step(k));
364 }
365 }
366 }
367
368 // Sort
369 // ----
370 for (int i = 0; i < ncands-1; i++) {
371 for (int j = i+1; j < ncands; j++) {
372 if (sqnorm[i] > sqnorm[j]) {
373 swap(sqnorm[i],sqnorm[j]);
374 for (k = 0; k < n; k++) {
375 swap(zFix[k][i], zFix[k][j]);
376 }
377 }
378 }
379 }
380}
381
382#ifdef LAMBDA_MAIN_TEST
383// Main test program
384// compile: g++ -DLAMBDA_MAIN_TEST -I../../newmat ../../newmat/*.cpp arLambda.cpp
385/////////////////////////////////////////////////////////////////////////////////////////
386int main(int argc, char* argv[]) {
387
388 const int nn = 12;
389
390 SymmetricMatrix QQ(nn);
391 QQ << 1.90688560e+04
392 << -1.57839723e+04 << 5.90277038e+04
393 << -1.73342006e+04 << 3.81426928e+04 << 2.81775654e+04
394 << 1.44119240e+04 << 5.62717388e+02 << -7.00050220e+03 << 1.56055082e+04
395 << 1.00557170e+04 << -1.38300856e+04 << -1.16958674e+04 << 5.03970282e+03 << 6.82077251e+03
396 << -1.42592953e+04 << 2.73734263e+04 << 2.18861681e+04 << -9.64896531e+03 << -6.88024051e+03 << 2.32465490e+04
397 << 1.48588484e+04 << -1.22991994e+04 << -1.35071695e+04 << 1.12300704e+04 << 7.83562345e+03 << -1.11111394e+04 << 1.15783237e+04
398 << -1.22991994e+04 << 4.59956130e+04 << 2.97215786e+04 << 4.38480888e+02 << -1.07766903e+04 << 2.13299424e+04 << -9.58379157e+03 << 3.58407377e+04
399 << -1.35071695e+04 << 2.97215786e+04 << 2.19565441e+04 << -5.45493698e+03 << -9.11366311e+03 << 1.70541567e+04 << -1.05250670e+04 << 2.31596718e+04 << 1.71089957e+04
400 << 1.12300704e+04 << 4.38480887e+02 << -5.45493698e+03 << 1.21601359e+04 << 3.92704096e+03 << -7.51867446e+03 << 8.75070439e+03 << 3.41673570e+02 << -4.25060009e+03 << 9.47543087e+03
401 << 7.83562345e+03 << -1.07766903e+04 << -9.11366311e+03 << 3.92704096e+03 << 5.31488728e+03 << -5.36122657e+03 << 6.10568076e+03 << -8.39742084e+03 << -7.10155552e+03 << 3.06003207e+03 << 4.14147091e+03
402 << -1.11111394e+04 << 2.13299424e+04 << 1.70541567e+04 << -7.51867446e+03 << -5.36122657e+03 << 1.81141936e+04 << -8.65803054e+03 << 1.66207345e+04 << 1.32889535e+04 << -5.85870722e+03 << -4.17757899e+03 << 1.41149564e+04;
403
404 ColumnVector aa(nn);
405 aa << -2.84908567e+04
406 << 6.57526299e+04
407 << 3.88303667e+04
408 << 5.00370834e+03
409 << -2.91960699e+04
410 << -2.97658932e+02
411 << -2.22010284e+04
412 << 5.12358375e+04
413 << 3.02577810e+04
414 << 3.89940332e+03
415 << -2.27491854e+04
416 << -1.59278780e+02;
417
418 ColumnVector aBie;
419 SymmetricMatrix covBie;
420
421 Lambda::search(aa, QQ, aBie, covBie);
422
423 cout.setf(ios::fixed);
424 cout << "aFlt = \n" << setw(10) << setprecision(2) << aa << endl;
425 cout << "aBie = \n" << setw(10) << setprecision(2) << aBie << endl;
426 cout << "covBie = \n" << setw( 7) << setprecision(2) << covBie << endl;
427
428 return 0;
429}
430#endif
Note: See TracBrowser for help on using the repository browser.