62 using namespace StringUtils;
67 const unsigned short SatPass::OK = 1;
68 const unsigned short SatPass::BAD = 0;
69 const unsigned short SatPass::LL1 = 2;
70 const unsigned short SatPass::LL2 = 4;
71 const unsigned short SatPass::LL3 = 6;
72 double SatPass::maxGap = 1800;
73 int SatPass::outRound = 3;
74 string SatPass::outFormat =
76 string SatPass::longfmt =
77 string(
"%04Y/%02m/%02d %02H:%02M:%06.3f = %04F %10.3g");
82 vector<string> defaultObsTypes;
83 defaultObsTypes.push_back(
"L1");
84 defaultObsTypes.push_back(
"L2");
85 defaultObsTypes.push_back(
"P1");
86 defaultObsTypes.push_back(
"P2");
88 init(insat, indt, defaultObsTypes);
92 std::vector<std::string> obstypes)
94 init(insat, indt, obstypes);
98 std::vector<std::string> obstypes)
107 for (
int i = 0; i < obstypes.size(); i++)
109 indexForLabel[obstypes[i]] = i;
110 labelForIndex[i] = obstypes[i];
126 spdvector.resize(right.
spdvector.size());
127 for (
int i = 0; i < right.
spdvector.size(); i++)
134 int SatPass::addData(
const Epoch& tt, std::vector<std::string>& ots,
135 std::vector<double>&
data)
151 int SatPass::addData(
const Epoch& tt,
152 const std::vector<std::string>& obstypes,
153 const std::vector<double>&
data,
154 const std::vector<unsigned short>&
lli,
155 const std::vector<unsigned short>&
ssi,
156 const unsigned short flag)
161 Exception e(
"Dimensions do not match in addData()" +
167 if (spdvector.size() > 0 && spdvector[0].data.size() !=
data.size())
170 "Error - addData passed different dimension that earlier!" +
179 for (
int k = 0; k <
data.size(); k++)
181 int i = indexForLabel[obstypes[k]];
189 return pushBack(tt, spd);
204 RinexObsData::RinexSatMap::const_iterator it;
205 RinexObsData::RinexObsTypeMap::const_iterator jt;
206 map<string, unsigned int>::const_iterator kt;
210 for (it = robs.
obs.begin(); it != robs.
obs.end(); it++)
212 if (it->first == sat)
216 for (kt = indexForLabel.begin(); kt != indexForLabel.end(); kt++)
218 if ((jt = it->second.find(RinexObsHeader::convertObsType(
219 kt->first))) == it->second.end())
221 spd.
data[kt->second] = 0.0;
222 spd.
lli[kt->second] = 0;
223 spd.
ssi[kt->second] = 0;
229 spd.
data[kt->second] = jt->second.data;
230 spd.
lli[kt->second] = jt->second.lli;
231 spd.
ssi[kt->second] = jt->second.ssi;
232 if (jt->second.data == 0.0)
239 return pushBack(robs.
time, spd);
249 int SatPass::trimAfter(
const Epoch ttag)
253 if (ttag <= firstTime)
257 if (ttag >= lastTime)
263 int count = countForTime(ttag);
270 unsigned int i, j, n(0);
271 for (i = 0; i < spdvector.size(); i++)
273 if (spdvector[i].ndt >=
static_cast<unsigned int>(count))
278 if (spdvector[i].flag != SatPass::BAD)
285 spdvector.resize(j + 1);
309 bool SatPass::getGLOchannel(
int& n, std::string& msg)
313 if (sat.system != SatelliteSystem::Glonass)
320 map<string, unsigned int>::const_iterator it;
321 if (indexForLabel.find(
"L1") == indexForLabel.end() ||
322 indexForLabel.find(
"L2") == indexForLabel.end() ||
323 (indexForLabel.find(
"C1") == indexForLabel.end() &&
324 indexForLabel.find(
"P1") == indexForLabel.end()) ||
325 (indexForLabel.find(
"P2") == indexForLabel.end() &&
326 indexForLabel.find(
"C2") == indexForLabel.end()))
329 "Obs types L1 L2 C1/P1 C2/P2 required for GLOchannel()");
332 if (indexForLabel.find(
"P1") == indexForLabel.end())
346 static const double alpha =
347 ((9. / 7.) * (9. / 7.) - 1.0);
348 static const double D11 = (alpha + 2.) / alpha;
349 static const double D12 = -2. / alpha;
350 static const double D21 = (2 * alpha + 2.) / alpha;
351 static const double D22 = -D11;
353 bool first, done, ok;
354 int i, dn, di, sign(0);
355 const int N(spdvector.size());
356 double pP1, pP2, pL1, pL2, pRB1, pRB2;
358 static const double testStdDev(40.0), testSlope(0.1), testRatio(10.0),
367 di = (N > 50 ? N / 50 : 1);
376 double wl1(
C_MPS / (1602.0e6 + (n + dn) * 562.5e3));
377 double wl2(
C_MPS / (1246.0e6 + (n + dn) * 437.5e3));
381 for (i = 0; i < N; i += di)
383 if (!(spdvector[i].flag &
OK))
389 spdvector[i].data[indexForLabel[(useC1 ?
"C1" :
"P1")]];
390 double P2 = spdvector[i].data[indexForLabel[
"P2"]];
391 double L1 = spdvector[i].data[indexForLabel[
"L1"]];
392 double L2 = spdvector[i].data[indexForLabel[
"L2"]];
393 double RB1 =
wl1 *
L1 - D11 *
P1 - D12 *
P2;
394 double RB2 =
wl2 *
L2 - D21 *
P1 - D22 *
P2;
397 (::fabs(RB1 - pRB1) > 2000. || ::fabs(RB2 - pRB2) > 2000. ||
398 ::fabs(
L1 - pL1) / 2848. > 1000. ||
399 ::fabs(
L2 - pL2) / 2848. > 1000.))
407 dN1.
Add((-
L1 + pL1) / 2848., RB1 - pRB1);
408 dN2.
Add((-
L2 + pL2) / 2848., RB2 - pRB2);
411 <<
"GLODMP " << sat <<
" " << setw(2) << n + dn <<
" "
413 << setprecision(2) <<
" " << setw(9) << RB1 - pRB1 <<
" "
414 << setw(9) << -(
L1 - pL1) / 2848. <<
" " << setw(4)
415 << dN1.
N() <<
" " << setw(9) << dN1.
StdDevY() <<
" "
416 << setw(9) << (dN1.
N() > 1 ? dN1.
Slope() : 0.0) <<
" "
417 << setw(9) << (dN1.
N() > 1 ? dN1.
SigmaSlope() : 0.0) <<
" "
418 << setw(9) << RB2 - pRB2 <<
" " << setw(9)
419 << -(
L2 - pL2) / 2848. <<
" " << setw(4) << dN2.
N() <<
" "
420 << setw(9) << dN2.
StdDevY() <<
" " << setw(9)
421 << (dN1.
N() > 1 ? dN2.
Slope() : 0.0) <<
" " << setw(9)
449 if (dN1.
StdDevY() < testStdDev && ::fabs(dN1.
Slope()) < testSlope &&
459 dnSeen.push_back(dn);
463 -int((dN1.
Slope() < 0 ? -0.5 : 0.5) + dN1.
Slope() / 0.1877);
464 if (::abs(dm) > 5 || n + dn + dm < -7 || n + dn + dm > 7 ||
481 if (n + dn > 7 || n + dn < -7)
483 msg = string(
"out of range : n+dn=") +
asString(n + dn);
488 msg = string(
"failed to converge : n+dn=") +
asString(n + dn);
493 LOG(
DEBUG) <<
"GETGLO " << setw(2) << n + m <<
" PRELIM " << sat
494 << fixed << setprecision(2) <<
" " << setw(2) << dN1.
N()
495 <<
" " << setw(9) << dN1.
StdDevY() <<
" ("
496 << setprecision(0) << testStdDev <<
")"
497 << setprecision(2) <<
" " << setprecision(3) << setw(10)
498 << dN1.
Slope() <<
" (" << testSlope <<
")"
499 << setprecision(2) <<
" " << setw(9)
501 << setprecision(0) << testRatio <<
")" << setprecision(2)
508 <<
" " << (done ?
"DONE" :
"NOPE");
523 oss <<
"FINAL" << fixed << setprecision(6) <<
" " << sat <<
" "
524 << setw(2) << n + dn <<
" "
525 <<
printTime(getFirstGoodTime(), outFormat) <<
" "
526 <<
printTime(getLastGoodTime(), outFormat) << fixed
527 << setprecision(4) <<
" " << setw(2) << dN1.
N() <<
" " << setw(8)
528 << dN1.
StdDevY() <<
" " << setw(8) << dN1.
Slope() <<
" " << setw(8)
553 void SatPass::smoothSF(
bool smoothPR,
bool debiasPH,
554 std::string& msg,
const int freq,
double wl)
558 if (freq != 1 && freq != 2)
565 if (freq == 1 && !hasType(
"L1"))
569 if (freq == 2 && !hasType(
"L2"))
573 if (freq == 1 && !hasType(
"C1") && !hasType(
"P1"))
577 if (freq == 2 && !hasType(
"C2") && !hasType(
"P2"))
581 if (!oss.str().empty())
584 string(
"smoothSF() requires pseudorange and phase:" + oss.str() +
585 string(
" are missing."))));
588 bool first, useC1(hasType(
"C1")), useC2(hasType(
"C2"));
590 double RB, dLB0(0.0);
595 for (first =
true, i = 0; i < spdvector.size(); i++)
597 if (!(spdvector[i].flag &
OK))
605 P = spdvector[i].data[indexForLabel[(useC1 ?
"C1" :
"P1")]];
609 P = spdvector[i].data[indexForLabel[(useC2 ?
"C2" :
"P2")]];
613 L = spdvector[i].data[indexForLabel[
"L1"]];
617 L = spdvector[i].data[indexForLabel[
"L2"]];
622 LB0 = long(L -
P / wl);
633 LOG(
DEBUG) <<
"SMTDMP sat week sow RmP L P RB LB0";
635 LOG(
DEBUG) <<
"SMTDMP " << sat <<
" " <<
time(i).printf(outFormat)
636 << fixed << setprecision(3) <<
" " << setw(13) << RB
637 <<
" " << setw(13) << L <<
" " << setw(13) <<
P <<
" "
638 << setw(13) << RB <<
" " << setw(13) << LB0;
644 LB = LB0 + long(RB + (RB > 0 ? 0.5 : -0.5));
647 oss <<
"SMT" << fixed << setprecision(2) <<
" " << sat <<
" "
648 << getFirstGoodTime().printf(outFormat) <<
" "
649 << getLastGoodTime().printf(outFormat) <<
" " << setw(5)
650 << (freq == 1 ? PB.
N() : 0) <<
" " << setw(12)
651 << (freq == 1 ? PB.
Average() + dLB0 : 0) <<
" " << setw(5)
652 << (freq == 1 ? PB.
StdDev() : 0) <<
" " << setw(12)
653 << (freq == 1 ? PB.
Minimum() + dLB0 : 0) <<
" " << setw(12)
654 << (freq == 1 ? PB.
Maximum() + dLB0 : 0) <<
" " << setw(5)
655 << (freq == 1 ? 0 : PB.
N()) <<
" " << setw(12)
656 << (freq == 1 ? 0 : PB.
Average() + dLB0) <<
" " << setw(5)
657 << (freq == 1 ? 0 : PB.
StdDev()) <<
" " << setw(12)
658 << (freq == 1 ? 0 : PB.
Minimum() + dLB0) <<
" " << setw(12)
659 << (freq == 1 ? 0 : PB.
Maximum() + dLB0) <<
" " << setw(13)
660 << (freq == 1 ? RB : 0) <<
" " << setw(13) << (freq == 1 ? 0 : RB)
661 <<
" " << setw(10) << (freq == 1 ? LB : 0) <<
" " << setw(10)
662 << (freq == 1 ? 0 : LB);
665 if (!debiasPH && !smoothPR)
670 for (i = 0; i < spdvector.size(); i++)
672 if (!(spdvector[i].flag &
OK))
683 spdvector[i].data[indexForLabel[(useC1 ?
"C1" :
"P1")]] =
684 spdvector[i].
data[indexForLabel[
"L1"]] - RB;
688 spdvector[i].data[indexForLabel[(useC2 ?
"C2" :
"P2")]] =
689 spdvector[i].
data[indexForLabel[
"L2"]] - RB;
699 spdvector[i].data[indexForLabel[
"L1"]] -= LB;
703 spdvector[i].data[indexForLabel[
"L2"]] -= LB;
717 void SatPass::smooth(
bool smoothPR,
bool debiasPH,
718 std::string& msg,
const double&
wl1,
const double&
wl2)
733 if (!hasType(
"C1") && !hasType(
"P1"))
737 if (!hasType(
"C2") && !hasType(
"P2"))
741 if (!oss.str().empty())
744 string(
"smooth() requires obs types L1 L2 C/P1 C/P2:") +
745 oss.str() +
string(
" missing.")));
748 bool useC1(hasType(
"C1")), useC2(hasType(
"C2"));
761 const double D11 = (alpha + 2.) / alpha;
762 const double D12 = -2. / alpha;
763 const double D21 = (2 * alpha + 2.) / alpha;
764 const double D22 = -D11;
768 double RB1, RB2, dbL1, dbL2, dLB10(0.0), dLB20(0.0);
769 long LB1, LB2, LB10, LB20;
773 for (first =
true, i = 0; i < spdvector.size(); i++)
775 if (!(spdvector[i].flag &
OK))
780 double P1 = spdvector[i].data[indexForLabel[(useC1 ?
"C1" :
"P1")]];
781 double P2 = spdvector[i].data[indexForLabel[(useC2 ?
"C2" :
"P2")]];
782 double L1 = spdvector[i].data[indexForLabel[
"L1"]] - dLB10;
783 double L2 = spdvector[i].data[indexForLabel[
"L2"]] - dLB20;
789 dLB10 = double(LB10);
790 dLB20 = double(LB20);
809 LOG(
DEBUG) <<
"SMTDMP " << sat <<
" "
811 << setprecision(3) <<
" " << setw(13) << RB1 - dbL1
812 <<
" " << setw(13) << RB2 - dbL2 <<
" " << setw(13) <<
L1
813 <<
" " << setw(13) <<
L2 <<
" " << setw(13) <<
P1 <<
" "
814 << setw(13) <<
P2 <<
" " << setw(13) << RB1 <<
" "
822 LB1 = LB10 + long(RB1 + (RB1 > 0 ? 0.5 : -0.5));
823 LB2 = LB20 + long(RB2 + (RB2 > 0 ? 0.5 : -0.5));
826 oss <<
"SMT" << fixed << setprecision(2) <<
" " << sat <<
" "
827 <<
printTime(getFirstGoodTime(), outFormat) <<
" "
828 <<
printTime(getLastGoodTime(), outFormat) <<
" " << setw(5)
829 << PB1.
N() <<
" " << setw(12) << PB1.
Average() + dbL1 <<
" "
830 << setw(5) << PB1.
StdDev() <<
" " << setw(12)
831 << PB1.
Minimum() + dbL1 <<
" " << setw(12) << PB1.
Maximum() + dbL1
832 <<
" " << setw(5) << PB2.
N() <<
" " << setw(12)
833 << PB2.
Average() + dbL2 <<
" " << setw(5) << PB2.
StdDev() <<
" "
834 << setw(12) << PB2.
Minimum() + dbL2 <<
" " << setw(12)
835 << PB2.
Maximum() + dbL2 <<
" " << setw(13) << RB1 <<
" "
836 << setw(13) << RB2 <<
" " << setw(10) << LB1 <<
" " << setw(10)
840 if (!debiasPH && !smoothPR)
845 for (i = 0; i < spdvector.size(); i++)
847 if (!(spdvector[i].flag &
OK))
856 dbL1 = spdvector[i].data[indexForLabel[
"L1"]] - RB1;
857 dbL2 = spdvector[i].data[indexForLabel[
"L2"]] - RB2;
859 spdvector[i].data[indexForLabel[(useC1 ?
"C1" :
"P1")]] =
860 D11 *
wl1 * dbL1 + D12 *
wl2 * dbL2;
861 spdvector[i].data[indexForLabel[(useC2 ?
"C2" :
"P2")]] =
862 D21 *
wl1 * dbL1 + D22 *
wl2 * dbL2;
869 spdvector[i].data[indexForLabel[
"L1"]] -= LB1;
870 spdvector[i].data[indexForLabel[
"L2"]] -= LB2;
884 if (i >= spdvector.size())
889 map<string, unsigned int>::const_iterator it;
890 if ((it = indexForLabel.find(type)) == indexForLabel.end())
892 Exception e(
"Invalid obs type in data() " + type);
895 return spdvector[i].data[it->second];
898 double& SatPass::timeoffset(
unsigned int i)
900 if (i >= spdvector.size())
905 return spdvector[i].toffset;
908 unsigned short& SatPass::LLI(
unsigned int i,
const std::string& type)
910 if (i >= spdvector.size())
915 map<string, unsigned int>::const_iterator it;
916 if ((it = indexForLabel.find(type)) == indexForLabel.end())
918 Exception e(
"Invalid obs type in LLI() " + type);
921 return spdvector[i].lli[it->second];
924 unsigned short& SatPass::SSI(
unsigned int i,
const std::string& type)
926 if (i >= spdvector.size())
931 map<string, unsigned int>::const_iterator it;
932 if ((it = indexForLabel.find(type)) == indexForLabel.end())
934 Exception e(
"Invalid obs type in SSI() " + type);
937 return spdvector[i].ssi[it->second];
942 void SatPass::setFlag(
unsigned int i,
unsigned short f)
944 if (i >= spdvector.size())
950 if (spdvector[i].flag != BAD && f == BAD)
954 if (spdvector[i].flag == BAD && f != BAD)
958 spdvector[i].flag = f;
964 void SatPass::setUserFlag(
unsigned int i,
unsigned int f)
966 if (i >= spdvector.size())
972 spdvector[i].userflag = f;
977 unsigned short SatPass::getFlag(
unsigned int i)
const
979 if (i >= spdvector.size())
984 return spdvector[i].flag;
990 unsigned int SatPass::getUserFlag(
unsigned int i)
const
992 if (i >= spdvector.size())
997 return spdvector[i].userflag;
1001 unsigned int SatPass::getCount(
unsigned int i)
const
1003 if (i >= spdvector.size())
1008 return spdvector[i].ndt;
1017 return time(spdvector.size() - 1);
1023 const std::string& type2)
const
1025 if (i >= spdvector.size())
1030 map<string, unsigned int>::const_iterator it;
1031 if ((it = indexForLabel.find(type1)) != indexForLabel.end())
1033 return spdvector[i].data[it->second];
1035 else if ((it = indexForLabel.find(type2)) != indexForLabel.end())
1037 return spdvector[i].data[it->second];
1041 Exception e(
"Invalid obs types in data() " + type1 +
" " + type2);
1046 unsigned short SatPass::LLI(
unsigned int i,
const std::string& type1,
1047 const std::string& type2)
1049 if (i >= spdvector.size())
1054 map<string, unsigned int>::const_iterator it;
1055 if ((it = indexForLabel.find(type1)) != indexForLabel.end())
1057 return spdvector[i].lli[it->second];
1059 else if ((it = indexForLabel.find(type2)) != indexForLabel.end())
1061 return spdvector[i].lli[it->second];
1065 Exception e(
"Invalid obs types in LLI() " + type1 +
" " + type2);
1070 unsigned short SatPass::SSI(
unsigned int i,
const std::string& type1,
1071 const std::string& type2)
1073 if (i >= spdvector.size())
1078 map<string, unsigned int>::const_iterator it;
1079 if ((it = indexForLabel.find(type1)) == indexForLabel.end())
1081 return spdvector[i].ssi[it->second];
1083 else if ((it = indexForLabel.find(type2)) == indexForLabel.end())
1085 return spdvector[i].ssi[it->second];
1089 Exception e(
"Invalid obs types in SSI() " + type1 +
" " + type2);
1099 if (i >= spdvector.size())
1105 double toff = spdvector[i].ndt * dt + spdvector[i].toffset;
1106 return (firstTime + toff);
1110 bool SatPass::includesTime(
const Epoch& tt)
const
1114 if ((firstTime - tt) > maxGap)
1119 else if (tt > lastTime)
1121 if ((tt - lastTime) > maxGap)
1136 int i, j, n, oldgood, ilast;
1146 for (i = 0; i < spdvector.size(); i++)
1148 n = spdvector[i].ndt;
1152 if (spdvector[i].flag != BAD)
1162 newSP.
ngood = oldgood - ngood;
1166 spdvector[i].ndt = j;
1167 spdvector[i].toffset = tt - newSP.
firstTime - j * dt;
1168 newSP.
spdvector.push_back(spdvector[i]);
1173 spdvector.resize(ilast + 1);
1174 lastTime =
time(ilast);
1184 void SatPass::decimate(
const int N,
Epoch refTime)
1192 if (spdvector.size() < N)
1199 refTime = firstTime;
1203 int i, j, nstart = int(0.5 + (firstTime - refTime) / dt);
1204 nstart = nstart % N;
1209 nstart = N - nstart;
1214 Epoch newfirstTime, tt;
1215 for (j = 0, i = 0; i < spdvector.size(); i++)
1217 if (spdvector[i].ndt % N != nstart)
1224 newfirstTime =
time(i);
1225 spdvector[i].toffset = 0.0;
1226 spdvector[i].ndt = 0;
1231 spdvector[i].ndt = int(0.5 + (tt - newfirstTime) / (N * dt));
1232 spdvector[i].toffset =
1233 tt - newfirstTime - spdvector[i].ndt * N * dt;
1235 spdvector[j] = spdvector[i];
1236 if (spdvector[j].flag != BAD)
1244 firstTime = newfirstTime;
1245 spdvector.resize(j);
1259 bool SatPass::hasOverlap(
const SatPass& that,
double *pdelt,
1310 *pdelt = lastTime - firstTime;
1325 *pdelt = that.
lastTime - firstTime;
1343 void SatPass::dump(ostream& os,
const std::string& msg1,
const std::string& msg2)
1347 os <<
'#' << msg1 <<
" " << *
this <<
" " << msg2 << endl;
1348 os <<
'#' << msg1 <<
" n Sat cnt flg time toffset";
1349 for (j = 0; j < indexForLabel.size(); j++)
1350 os <<
" " << labelForIndex[j] <<
" L S";
1354 for (i = 0; i < spdvector.size(); i++)
1357 os << msg1 <<
" " << setw(3) << i <<
" " << sat <<
" " << setw(3)
1358 << spdvector[i].ndt <<
" " << setw(2) << spdvector[i].flag <<
" "
1359 <<
printTime(tt, SatPass::outFormat) << fixed << setprecision(6)
1360 <<
" " << setw(9) << spdvector[i].toffset << setprecision(3);
1361 for (j = 0; j < indexForLabel.size(); j++)
1362 os <<
" " << setw(13) << spdvector[i].data[j] <<
" "
1363 << spdvector[i].lli[j] <<
" " << spdvector[i].ssi[j];
1366 last = spdvector[i].ndt;
1368 if (spdvector[i].ndt - last > 1)
1370 os <<
" " << spdvector[i].ndt - last;
1372 last = spdvector[i].ndt;
1380 os << setw(4) <<
sp.spdvector.size() <<
" " <<
sp.sat <<
" " << setw(4)
1381 <<
sp.ngood <<
" " << setw(2) <<
sp.Status <<
" "
1382 <<
printTime(
sp.firstTime, SatPass::outFormat) <<
" "
1383 <<
printTime(
sp.lastTime, SatPass::outFormat) <<
" " << fixed
1384 << setprecision(1) <<
sp.dt;
1385 for (
int i = 0; i <
sp.labelForIndex.size(); i++)
1386 os <<
" " <<
sp.labelForIndex[i];
1398 if (spdvector.size() == 0)
1400 firstTime = lastTime = tt;
1405 if (tt - lastTime < 1.e-8)
1410 n = countForTime(tt);
1412 if ((n - spdvector[spdvector.size() - 1].ndt) * dt > maxGap)
1422 if (spd.
flag != SatPass::BAD)
1427 spd.
toffset = tt - firstTime - n * dt;
1428 spdvector.push_back(spd);
1429 return (spdvector.size() - 1);
1435 if (i >= spdvector.size())
1440 return spdvector[i];