diff --git a/deps/RTKLIB b/deps/RTKLIB index edbded11390e0339ce37d9677905b8425bed80fb..3f0bb15ea844df97592a5d0c96222da5db6566ae 160000 --- a/deps/RTKLIB +++ b/deps/RTKLIB @@ -1 +1 @@ -Subproject commit edbded11390e0339ce37d9677905b8425bed80fb +Subproject commit 3f0bb15ea844df97592a5d0c96222da5db6566ae diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index bcb6843faf81dfae31baf09963498000ad842dea..8fc1dd999b292064f92ea7ac572a5a3546b2a136 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -15,18 +15,30 @@ extern "C" { // eigen #include <eigen3/Eigen/Dense> +#ifndef RAD2DEG +#define RAD2DEG 180.0 / 3.14159265358979323846 +#endif + +#ifndef DEG2RAD +#define DEG2RAD 3.14159265358979323846 / 180.0 +#endif + namespace GnssUtils { // Structs struct PseudoRange { + int sat = 0; double p = -1; - double var = 1; + double prange_var = 1; double prange = -1; double iono_corr = 0; double tropo_corr = 0; double sat_clock_corr = 0; + double L = -1; + double carrier_range = -1; + double carrier_range_var = 1; }; struct ComputePosOutput @@ -48,6 +60,106 @@ struct ComputePosOutput Eigen::Vector3d lat_lon; // latitude_longitude_altitude std::map<int,Eigen::Vector2d> sat_azel; // azimuth and elevation of each satellite provided + std::set<int> used_sats; // used sats for computing fix (applying RAIM if the case and discarding wrong data) + std::set<int> discarded_sats; // discarded sats when computing fix (applying RAIM if the case and discarding wrong data) + std::map<int,double> prange_residuals; // residuals of used pseudoranges (applying RAIM if the case and discarding wrong data) +}; + +/* defaults processing options */ +const prcopt_t opt_default = { + PMODE_SINGLE, /* mode: positioning mode (PMODE_???) */ + 0, /* soltype: solution type (0:forward,1:backward,2:combined) */ + 2, /* nf: number of frequencies (1:L1,2:L1+L2,3:L1+L2+L3,4:L1+L2+L3+L4) */ + SYS_GPS|SYS_GLO|SYS_GAL, /* navsys */ + 15.0*D2R,{{0,0}}, /* elmin (rad) ,snrmask */ + 0, /* satellite ephemeris/clock (EPHOPT_???) */ + 3,3,1,0,1, /* modear,glomodear,gpsmodear,bdsmodear,arfilter */ + 20,0,4,5,10,20, /* maxout,minlock,minfixsats,minholdsats,mindropsats,minfix */ + 0,1, /* rcvstds,armaxiter */ + IONOOPT_BRDC, /* ionoopt: ionosphere option (IONOOPT_???) */ + TROPOPT_SAAS, /* tropopt: troposphere option (TROPOPT_???) */ + 1,0, /* dynamics,tidecorr */ + 3, /* niter: number of filter iteration */ + 0,0, /* codesmooth,intpref */ + 0, /* sbascorr: SBAS correction options */ + 0, /* sbasssatsel: SBAS satellite selection (0:all) */ + 0,0, /* rovpos,refpos */ + WEIGHTOPT_ELEVATION, /* weightmode */ + {300.0,300.0,300.0}, /* eratio[]: measurement error factor [0]:reserved [1-3]:error factor a/b/c of phase (m) [4]:doppler frequency (hz) [5]: snr max value (dB.Hz) */ + {100.0,0.003,0.003,0.0,1.0,52.0}, /* err[]: */ + {30.0,0.03,0.3}, /* std[] initial-state std [0]bias,[1]iono [2]trop */ + {1E-4,1E-3,1E-4,1E-1,1E-2,0.0}, /* prn[] */ + 5E-12, /* sclkstab */ + {3.0,0.25,0.0,1E-9,1E-5,0.0,0.0,0.0}, /* thresar */ + 0.0,0.0,0.05,0.1,0.01, /* elmaskar,elmaskhold,thresslip,varholdamb,gainholdamb */ + 30.0, /* maxtdiff: max difference of time (sec) */ + 5.0, /* maxinno: reject threshold of innovation (m) */ + 30.0, /* maxgdop: reject threshold of gdop */ + {0},{0},{0}, /* baseline,ru,rb */ + {"",""}, /* anttype */ + {{0}},{{0}},{0}, /* antdel,pcvr,exsats */ + 1,1, /* maxaveep,initrst */ + 0, /* outsingle */ + {{0}}, /* rnxopt */ + {0,0,0,0,1,0}, /* posopt: positioning options [0-1]:PPP pcv [2]:PPP phase windup [3]:PPP exclude eclipsing [4]:PNTPOS RAIM, [5]:PPP clock jump */ + 0, /* syncsol */ + {{0}}, /* odisp */ + {{0}}, /* exterr */ + 0, /* freqopt */ + {0} /* pppopt */ +}; + +struct TdcpOptions +{ + bool enabled; // TDCP enabled + bool corr_iono; // apply correction also in TDCP + bool corr_tropo; // apply correction also in TDCP + bool loss_function; // apply loss function in TDCP factors + //double std_time_factor; // std of TDCP measurements: std_time_factor * dt + double sigma_atm; + double sigma_carrier; + bool use_old_nav; + bool use_multi_freq; + double time_window; // window of time in which we perform TDCP +}; + +struct Options +{ + int sateph; // satellite ephemeris/clock (0:broadcast ephemeris,1:precise ephemeris,2:broadcast + SBAS,3:ephemeris option: broadcast + SSR_APC,4:broadcast + SSR_COM,5: QZSS LEX ephemeris + int ionoopt; // ionosphere option (IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation,5:IONEX TEC model,6:QZSS broadcast,7:QZSS LEX ionosphere,8:SLANT TEC mode) + int tropopt; // troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction) + int sbascorr; // SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging) + bool raim; // RAIM enabled + double elmin; // min elevation (degrees) + double maxgdop; // maxgdop: reject threshold of gdop + bool GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used + TdcpOptions tdcp; // TDCP options + + // compute navsys int + int getNavSys() const + { + return GPS*SYS_GPS + SBS*SYS_SBS + GLO*SYS_GLO + GAL*SYS_GAL + QZS*SYS_QZS + CMP*SYS_CMP + IRN*SYS_IRN + LEO*SYS_LEO; + } + + snrmask_t getSnrMask() const + { + return opt_default.snrmask; + } + + // create a rtklib option struct from this + prcopt_t getPrcopt() const + { + prcopt_t opt{opt_default}; + opt.sateph = sateph; + opt.ionoopt = ionoopt; + opt.tropopt = tropopt; + opt.sbascorr = sbascorr; + opt.posopt[4] = raim; + opt.elmin = elmin; + opt.maxgdop = maxgdop; + opt.navsys = getNavSys(); + return opt; + } }; // forward declarations @@ -78,6 +190,8 @@ enum Combination CARRIER_IONO_FREE ///< iono-free combination for carrier phase }; + + } #endif diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h index f4abefb6810578cbbf65c6396238a995171fa211..91deec41d86cc40b6e2ef0b6aa5d77083deb873d 100644 --- a/include/gnss_utils/observations.h +++ b/include/gnss_utils/observations.h @@ -45,16 +45,18 @@ public: std::set<int> filterByElevationSnr(const Eigen::Vector3d& x_r, const Satellites& sats, const snrmask_t& snrmask, - const double& elmin); + const double& elmin, + const bool& multi_freq); std::set<int> filterByElevationSnr(const std::map<int,Eigen::Vector2d>& azels, const snrmask_t& snrmask, - const double& elmin); + const double& elmin, + const bool& multi_freq); std::set<int> filter(const Satellites& sats, const std::set<int>& discarded_sats, const Eigen::Vector3d& x_r, const bool& check_code, const bool& check_carrier_phase, - const prcopt_t& opt); + const Options& opt); std::set<int> filter(const Satellites& sats, const std::set<int>& discarded_sats, const Eigen::Vector3d& x_r, @@ -62,13 +64,14 @@ public: const bool& check_carrier_phase, const int& navsys, const snrmask_t& snrmask, - const double& elmin); + const double& elmin, + const bool& multi_freq); std::set<int> filter(const Satellites& sats, const std::set<int>& discarded_sats, const std::map<int,Eigen::Vector2d>& azels, const bool& check_code, const bool& check_carrier_phase, - const prcopt_t& opt); + const Options& opt); std::set<int> filter(const Satellites& sats, const std::set<int>& discarded_sats, const std::map<int,Eigen::Vector2d>& azels, @@ -76,7 +79,8 @@ public: const bool& check_carrier_phase, const int& navsys, const snrmask_t& snrmask, - const double& elmin); + const double& elmin, + const bool& multi_freq); // Others static std::set<int> findCommonObservations(const Observations& obs_1, const Observations& obs_2); diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h index 69b27055a6794e438fb78c94f6f0d2fd2cf18e2e..04e2d6da55156844c9636cbd0bdfdf05a0f95996 100644 --- a/include/gnss_utils/snapshot.h +++ b/include/gnss_utils/snapshot.h @@ -42,7 +42,12 @@ public: const Eigen::Vector3d &x_r, const bool &check_code, const bool &check_carrier_phase, - const prcopt_t &opt); + const Options &opt); + std::set<int> filterObservations(const std::set<int> &discarded_sats, + const std::map<int,Eigen::Vector2d>& azels, + const bool &check_code, + const bool &check_carrier_phase, + const Options &opt); // Pseudo-ranges PseudoRanges& getPseudoRanges(); @@ -50,7 +55,7 @@ public: bool pseudoRangesComputed() const; void computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel, const Eigen::Vector3d& latlonalt, - const prcopt_t& opt); + const Options& opt); void print(); @@ -114,11 +119,20 @@ inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded const Eigen::Vector3d &x_r, const bool &check_code, const bool &check_carrier_phase, - const prcopt_t &opt) + const Options &opt) { return obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt); } +inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded_sats, + const std::map<int,Eigen::Vector2d>& azels, + const bool &check_code, + const bool &check_carrier_phase, + const Options &opt) +{ + return obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt); +} + inline const PseudoRanges& Snapshot::getPseudoRanges() const { return pranges_; diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index 45504e0dcd1c78d2ed34d0f56224eeb5427cb859..f5f8a8205a6ad183aa8505471dd3acc96e07fa11 100644 --- a/include/gnss_utils/tdcp.h +++ b/include/gnss_utils/tdcp.h @@ -7,44 +7,37 @@ namespace GnssUtils { -struct TdcpParams +struct TdcpBatchParams { + TdcpOptions tdcp; int min_common_sats; int raim_n; double raim_min_residual; - bool use_carrier_phase; - bool correct_tropo; - bool correct_iono; bool relinearize_jacobian; - int max_iterations; - double sigma_atm; - double sigma_code; - double sigma_carrier; bool use_old_nav; - bool use_multi_freq; - double time_window; + int max_iterations; }; -bool Tdcp(SnapshotPtr snapshot_r, - SnapshotPtr snapshot_k, - Eigen::Vector4d& d, - Eigen::Matrix4d& cov_d, - double& residual, - const std::set<int>& discarded_sats, - std::set<int>& raim_discarded_sats, - const TdcpParams& sd_opt, - const prcopt_t& opt); +bool Tdcp(SnapshotPtr snapshot_r, + SnapshotPtr snapshot_k, + Eigen::Vector4d& d, + Eigen::Matrix4d& cov_d, + double& residual, + const std::set<int>& discarded_sats, + std::set<int>& raim_discarded_sats, + const TdcpBatchParams& tdcp_params, + const Options& opt); -bool Tdcp(SnapshotPtr snapshot_r, - SnapshotPtr snapshot_k, - const Eigen::Vector3d& x_r, - Eigen::Vector4d& d, - Eigen::Matrix4d& cov_d, - double& residual, - const std::set<int>& discarded_sats, - std::set<int>& raim_discarded_sats, - const TdcpParams& sd_params, - const prcopt_t& opt); +bool Tdcp(SnapshotPtr snapshot_r, + SnapshotPtr snapshot_k, + const Eigen::Vector3d& x_r, + Eigen::Vector4d& d, + Eigen::Matrix4d& cov_d, + double& residual, + const std::set<int>& discarded_sats, + std::set<int>& raim_discarded_sats, + const TdcpBatchParams& tdcp_params, + const Options& opt); bool Tdcp(SnapshotPtr snapshot_r, SnapshotPtr snapshot_k, @@ -54,7 +47,7 @@ bool Tdcp(SnapshotPtr snapshot_r, Eigen::Matrix4d& cov_d, double& residual, std::set<int> raim_discarded_sats, - const TdcpParams& sd_params); + const TdcpBatchParams& tdcp_params); } // namespace GnssUtils #endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */ diff --git a/include/gnss_utils/utils/rcv_position.h b/include/gnss_utils/utils/rcv_position.h index 9ac529ce72cf96f98344dd16da856a2e9ff76fff..962d0bafacfede4e1f1398991a47bf896ce597ab 100644 --- a/include/gnss_utils/utils/rcv_position.h +++ b/include/gnss_utils/utils/rcv_position.h @@ -20,7 +20,7 @@ namespace GnssUtils { -ComputePosOutput computePos(const Observations& _observations, const Navigation& _navigation, const prcopt_t& _prcopt); +ComputePosOutput computePos(const Observations& _observations, const Navigation& _navigation, const Options& _prcopt); // ComputePosOutput computePosOwn(const Observations & _observations, // Navigation & _navigation, diff --git a/src/navigation.cpp b/src/navigation.cpp index 8a4264367d98a9add8430b5a0a25b8f1f123bffa..af421b711f719a741451bbdf9093a20b252b2614 100644 --- a/src/navigation.cpp +++ b/src/navigation.cpp @@ -244,8 +244,8 @@ void Navigation::copySbasCorrections(const nav_t& nav) nav_.pcvs[i].sat = nav.pcvs[i].sat; nav_.pcvs[i].te = nav.pcvs[i].te; nav_.pcvs[i].ts = nav.pcvs[i].ts; - std::cout << "copySbasCorrections : " << nav.pcvs[i].te.time << " + " << nav.pcvs[i].te.sec << "\n"; - std::cout << "copySbasCorrections : " << nav_.pcvs[i].te.time << " + " << nav_.pcvs[i].te.sec << "\n"; + // std::cout << "copySbasCorrections : " << nav.pcvs[i].te.time << " + " << nav.pcvs[i].te.sec << "\n"; + // std::cout << "copySbasCorrections : " << nav_.pcvs[i].te.time << " + " << nav_.pcvs[i].te.sec << "\n"; std::copy(nav.pcvs[i].type, nav.pcvs[i].type + ARRAY_SIZE(nav.pcvs[i].type), nav_.pcvs[i].type); std::copy(&nav.pcvs[i].var[0][0], &nav.pcvs[i].var[0][0] + ARRAY2D_SIZE(nav.pcvs[i].var), &nav_.pcvs[i].var[0][0]); } diff --git a/src/observations.cpp b/src/observations.cpp index 45695850ea186b541438f47c52271776d19f48be..191ed998efac58027feb800933cda805bf915f82 100644 --- a/src/observations.cpp +++ b/src/observations.cpp @@ -306,7 +306,8 @@ std::set<int> Observations::filterByConstellations(const int& navsys) std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vector2d>& azels, const snrmask_t& snrmask, - const double& elmin) + const double& elmin, + const bool &multi_freq) { std::set<int> remove_sats; @@ -324,12 +325,22 @@ std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vecto continue; } - // snr TODO: multifrequency (2nd param and 3rd idx) + // snr L1 if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &snrmask) == 1) { - //std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl; + //std::cout << "Discarding sat " << sat_number << ": snr test L1" << std::endl; remove_sats.insert(sat_number); } + if (multi_freq) + { + int L25 = (satsys(sat_number,NULL)&(SYS_GAL|SYS_SBS) ? 2 : 1); + //srn L2/L + if (testsnr(0, L25, elevation, obs_sat.SNR[L25] * 0.25, &snrmask) == 1) + { + //std::cout << "Discarding sat " << sat_number << ": snr test L2/L5" << std::endl; + remove_sats.insert(sat_number); + } + } } // remove sats @@ -346,41 +357,54 @@ std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vecto std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r, const Satellites& sats, const snrmask_t& snrmask, - const double& elmin) + const double& elmin, + const bool &multi_freq) { std::set<int> remove_sats; + std::map<int,Eigen::Vector2d> azels; for (int obs_i = 0; obs_i < obs_.size(); obs_i++) { auto&& obs_sat = getObservationByIdx(obs_i); const int& sat_number = obs_sat.sat; - // check elevation - double elevation = computeSatElevation(x_r, sats.at(sat_number).pos); - if (elevation < elmin) - { - //std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << elmin << std::endl; - remove_sats.insert(sat_number); - continue; - } - - // snr TODO: multifrequency (2nd param and 3rd idx) - if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &snrmask) == 1) - { - //std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl; - remove_sats.insert(sat_number); - } + double elevation = computeSatElevation(x_r, sats.at(obs_sat.sat).pos); + azels.emplace(obs_sat.sat,Eigen::Vector2d(0.0,elevation)); } - // remove sats - // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl; - for (auto sat : remove_sats) - { - assert(hasSatellite(sat)); - removeObservationBySat(sat); - } - - return remove_sats; + return filterByElevationSnr(azels, snrmask, elmin, multi_freq); + +// for (int obs_i = 0; obs_i < obs_.size(); obs_i++) +// { +// auto&& obs_sat = getObservationByIdx(obs_i); +// const int& sat_number = obs_sat.sat; +// +// // check elevation +// double elevation = computeSatElevation(x_r, sats.at(sat_number).pos); +// if (elevation < elmin) +// { +// //std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << elmin << std::endl; +// remove_sats.insert(sat_number); +// continue; +// } +// +// // snr TODO: multifrequency (2nd param and 3rd idx) +// if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &snrmask) == 1) +// { +// //std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl; +// remove_sats.insert(sat_number); +// } +// } +// +// // remove sats +// // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl; +// for (auto sat : remove_sats) +// { +// assert(hasSatellite(sat)); +// removeObservationBySat(sat); +// } +// +// return remove_sats; } std::set<int> Observations::filter(const Satellites& sats, @@ -388,16 +412,17 @@ std::set<int> Observations::filter(const Satellites& sats, const Eigen::Vector3d& x_r, const bool& check_code, const bool& check_carrier_phase, - const prcopt_t& opt) + const Options& opt) { return filter(sats, discarded_sats, x_r, check_code, check_carrier_phase, - opt.navsys, - opt.snrmask, - opt.elmin); + opt.getNavSys(), + opt.getSnrMask(), + opt.elmin, + opt.ionoopt == IONOOPT_IFLC or (opt.tdcp.enabled and opt.tdcp.use_multi_freq)); } std::set<int> Observations::filter(const Satellites& sats, @@ -407,7 +432,8 @@ std::set<int> Observations::filter(const Satellites& sats, const bool& check_carrier_phase, const int& navsys, const snrmask_t& snrmask, - const double& elmin) + const double& elmin, + const bool& multi_freq) { //std::cout << "filter: initial size: " << obs_.size() << std::endl; // Ephemeris @@ -446,7 +472,7 @@ std::set<int> Observations::filter(const Satellites& sats, // Elevation and SNR if (!x_r.isApprox(Eigen::Vector3d::Zero(), 1e-3)) { - std::set<int> remove_sats_elevation = filterByElevationSnr(x_r, sats, snrmask, elmin); + std::set<int> remove_sats_elevation = filterByElevationSnr(x_r, sats, snrmask, elmin, multi_freq); remove_sats.insert(remove_sats_elevation.begin(), remove_sats_elevation.end()); } return remove_sats; @@ -458,16 +484,17 @@ std::set<int> Observations::filter(const Satellites& sats, const std::map<int,Eigen::Vector2d>& azels, const bool& check_code, const bool& check_carrier_phase, - const prcopt_t& opt) + const Options& opt) { return filter(sats, discarded_sats, azels, check_code, check_carrier_phase, - opt.navsys, - opt.snrmask, - opt.elmin); + opt.getNavSys(), + opt.getSnrMask(), + opt.elmin, + opt.ionoopt == IONOOPT_IFLC or (opt.tdcp.enabled and opt.tdcp.use_multi_freq)); } std::set<int> Observations::filter(const Satellites& sats, @@ -477,7 +504,8 @@ std::set<int> Observations::filter(const Satellites& sats, const bool& check_carrier_phase, const int& navsys, const snrmask_t& snrmask, - const double& elmin) + const double& elmin, + const bool& multi_freq) { //std::cout << "filter: initial size: " << obs_.size() << std::endl; // Ephemeris @@ -514,7 +542,7 @@ std::set<int> Observations::filter(const Satellites& sats, remove_sats.insert(remove_sats_constellations.begin(), remove_sats_constellations.end()); // Elevation and SNR - std::set<int> remove_sats_elevation = filterByElevationSnr(azels, snrmask, elmin); + std::set<int> remove_sats_elevation = filterByElevationSnr(azels, snrmask, elmin, multi_freq); remove_sats.insert(remove_sats_elevation.begin(), remove_sats_elevation.end()); return remove_sats; diff --git a/src/receivers/novatel_raw.cpp b/src/receivers/novatel_raw.cpp index 3c59e2544a78bb6b5e4faac00c75da936efb1a4f..d1b650a71c06f751f406a2895de4e2646f1fc4b5 100644 --- a/src/receivers/novatel_raw.cpp +++ b/src/receivers/novatel_raw.cpp @@ -36,6 +36,7 @@ RawDataType NovatelRaw::addDataStream(const std::vector<uint8_t>& data_stream) case NAV_SBAS: // SBAS std::cout << "SBAS received!\n"; + nav_.copySbasCorrections(raw_data_.nav); break; case NAV_ALM_IONUTC: // Almanac and ion/utc parameters diff --git a/src/receivers/ublox_raw.cpp b/src/receivers/ublox_raw.cpp index d80cb5ce05716fef7afbfcd8904ea488534359be..934d5b68f6349d7ae150a9912f07d6698262edfe 100644 --- a/src/receivers/ublox_raw.cpp +++ b/src/receivers/ublox_raw.cpp @@ -15,7 +15,7 @@ RawDataType UBloxRaw::addDataStream(const std::vector<uint8_t>& data_stream) // Update type based on RTKLIB for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte) { - raw_data_type_ = static_cast<RawDataType> ( input_ubx(&raw_data_, (unsigned char)*data_byte) ); + raw_data_type_ = static_cast<RawDataType>(input_ubx(&raw_data_, (unsigned char)*data_byte)); switch (raw_data_type_) { @@ -26,7 +26,6 @@ RawDataType UBloxRaw::addDataStream(const std::vector<uint8_t>& data_stream) case OBS: // Observations // std::cout << "Observations received!\n"; updateObservations(); - // std::cout << "Observations updated!\n"; break; case NAV_EPH: // Ephemeris diff --git a/src/snapshot.cpp b/src/snapshot.cpp index 994f9499e6ee20721c6b87139f378e66f0241258..5ff3285b5255469cdebeae80102f4ac4bf9f6569 100644 --- a/src/snapshot.cpp +++ b/src/snapshot.cpp @@ -35,11 +35,12 @@ void Snapshot::computeSatellites(const int& eph_opt) void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel, const Eigen::Vector3d& latlonalt, - const prcopt_t& opt) + const Options& opt) { assert(pranges_.empty() && "pseudo ranges already computed!"); double dion,dtrp,vmeas,vion,vtrp,P,lam_L1; + prcopt_t prcopt = opt.getPrcopt(); //std::cout << "compute pseudo ranges: \n"; @@ -57,15 +58,15 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel, pranges_.emplace(sat,PseudoRange()); /* psudorange with code bias correction */ - std::cout << "prange...\n"; - if ((P=prange(&obs_i,&nav_->getNavigation(),azel_i,1,&opt,&vmeas))==0.0) + //std::cout << "prange...\n"; + if ((P=prange(&obs_i,&nav_->getNavigation(),azel_i,1,&prcopt,&vmeas))==0.0) { //std::cout << " error in prange\n"; continue; } /* ionospheric corrections */ - std::cout << "iono...\n"; + //std::cout << "iono...\n"; //std::cout << "\ttime: " << obs_i.time.time << " + " << obs_i.time.sec << "\n"; //std::cout << "\tnav: \n"; //nav_->print(); @@ -77,27 +78,27 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel, { if (opt.ionoopt == IONOOPT_SBAS) { - std::cout << "error IONOOPT_SBAS ionocorr, trying with IONOOPT_BRDC..."; + //std::cout << "error IONOOPT_SBAS ionocorr, trying with IONOOPT_BRDC..."; if (!ionocorr(obs_i.time,&nav_->getNavigation(),sat,latlonalt.data(),azel_i,IONOOPT_BRDC,&dion,&vion)) { - std::cout << " error in ionocorr\n"; + //std::cout << " error in ionocorr\n"; continue; } } else { - std::cout << " error in ionocorr\n"; + //std::cout << " error in ionocorr\n"; continue; } } /* GPS-L1 -> L1/B1 */ - std::cout << "iono2...\n"; + //std::cout << "iono2...\n"; if ((lam_L1=nav_->getNavigation().lam[sat-1][0])>0.0) dion*=std::pow(lam_L1/lam_carr[0],2); /* tropospheric corrections */ - std::cout << "tropo...\n"; + //std::cout << "tropo...\n"; if (!tropcorr(obs_i.time,&nav_->getNavigation(),latlonalt.data(),azel_i,opt.tropopt,&dtrp,&vtrp)) { //std::cout << " error in tropcorr\n"; @@ -105,7 +106,8 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel, } /* Store in PseudoRange struct */ - std::cout << "storing\n"; + //std::cout << "storing\n"; + pranges_[sat].sat = sat; pranges_[sat].p = P; pranges_[sat].iono_corr = -dion; pranges_[sat].tropo_corr = -dtrp; @@ -118,7 +120,9 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel, pranges_[sat].sat_clock_corr; /* error variance */ - pranges_[sat].var = varerr(&opt,azel_i[1],opt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp; + pranges_[sat].prange_var = varerr(&prcopt,azel_i[1],prcopt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp; + + // todo: fill L and carrier_range //std::cout << std::endl // << "\t\tprange: " << pranges_[sat].prange << std::endl diff --git a/src/tdcp.cpp b/src/tdcp.cpp index cd1b82c09aa9dc438f136d38e4e87ad63332477e..9207f4cf0e3a4cc4515a80789fcf9b8d71c4ce23 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -5,15 +5,15 @@ namespace GnssUtils { -bool Tdcp(SnapshotPtr snapshot_r, - SnapshotPtr snapshot_k, - Eigen::Vector4d& d, - Eigen::Matrix4d& cov_d, - double& residual, - const std::set<int>& discarded_sats, - std::set<int>& raim_discarded_sats, - const TdcpParams& sd_params, - const prcopt_t& opt) +bool Tdcp(SnapshotPtr snapshot_r, + SnapshotPtr snapshot_k, + Eigen::Vector4d& d, + Eigen::Matrix4d& cov_d, + double& residual, + const std::set<int>& discarded_sats, + std::set<int>& raim_discarded_sats, + const TdcpBatchParams& tdcp_params, + const Options& opt) { return Tdcp(snapshot_r, snapshot_k, @@ -23,7 +23,7 @@ bool Tdcp(SnapshotPtr snapshot_r, residual, discarded_sats, raim_discarded_sats, - sd_params, + tdcp_params, opt); } @@ -35,12 +35,12 @@ bool Tdcp(SnapshotPtr snapshot_r, double& residual, const std::set<int>& discarded_sats, std::set<int>& raim_discarded_sats, - const TdcpParams& sd_params, - const prcopt_t& opt) + const TdcpBatchParams& tdcp_params, + const Options& opt) { // If use old nav temporary change navigation to (re)compute satellites positions auto nav_k = snapshot_k->getNavigation(); - if (sd_params.use_old_nav) + if (tdcp_params.use_old_nav) { snapshot_k->getSatellites().clear(); snapshot_k->setNavigation(snapshot_r->getNavigation()); @@ -56,14 +56,14 @@ bool Tdcp(SnapshotPtr snapshot_r, snapshot_r->getObservations()->filter(snapshot_r->getSatellites(), discarded_sats, x_r, - !sd_params.use_carrier_phase, - sd_params.use_carrier_phase, + false, + true, opt); snapshot_k->getObservations()->filter(snapshot_k->getSatellites(), discarded_sats, x_r, - !sd_params.use_carrier_phase, - sd_params.use_carrier_phase, + false, + true, opt); // FIND COMMON SATELLITES OBSERVATIONS @@ -79,10 +79,10 @@ bool Tdcp(SnapshotPtr snapshot_r, cov_d, residual, raim_discarded_sats, - sd_params); + tdcp_params); // UNDO temporary change navigation - if (sd_params.use_old_nav) + if (tdcp_params.use_old_nav) { snapshot_k->setNavigation(nav_k); snapshot_k->computeSatellites(opt.sateph); @@ -99,7 +99,7 @@ bool Tdcp(SnapshotPtr snapshot_r, Eigen::Matrix4d& cov_d, double& residual, std::set<int> raim_discarded_sats, - const TdcpParams& sd_params) + const TdcpBatchParams& tdcp_params) { // Checks assert(snapshot_r->satellitesComputed() && "this TDCP assumes satellites already computed"); @@ -119,7 +119,7 @@ bool Tdcp(SnapshotPtr snapshot_r, } int n_common_sats = common_sats.size(); - int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n)); + int required_n_sats(std::max(tdcp_params.min_common_sats + tdcp_params.raim_n, 4 + tdcp_params.raim_n)); if (n_common_sats < required_n_sats) { #if GNSS_UTILS_TDCP_DEBUG == 1 @@ -151,30 +151,22 @@ bool Tdcp(SnapshotPtr snapshot_r, } // Carrier phase - if (sd_params.use_carrier_phase) - { - // L1/E1 - if (std::abs(obs_r.L[0]) > 1e-12 and std::abs(obs_k.L[0]) > 1e-12) - row_2_sat_freq[row++] = std::make_pair(sat, 0); - - if (!sd_params.use_multi_freq) - continue; - - // L2 (GPS/GLO/QZS) - int sys = satsys(sat, NULL); - if (NFREQ >= 2 and (sys & (SYS_GPS | SYS_GLO | SYS_QZS)) and std::abs(obs_r.L[1]) > 1e-12 and - std::abs(obs_k.L[1]) > 1e-12) - row_2_sat_freq[row++] = std::make_pair(sat, 1); - - // E5 (GAL) - else if (NFREQ >= 3 and (sys & SYS_GAL) and std::abs(obs_r.L[2]) > 1e-12 and std::abs(obs_k.L[2]) > 1e-12) - row_2_sat_freq[row++] = std::make_pair(sat, 2); - } - // Code - else - // L1/E1 - if (std::abs(obs_r.P[0]) > 1e-12 and std::abs(obs_k.P[0]) > 1e-12) - row_2_sat_freq[row++] = std::make_pair(sat, 0); + // L1/E1 + if (std::abs(obs_r.L[0]) > 1e-12 and std::abs(obs_k.L[0]) > 1e-12) + row_2_sat_freq[row++] = std::make_pair(sat, 0); + + if (!tdcp_params.tdcp.use_multi_freq) + continue; + + // L2 (GPS/GLO/QZS) + int sys = satsys(sat, NULL); + if (NFREQ >= 2 and (sys & (SYS_GPS | SYS_GLO | SYS_QZS)) and std::abs(obs_r.L[1]) > 1e-12 and + std::abs(obs_k.L[1]) > 1e-12) + row_2_sat_freq[row++] = std::make_pair(sat, 1); + + // E5 (GAL) + else if (NFREQ >= 3 and (sys & SYS_GAL) and std::abs(obs_r.L[2]) > 1e-12 and std::abs(obs_k.L[2]) > 1e-12) + row_2_sat_freq[row++] = std::make_pair(sat, 2); } int n_differences = row_2_sat_freq.size(); @@ -214,11 +206,8 @@ bool Tdcp(SnapshotPtr snapshot_r, s_k.col(row) = snapshot_r->getSatellites().at(sat_number).pos; nav_k.ion_gps; - if (sd_params.use_carrier_phase) // TODO: add iono and tropo corrections (optionally) - drho_m(row) = (obs_k.L[freq] * nav_k.lam[sat_number - 1][freq]) - - (obs_r.L[freq] * nav_r.lam[sat_number - 1][freq]); - else - drho_m(row) = obs_k.P[freq] - obs_r.P[freq]; + drho_m(row) = (obs_k.L[freq] * nav_k.lam[sat_number - 1][freq]) - + (obs_r.L[freq] * nav_r.lam[sat_number - 1][freq]); #if GNSS_UTILS_TDCP_DEBUG == 1 // std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " << @@ -233,7 +222,7 @@ bool Tdcp(SnapshotPtr snapshot_r, // std::endl; #endif - if (!sd_params.relinearize_jacobian) + if (!tdcp_params.relinearize_jacobian) { // Unit vectors from receivers to satellites Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized(); @@ -247,7 +236,7 @@ bool Tdcp(SnapshotPtr snapshot_r, } // LEAST SQUARES SOLUTION ======================================================================= - for (int j = 0; j < sd_params.max_iterations; j++) + for (int j = 0; j < tdcp_params.max_iterations; j++) { // fill A and r for (int row = 0; row < A.rows(); row++) @@ -256,7 +245,7 @@ bool Tdcp(SnapshotPtr snapshot_r, r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3); // Evaluate A - if (sd_params.relinearize_jacobian) + if (tdcp_params.relinearize_jacobian) { // Unit vectors from rcvrs to sats Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized(); @@ -301,7 +290,7 @@ bool Tdcp(SnapshotPtr snapshot_r, #endif // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS - if (j == 0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual) + if (j == 0 and tdcp_params.raim_n > 0 and residual > tdcp_params.raim_min_residual) { int worst_sat_row = -1; double best_residual = 1e12; @@ -309,7 +298,7 @@ bool Tdcp(SnapshotPtr snapshot_r, int n_removed_rows = 1; // remove some satellites - while (raim_discarded_sats.size() < sd_params.raim_n) + while (raim_discarded_sats.size() < tdcp_params.raim_n) { auto A_raim = A; auto r_raim = r; @@ -321,7 +310,7 @@ bool Tdcp(SnapshotPtr snapshot_r, // Multi-freq: some rows for the same satellite n_removed_rows = 1; - if (sd_params.use_multi_freq) + if (tdcp_params.tdcp.use_multi_freq) while (row_removed + n_removed_rows < A_raim.rows() and row_2_sat_freq.at(row_removed + n_removed_rows).first == sat_removed) n_removed_rows++; @@ -452,9 +441,8 @@ bool Tdcp(SnapshotPtr snapshot_r, } // weight covariance with the measurement noise (proportional to time) - double sq_sigma_Tdcp = (tk - tr) * sd_params.sigma_atm * sd_params.sigma_atm + - 2 * (sd_params.use_carrier_phase ? sd_params.sigma_carrier * sd_params.sigma_carrier : - sd_params.sigma_code * sd_params.sigma_code); + double sq_sigma_Tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm + + 2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier; cov_d *= sq_sigma_Tdcp; // residual = (r - A * d).norm() / A.rows(); diff --git a/src/utils/rcv_position.cpp b/src/utils/rcv_position.cpp index 8a2b450c1b9e8ca4d40cc0e19677f7821a2f5aa9..4e3501e1824d8021d553ce0e436f30fe294a2d42 100644 --- a/src/utils/rcv_position.cpp +++ b/src/utils/rcv_position.cpp @@ -13,26 +13,32 @@ namespace GnssUtils { ComputePosOutput computePos(const GnssUtils::Observations& _observations, const GnssUtils::Navigation& _navigation, - const prcopt_t& _prcopt) + const Options& _opt) { // Define error msg char msg[128] = ""; + // Convert options to rtklib + prcopt_t prcopt = _opt.getPrcopt(); + + // output data GnssUtils::ComputePosOutput output; sol_t sol; sol = { { 0 } }; - + ssat_t sats_status[MAXSAT]; std::vector<double> sat_elevations(2 * _observations.size()); + // compute pose output.pos_stat = pntpos(_observations.data(), _observations.size(), &(_navigation.getNavigation()), - &_prcopt, + &prcopt, &sol, sat_elevations.data(), - NULL, + sats_status, msg); + // Fill output struct if (output.pos_stat == 0) std::cout << "computePos: error in computing positioning, message: " << msg << "\n"; @@ -59,12 +65,27 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations, output.sat_azel.emplace(_observations.getObservationByIdx(i).sat, Eigen::Vector2d(sat_elevations.at(2 * i), sat_elevations.at(2 * i + 1))); + for (auto i = 0; i < MAXSAT; i++) + { + int sat = i+1; + if (_observations.hasSatellite(sat)) + { + if (sats_status[i].vs) + { + output.prange_residuals.emplace(sat,sats_status[i].resp[0]); + output.used_sats.insert(sat); + } + else + output.discarded_sats.insert(sat); + } + } + return output; } // ComputePosOutput computePosOwn(const GnssUtils::Observations & _observations, // GnssUtils::Navigation & _navigation, -// const prcopt_t & _prcopt) +// const prcopt_t & prcopt) // { // // Remove duplicated satellites @@ -79,7 +100,7 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations, // output.pos_stat = pntposOwn(_observations.data(), _observations.size(), // &(_navigation.getNavigation()), -// &_prcopt, &sol, NULL, NULL, msg); +// &prcopt, &sol, NULL, NULL, msg); // if (output.pos_stat == 0) // {