Index: trunk/BNC/src/orbComp/sp3Comp.cpp =================================================================== --- trunk/BNC/src/orbComp/sp3Comp.cpp (revision 6342) +++ trunk/BNC/src/orbComp/sp3Comp.cpp (revision 6344) @@ -105,2 +105,261 @@ } +//// Satellite Index in clkSats set +////////////////////////////////////////////////////////////////////////////////// +//int satIndex(const set& clkSats, const GPSS::t_prn& prn) { +// int ret = 0; +// for (set::const_iterator it = clkSats.begin(); it != clkSats.end(); it++) { +// if ( *it == prn) { +// return ret; +// } +// ++ret; +// } +// cerr << "satellite not found in used Sats " << prn << endl; +// exit(1); +//} +// +//// Estimate Clock Offsets +////////////////////////////////////////////////////////////////////////////////// +//void processClocks(const set& clkSats, vector& epochs, +// map& stat) { +// +// if (clkSats.size() == 0) { +// return; +// } +// +// int nPar = epochs.size() + clkSats.size(); +// SymmetricMatrix NN(nPar); NN = 0.0; +// ColumnVector bb(nPar); bb = 0.0; +// +// // Create Matrix A'A and vector b +// // ------------------------------ +// for (unsigned ie = 0; ie < epochs.size(); ie++) { +// const GPSS::t_gpstime& tt = epochs[ie]->_tt; +// const map& dc = epochs[ie]->_dc; +// Matrix AA(dc.size(), nPar); AA = 0.0; +// ColumnVector ll(dc.size()); ll = 0.0; +// map::const_iterator it; int ii; +// for (it = dc.begin(), ii = 0; it != dc.end(); it++, ii++) { +// const GPSS::t_prn& prn = it->first; +// int index = epochs.size() + satIndex(clkSats, prn); +// AA[ii][ie] = 1.0; // epoch-specfic offset (common for all satellites) +// AA[ii][index] = 1.0; // satellite-specific offset (common for all epochs) +// ll[ii] = it->second; +// } +// SymmetricMatrix dN; dN << AA.t() * AA; +// NN += dN; +// bb += AA.t() * ll; +// } +// +// // Regularize NN +// // ------------- +// RowVector HH(nPar); +// HH.columns(1, epochs.size()) = 0.0; +// HH.columns(epochs.size()+1, nPar) = 1.0; +// SymmetricMatrix dN; dN << HH.t() * HH; +// NN += dN; +// +// // Estimate Parameters +// // ------------------- +// ColumnVector xx = NN.i() * bb; +// +// // Compute clock residuals +// // ----------------------- +// for (unsigned ie = 0; ie < epochs.size(); ie++) { +// const GPSS::t_gpstime& tt = epochs[ie]->_tt; +// map& dc = epochs[ie]->_dc; +// for (map::iterator it = dc.begin(); it != dc.end(); it++) { +// const GPSS::t_prn& prn = it->first; +// int index = epochs.size() + satIndex(clkSats, prn); +// dc[prn] = it->second - xx[ie] - xx[index]; +// stat[prn]._offset = xx[index]; +// } +// } +//} +// +//// Program +////////////////////////////////////////////////////////////////////////////////// +//int main(int argc, char* argv[]) { +// +// GPSS::t_sysout* sysout = new GPSS::t_sysout("", 0, false); +// GPSS::rtnet_core_log = sysout->getSysoutCore(); +// +// // Parse Input Options +// // ------------------- +// map OPT; +// parseCmdLine(argc, argv, OPT); +// if (OPT.find("sp3File1") == OPT.end() || OPT.find("sp3File2") == OPT.end()) { +// cerr << "usage: sp3comp --sp3File1 --sp3File2 " << endl; +// return 1; +// } +// +// // Synchronize reading of two sp3 files +// // ------------------------------------ +// vector prnList; +// for (unsigned prn = 1; prn <= GPSS::t_prn::MAXPRN; prn++) { +// prnList.push_back(GPSS::t_prn(prn)); +// } +// GPSS::t_sp3 in1(prnList); if (in1.assignFile(OPT["sp3File1"].c_str())) return 1; +// GPSS::t_sp3 in2(prnList); if (in2.assignFile(OPT["sp3File2"].c_str())) return 1; +// +// vector epochs; +// set clkSats; +// while (true) { +// in1.readNextEpoch(); +// GPSS::t_gpstime tt = in1.etime(); +// if (!tt.valid()) { +// break; +// } +// if (in2.readEpoch(tt) == t_irc::success) { +// t_epoch* epo = new t_epoch; epo->_tt = tt; +// bool epochOK = false; +// for (unsigned iPrn = 1; iPrn <= GPSS::t_prn::MAXPRN; iPrn++) { +// GPSS::t_prn prn(iPrn); +// ColumnVector xyz1(3), xyz2(3); +// ColumnVector vel1(3), vel2(3); +// double clk1, clk2; +// bool posSet1, posSet2; +// bool clkSet1, clkSet2; +// bool velSet1, velSet2; +// if (in1.position(prn, xyz1[0], xyz1[1], xyz1[2], posSet1, clk1, clkSet1, +// vel1[0], vel1[1], vel1[2], velSet1) == 0 && +// in2.position(prn, xyz2[0], xyz2[1], xyz2[2], posSet2, clk2, clkSet2, +// vel1[0], vel1[1], vel1[2], velSet1) == 0) { +// if (posSet1 && posSet2) { +// epochOK = true; +// epo->_dr[prn] = xyz1 - xyz2; +// epo->_xyz[prn] = xyz1; +// if (clkSet1 && clkSet2) { +// epo->_dc[prn] = (clk1 - clk2) * t_genConst::c; +// clkSats.insert(prn); +// } +// } +// } +// } +// if (epochOK) { +// epochs.push_back(epo); +// } +// else { +// delete epo; +// } +// } +// if (in1.finished()) { +// break; +// } +// } +// +// // Transform xyz into radial, along-track, and out-of-plane +// // -------------------------------------------------------- +// for (unsigned ie = 0; ie < epochs.size(); ie++) { +// t_epoch* epoch = epochs[ie]; +// t_epoch* epoch2 = 0; +// if (ie == 0 && epochs.size() > 1) { +// epoch2 = epochs[ie+1]; +// } +// else { +// epoch2 = epochs[ie-1]; +// } +// double dt = epoch->_tt - epoch2->_tt; +// map& dr = epoch->_dr; +// map& xyz = epoch->_xyz; +// map& xyz2 = epoch2->_xyz; +// for (map::const_iterator it = dr.begin(); it != dr.end(); it++) { +// const GPSS::t_prn& prn = it->first; +// if (xyz2.find(prn) != xyz2.end()) { +// const ColumnVector dx = dr[prn]; +// const ColumnVector& x1 = xyz[prn]; +// const ColumnVector& x2 = xyz2[prn]; +// ColumnVector vel = (x1 - x2) / dt; +// GPSS::t_astro::XYZ_to_RSW(x1, vel, dx, dr[prn]); +// } +// else { +// cerr << "not found: " << prn << endl; +// } +// } +// } +// +// map stat; +// +// // Estimate Clock Offsets +// // ---------------------- +// processClocks(clkSats, epochs, stat); +// +// // Print Residuals +// // --------------- +// const string all = "ZZZ"; +// +// cout.setf(ios::fixed); +// cout << "!\n! MJD PRN radial along out clk clkRed iPRN" +// "\n! ----------------------------------------------------------------\n"; +// for (unsigned ii = 0; ii < epochs.size(); ii++) { +// const t_epoch* epo = epochs[ii]; +// const map& dr = epochs[ii]->_dr; +// const map& dc = epochs[ii]->_dc; +// for (map::const_iterator it = dr.begin(); it != dr.end(); it++) { +// const GPSS::t_prn& prn = it->first; +// const ColumnVector& rao = it->second; +// cout << setprecision(6) << epo->_tt.mjddec() << ' ' << prn << ' ' +// << setw(7) << setprecision(4) << rao[0] << ' ' +// << setw(7) << setprecision(4) << rao[1] << ' ' +// << setw(7) << setprecision(4) << rao[2] << " "; +// stat[prn]._rao += SP(rao, rao); // Schur product +// stat[prn]._nr += 1; +// stat[all]._rao += SP(rao, rao); +// stat[all]._nr += 1; +// if (dc.find(prn) != dc.end()) { +// double clkRes = dc.find(prn)->second; +// double clkResRed = clkRes - it->second[0]; // clock minus radial component +// cout << setw(7) << setprecision(4) << clkRes << ' ' +// << setw(7) << setprecision(4) << clkResRed; +// stat[prn]._dc += clkRes * clkRes; +// stat[prn]._dcRed += clkResRed * clkResRed; +// stat[prn]._nc += 1; +// stat[all]._dc += clkRes * clkRes; +// stat[all]._dcRed += clkResRed * clkResRed; +// stat[all]._nc += 1; +// } +// else { +// cout << " . . "; +// } +// cout << " " << setw(2) << int(prn) << endl; +// } +// delete epo; +// } +// +// // Print Summary +// // ------------- +// cout << "!\n! RMS:\n"; +// for (map::iterator it = stat.begin(); it != stat.end(); it++) { +// const string& prn = it->first; +// t_stat& stat = it->second; +// if (stat._nr > 0) { +// stat._rao[0] = sqrt(stat._rao[0] / stat._nr); +// stat._rao[1] = sqrt(stat._rao[1] / stat._nr); +// stat._rao[2] = sqrt(stat._rao[2] / stat._nr); +// if (prn == all) { +// cout << "!\n! Total "; +// } +// else { +// cout << "! " << prn << ' '; +// } +// cout << setw(7) << setprecision(4) << stat._rao[0] << ' ' +// << setw(7) << setprecision(4) << stat._rao[1] << ' ' +// << setw(7) << setprecision(4) << stat._rao[2] << " "; +// if (stat._nc > 0) { +// stat._dc = sqrt(stat._dc / stat._nc); +// stat._dcRed = sqrt(stat._dcRed / stat._nc); +// cout << setw(7) << setprecision(4) << stat._dc << ' ' +// << setw(7) << setprecision(4) << stat._dcRed; +// if (prn != all) { +// cout << " offset " << setw(7) << setprecision(4) << stat._offset; +// } +// } +// else { +// cout << " . . "; +// } +// cout << endl; +// } +// } +// +// return 0; +//} Index: trunk/BNC/src/orbComp/sp3Comp.h =================================================================== --- trunk/BNC/src/orbComp/sp3Comp.h (revision 6342) +++ trunk/BNC/src/orbComp/sp3Comp.h (revision 6344) @@ -26,5 +26,9 @@ #define SP3COMP_H +#include #include +#include +#include "bnctime.h" +#include "t_prn.h" class t_sp3Comp : public QThread { @@ -46,4 +50,31 @@ private: + class t_epoch { + public: + bncTime _tt; + std::map _dr; + std::map _xyz; + std::map _dc; + }; + + class t_stat { + public: + t_stat() { + _rao.ReSize(3); + _rao = 0.0; + _dc = 0.0; + _dcRed = 0.0; + _offset = 0.0; + _nr = 0; + _nc = 0; + } + ColumnVector _rao; + double _dc; + double _dcRed; + double _offset; + int _nr; + int _nc; + }; + QStringList _sp3FileNames; QString _logFileName;