Changeset 3383 in ntrip for trunk/BNC/bncmodel.cpp
- Timestamp:
- Aug 30, 2011, 4:56:38 PM (13 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/BNC/bncmodel.cpp
r3382 r3383 56 56 using namespace std; 57 57 58 const unsigned MINOBS = 5; 59 const double MINELE_GPS = 10.0 * M_PI / 180.0; 60 const double MINELE_GLO = 10.0 * M_PI / 180.0; 61 const double MINELE_GAL = 10.0 * M_PI / 180.0; 62 const double MAXRES_CODE_GPS = 10.0; 63 const double MAXRES_PHASE_GPS = 0.04; 64 const double MAXRES_PHASE_GLO = 0.04; 65 const double MAXRES_CODE_GAL = 10.0; 66 const double MAXRES_PHASE_GAL = 0.04; 58 const unsigned MINOBS = 5; 59 const double MINELE = 10.0 * M_PI / 180.0; 60 const double MAXRES_CODE = 10.0; 61 const double MAXRES_PHASE = 0.04; 67 62 68 63 // Constructor … … 327 322 Tracer tracer("bncModel::cmpBancroft"); 328 323 329 if (epoData->size GPS() < MINOBS) {324 if (epoData->sizeSys('G') < MINOBS) { 330 325 _log += "bncModel::cmpBancroft: not enough data\n"; 331 326 return failure; 332 327 } 333 328 334 Matrix BB(epoData->size GPS(), 4);335 336 QMapIterator<QString, t_satData*> it(epoData->satData GPS);329 Matrix BB(epoData->sizeSys('G'), 4); 330 331 QMapIterator<QString, t_satData*> it(epoData->satData); 337 332 int iObsBanc = 0; 338 333 while (it.hasNext()) { 339 ++iObsBanc;340 334 it.next(); 341 QString prn = it.key();342 335 t_satData* satData = it.value(); 343 BB(iObsBanc, 1) = satData->xx(1); 344 BB(iObsBanc, 2) = satData->xx(2); 345 BB(iObsBanc, 3) = satData->xx(3); 346 BB(iObsBanc, 4) = satData->P3 + satData->clk; 336 if (satData->system() == 'G') { 337 ++iObsBanc; 338 QString prn = it.key(); 339 BB(iObsBanc, 1) = satData->xx(1); 340 BB(iObsBanc, 2) = satData->xx(2); 341 BB(iObsBanc, 3) = satData->xx(3); 342 BB(iObsBanc, 4) = satData->P3 + satData->clk; 343 } 347 344 } 348 345 … … 355 352 // Compute Satellite Elevations 356 353 // ---------------------------- 357 QMutableMapIterator<QString, t_satData*> i GPS(epoData->satDataGPS);358 while (i GPS.hasNext()) {359 i GPS.next();360 t_satData* satData = i GPS.value();354 QMutableMapIterator<QString, t_satData*> im(epoData->satData); 355 while (im.hasNext()) { 356 im.next(); 357 t_satData* satData = im.value(); 361 358 cmpEle(satData); 362 if (satData->eleSat < MINELE _GPS) {359 if (satData->eleSat < MINELE) { 363 360 delete satData; 364 iGPS.remove(); 365 } 366 } 367 368 QMutableMapIterator<QString, t_satData*> iGlo(epoData->satDataGlo); 369 while (iGlo.hasNext()) { 370 iGlo.next(); 371 t_satData* satData = iGlo.value(); 372 cmpEle(satData); 373 if (satData->eleSat < MINELE_GLO) { 374 delete satData; 375 iGlo.remove(); 376 } 377 } 378 379 QMutableMapIterator<QString, t_satData*> iGal(epoData->satDataGal); 380 while (iGal.hasNext()) { 381 iGal.next(); 382 t_satData* satData = iGal.value(); 383 cmpEle(satData); 384 if (satData->eleSat < MINELE_GAL) { 385 delete satData; 386 iGal.remove(); 361 im.remove(); 387 362 } 388 363 } … … 605 580 // ------------------------------------------------ 606 581 int iPar = 0; 607 QMutableVectorIterator<bncParam*> i t(_params);608 while (i t.hasNext()) {609 bncParam* par = i t.next();582 QMutableVectorIterator<bncParam*> im(_params); 583 while (im.hasNext()) { 584 bncParam* par = im.next(); 610 585 bool removed = false; 611 586 if (par->type == bncParam::AMB_L3) { 612 if (epoData->satDataGPS.find(par->prn) == epoData->satDataGPS.end() && 613 epoData->satDataGlo.find(par->prn) == epoData->satDataGlo.end() && 614 epoData->satDataGal.find(par->prn) == epoData->satDataGal.end() ) { 587 if (epoData->satData.find(par->prn) == epoData->satData.end()) { 615 588 removed = true; 616 589 delete par; 617 i t.remove();590 im.remove(); 618 591 } 619 592 } … … 626 599 // Add new ambiguity parameters 627 600 // ---------------------------- 628 QMapIterator<QString, t_satData*> iGPS(epoData->satDataGPS); 629 while (iGPS.hasNext()) { 630 iGPS.next(); 631 t_satData* satData = iGPS.value(); 632 addAmb(satData); 633 } 634 635 QMapIterator<QString, t_satData*> iGlo(epoData->satDataGlo); 636 while (iGlo.hasNext()) { 637 iGlo.next(); 638 t_satData* satData = iGlo.value(); 639 addAmb(satData); 640 } 641 642 QMapIterator<QString, t_satData*> iGal(epoData->satDataGal); 643 while (iGal.hasNext()) { 644 iGal.next(); 645 t_satData* satData = iGal.value(); 601 QMapIterator<QString, t_satData*> it(epoData->satData); 602 while (it.hasNext()) { 603 it.next(); 604 t_satData* satData = it.value(); 646 605 addAmb(satData); 647 606 } … … 943 902 //////////////////////////////////////////////////////////////////////////// 944 903 bool bncModel::outlierDetection(int iPhase, const ColumnVector& vv, 945 QMap<QString, t_satData*>& satDataGPS, 946 QMap<QString, t_satData*>& satDataGlo, 947 QMap<QString, t_satData*>& satDataGal) { 904 QMap<QString, t_satData*>& satData) { 948 905 949 906 Tracer tracer("bncModel::outlierDetection"); … … 959 916 bool irc = false; 960 917 918 // Check Code 919 // ---------- 961 920 if (iPhase == 0) { 962 963 // Check GPS Code 964 // -------------- 965 if (!irc) { 966 findMaxRes(iPhase, vv,satDataGPS, prnCode, maxResCode, prnPhase, maxResPhase); 967 if (maxResCode > MAXRES_CODE_GPS) { 968 prnRemoved = prnCode; 969 maxRes = maxResCode; 970 irc = true; 971 } 972 } 973 974 // Check Galileo Code 975 // ------------------ 976 if (!irc) { 977 findMaxRes(iPhase, vv,satDataGal, prnCode, maxResCode, prnPhase, maxResPhase); 978 if (maxResCode > MAXRES_CODE_GAL) { 979 prnRemoved = prnCode; 980 maxRes = maxResCode; 981 irc = true; 982 } 983 } 984 } 985 921 findMaxRes(iPhase, vv, satData, prnCode, maxResCode, prnPhase, maxResPhase); 922 if (maxResCode > MAXRES_CODE) { 923 prnRemoved = prnCode; 924 maxRes = maxResCode; 925 irc = true; 926 } 927 } 928 929 // Check Phase 930 // ----------- 986 931 else { 987 988 // Check Glonass Phase 989 // ------------------- 990 if (!irc) { 991 findMaxRes(iPhase, vv,satDataGlo, prnCode, maxResCode, prnPhase, maxResPhase); 992 if (maxResPhase > MAXRES_PHASE_GLO) { 993 prnRemoved = prnPhase; 994 maxRes = maxResPhase; 995 irc = true; 996 } 997 } 998 999 // Check Galileo Phase 1000 // ------------------- 1001 if (!irc) { 1002 findMaxRes(iPhase, vv,satDataGal, prnCode, maxResCode, prnPhase, maxResPhase); 1003 if (maxResPhase > MAXRES_PHASE_GAL) { 1004 prnRemoved = prnPhase; 1005 maxRes = maxResPhase; 1006 irc = true; 1007 } 1008 } 1009 1010 // Check GPS Phase 1011 // --------------- 1012 if (!irc) { 1013 findMaxRes(iPhase, vv,satDataGPS, prnCode, maxResCode, prnPhase, maxResPhase); 1014 if (maxResPhase > MAXRES_PHASE_GPS) { 1015 prnRemoved = prnPhase; 1016 maxRes = maxResPhase; 1017 irc = true; 1018 } 932 findMaxRes(iPhase, vv, satData, prnCode, maxResCode, prnPhase, maxResPhase); 933 if (maxResPhase > MAXRES_PHASE) { 934 prnRemoved = prnPhase; 935 maxRes = maxResPhase; 936 irc = true; 1019 937 } 1020 938 } … … 1325 1243 1326 1244 std::vector<QString> allPrns; 1327 getAllPrns(epoData, &allPrns); 1245 QMapIterator<QString, t_satData*> it(epoData->satData); 1246 while (it.hasNext()) { 1247 it.next(); 1248 t_satData* satData = it.value(); 1249 allPrns.push_back(satData->prn); 1250 } 1328 1251 1329 1252 std::vector<QString> usedPrns; … … 1355 1278 QString prn = allPrns[ip]; 1356 1279 if ( !findInVector(usedPrns, prn) ) { 1357 epoData->satDataGPS.remove(prn); 1358 epoData->satDataGlo.remove(prn); 1359 epoData->satDataGal.remove(prn); 1280 epoData->satData.remove(prn); 1360 1281 } 1361 1282 } … … 1379 1300 } 1380 1301 else { 1381 if (epoData->size GPS() < MINOBS) {1302 if (epoData->sizeSys('G') < MINOBS) { 1382 1303 restoreState(epoData); 1383 1304 _log += "bncModel::update_p: not enough data\n"; … … 1410 1331 unsigned nObs = 0; 1411 1332 if (iPhase == 0) { 1412 nObs = epoData->size GPS() + epoData->sizeGal(); // Glonass code not used1333 nObs = epoData->sizeAll() - epoData->sizeSys('R'); // Glonass code not used 1413 1334 } 1414 1335 else { 1415 nObs = epoData->size GPS() + epoData->sizeGal() + epoData->sizeGlo();1336 nObs = epoData->sizeAll(); 1416 1337 } 1417 1338 … … 1424 1345 // GPS 1425 1346 // --- 1426 QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS); 1427 while (itGPS.hasNext()) { 1428 itGPS.next(); 1429 t_satData* satData = itGPS.value(); 1430 QString prn = satData->prn; 1431 if (findInVector(usedPrns, satData->prn)) { 1432 addObs(iPhase, iObs, satData, AA, ll, PP); 1433 } 1434 } 1435 1436 // Glonass 1437 // ------- 1438 if (iPhase == 1) { 1439 QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo); 1440 while (itGlo.hasNext()) { 1441 itGlo.next(); 1442 t_satData* satData = itGlo.value(); 1347 QMapIterator<QString, t_satData*> it(epoData->satData); 1348 while (it.hasNext()) { 1349 it.next(); 1350 t_satData* satData = it.value(); 1351 if (iPhase == 1 || satData->system() != 'R') { 1352 QString prn = satData->prn; 1443 1353 if (findInVector(usedPrns, satData->prn)) { 1444 1354 addObs(iPhase, iObs, satData, AA, ll, PP); … … 1446 1356 } 1447 1357 } 1448 1449 // Galileo 1450 // ------- 1451 QMapIterator<QString, t_satData*> itGal(epoData->satDataGal); 1452 while (itGal.hasNext()) { 1453 itGal.next(); 1454 t_satData* satData = itGal.value(); 1455 if (findInVector(usedPrns, satData->prn)) { 1456 addObs(iPhase, iObs, satData, AA, ll, PP); 1457 } 1458 } 1459 1358 1460 1359 // Compute Filter Update 1461 1360 // --------------------- … … 1470 1369 str.setf(ios::fixed); 1471 1370 1472 QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS); 1473 while (itGPS.hasNext()) { 1474 itGPS.next(); 1475 t_satData* satData = itGPS.value(); 1476 printRes(iPhase, vv, str, satData); 1477 } 1478 if (iPhase == 1) { 1479 QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo); 1480 while (itGlo.hasNext()) { 1481 itGlo.next(); 1482 t_satData* satData = itGlo.value(); 1371 QMapIterator<QString, t_satData*> it(epoData->satData); 1372 while (it.hasNext()) { 1373 it.next(); 1374 t_satData* satData = it.value(); 1375 if (iPhase == 1 || satData->system() != 'R') { 1483 1376 printRes(iPhase, vv, str, satData); 1484 1377 } 1485 1378 } 1486 QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);1487 while (itGal.hasNext()) {1488 itGal.next();1489 t_satData* satData = itGal.value();1490 printRes(iPhase, vv, str, satData);1491 }1492 1379 _log += str.str().c_str(); 1493 1380 } … … 1495 1382 // Check the residuals 1496 1383 // ------------------- 1497 outlierDetected = outlierDetection(iPhase, vv, 1498 epoData->satDataGPS, 1499 epoData->satDataGlo, 1500 epoData->satDataGal); 1384 outlierDetected = outlierDetection(iPhase, vv, epoData->satData); 1385 1501 1386 if (outlierDetected) { 1502 1387 restoreState(epoData); … … 1568 1453 epoData->deepCopy(_epoData_sav); 1569 1454 } 1570 1571 //1572 ////////////////////////////////////////////////////////////////////////////1573 void bncModel::getAllPrns(const t_epoData* epoData,1574 std::vector<QString>* allPrns) {1575 1576 // GPS1577 // ---1578 QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);1579 while (itGPS.hasNext()) {1580 itGPS.next();1581 t_satData* satData = itGPS.value();1582 allPrns->push_back(satData->prn);1583 }1584 1585 // Glonass1586 // -------1587 QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);1588 while (itGlo.hasNext()) {1589 itGlo.next();1590 t_satData* satData = itGlo.value();1591 allPrns->push_back(satData->prn);1592 }1593 1594 // Galileo1595 // -------1596 QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);1597 while (itGal.hasNext()) {1598 itGal.next();1599 t_satData* satData = itGal.value();1600 allPrns->push_back(satData->prn);1601 }1602 }1603
Note:
See TracChangeset
for help on using the changeset viewer.