diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index 66228d6b3bd1f4a07ad27de52d41f93939fa6744..16dbcce589aa878705c44e452309beeefb96539e 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -99,6 +99,7 @@ const prcopt_t default_prcopt = { struct TdcpOptions { bool enabled; // TDCP enabled + bool batch; // precompute global displacement between 2 epochs bool corr_iono; // apply correction also in TDCP bool corr_tropo; // apply correction also in TDCP bool corr_clock; @@ -116,7 +117,7 @@ struct Options 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, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode int tropopt; // troposphere option: TROPOPT_OFF(0):correction off, TROPOPT_SAAS(1):Saastamoinen model, TROPOPT_SBAS(2):SBAS model, TROPOPT_EST(3):troposphere option: ZTD estimation, TROPOPT_ESTG(4):ZTD+grad estimation, TROPOPT_ZTD(5):ZTD correction,6:ZTD+grad correction int sbascorr; // SBAS correction options (can be added): SBSOPT_LCORR(1): long term correction, SBSOPT_FCORR(2): fast correction, SBSOPT_ICORR(4): ionosphere correction, SBSOPT_RANGE(8): ranging - bool raim; // RAIM enabled + int raim; // RAIM removed sats double elmin; // min elevation (degrees) double maxgdop; // maxgdop: reject threshold of gdop bool GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used @@ -155,7 +156,7 @@ const Options default_options = IONOOPT_BRDC, // 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, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode TROPOPT_SAAS, // troposphere option: TROPOPT_OFF(0):correction off, TROPOPT_SAAS(1):Saastamoinen model, TROPOPT_SBAS(2):SBAS model, TROPOPT_EST(3):troposphere option: ZTD estimation, TROPOPT_ESTG(4):ZTD+grad estimation, TROPOPT_ZTD(5):ZTD correction,6:ZTD+grad correction 0, // SBAS correction options (can be added): SBSOPT_LCORR(1): long term correction, SBSOPT_FCORR(2): fast correction, SBSOPT_ICORR(4): ionosphere correction, SBSOPT_RANGE(8): ranging - true, // RAIM enabled + 1, // RAIM enabled D2R*15.0, // min elevation (degrees) 30.0, // maxgdop: reject threshold of gdop true, false, true, true, false, false, false, false, //GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index 7885eb45d1d25d69f89ebe921a186ce9b921c1fa..143fa67b4d775ae123fafffbb43df65700445946 100644 --- a/include/gnss_utils/tdcp.h +++ b/include/gnss_utils/tdcp.h @@ -1,7 +1,7 @@ #ifndef INCLUDE_GNSS_UTILS_TDCP_H_ #define INCLUDE_GNSS_UTILS_TDCP_H_ -#define GNSS_UTILS_TDCP_DEBUG 1 +#define GNSS_UTILS_TDCP_DEBUG 0 #include "gnss_utils/gnss_utils.h" diff --git a/src/tdcp.cpp b/src/tdcp.cpp index 783f738183e3935f254ac7bdf371e3cc3c0d4e77..1bea6dab33bd705a8c5fee41bfe052b8bade5953 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -1,3 +1,4 @@ +#include <iomanip> #include "gnss_utils/tdcp.h" #include "gnss_utils/utils/rcv_position.h" #include "gnss_utils/utils/satellite.h" @@ -203,23 +204,26 @@ bool Tdcp(SnapshotPtr snapshot_r, // Satellite's positions s_r.col(row) = snapshot_r->getSatellites().at(sat_number).pos; - s_k.col(row) = snapshot_r->getSatellites().at(sat_number).pos; - nav_k.ion_gps; + s_k.col(row) = snapshot_k->getSatellites().at(sat_number).pos; + //nav_k.ion_gps; 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 = " << - // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tpositions:\n\t\tt_r: " << - // s_r.col(row).transpose() << "\n\t\tt_k: " << s_k.col(row).transpose() << std::endl; std::cout << "\tobs_r.L: " - // << obs_r.L[freq] << std::endl; std::cout << "\tobs_k.L: " << obs_k.L[freq] << std::endl; std::cout << - // "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.getNavigation().lam[sat_number-1][freq] << - // std::endl; std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " << - // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_r: " << obs_r.L[freq] * - // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_k: " << obs_k.L[freq] * - // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tdrho_m: " << drho_m(row) << - // std::endl; + std::cout << "\tsat " << sat_number + << " frequency " << freq + << " wavelength = " << nav_r.lam[sat_number-1][freq] << std::endl; + std::cout << std::setprecision(10) << "\tpositions:\n\t\ts_r: " << s_r.col(row).transpose() + << "\n\t\ts_k: " << s_k.col(row).transpose() << std::endl; + std::cout << "\tobs_r.L: " << obs_r.L[freq] << std::endl; + std::cout << "\tobs_k.L: " << obs_k.L[freq] << std::endl; + std::cout << "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.lam[sat_number-1][freq] << std::endl; + std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " << nav_k.lam[sat_number-1][freq] << std::endl; + std::cout << "\trho_r: " << obs_r.L[freq] * nav_r.lam[sat_number-1][freq] << std::endl; + std::cout << "\trho_k: " << obs_k.L[freq] * nav_k.lam[sat_number-1][freq] << std::endl; + std::cout << "\tdrho_m: " << drho_m(row) << std::endl; #endif if (!tdcp_params.relinearize_jacobian) @@ -266,7 +270,7 @@ bool Tdcp(SnapshotPtr snapshot_r, // wrong solution if (d.array().isNaN().any()) { - std::cout << "Tdcp: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl; + std::cout << "Tdcp: NLS DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl; return false; } @@ -283,9 +287,11 @@ bool Tdcp(SnapshotPtr snapshot_r, printf("Computed distance = %7.3f m\n", d.head<3>().norm()); printf("Tdcp: residual = %13.10f\n", residual); printf("Tdcp: row = %lu\n", r.rows()); - std::cout << "Tdcp: drho = " << r.transpose() << std::endl; + std::cout << "Tdcp: r = " << r.transpose() << std::endl; std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl; - std::cout << "Tdcp: H = \n" << A << std::endl; + std::cout << "Tdcp: A = \n" << A << std::endl; + std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl; + std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl; printf("Tdcp: dT = %8.3f secs\n", d(3)); #endif @@ -342,7 +348,9 @@ bool Tdcp(SnapshotPtr snapshot_r, printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm()); printf("Tdcp: residual = %13.10f\n", residual); std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl; - std::cout << "Tdcp: H = \n" << A_raim << std::endl; + std::cout << "Tdcp: A = \n" << A << std::endl; + std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl; + std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl; printf("Tdcp: dT = %8.3f secs\n", d_raim(3)); #endif @@ -403,17 +411,6 @@ bool Tdcp(SnapshotPtr snapshot_r, // apply the RAIM solution d = best_d; cov_d = (A.transpose() * A).inverse(); - - #if GNSS_UTILS_TDCP_DEBUG == 1 - std::cout << "Tdcp After RAIM iteration" << std::endl; - std::cout << "\tExcluded sats : "; - std::cout << "\tCommon sats : " << n_common_sats << std::endl; - std::cout << "\tRows : " << n_differences << std::endl; - std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl; - std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl; - std::cout << "\tClock offset = " << d(3) << std::endl; - std::cout << "\tResidual = " << best_residual << std::endl; - #endif } #if GNSS_UTILS_TDCP_DEBUG == 1 @@ -425,6 +422,9 @@ bool Tdcp(SnapshotPtr snapshot_r, std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl; std::cout << "\tClock offset = " << d(3) << std::endl; std::cout << "\tResidual = " << best_residual << std::endl; + std::cout << "\tA = \n" << A << std::endl; + std::cout << "\tH = \n" << A.transpose() * A << std::endl; + std::cout << "\tcov_d = \n" << cov_d << std::endl; #endif } @@ -437,6 +437,9 @@ bool Tdcp(SnapshotPtr snapshot_r, std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl; std::cout << "\tClock offset = " << d(3) << std::endl; std::cout << "\tResidual = " << residual << std::endl; + std::cout << "\tA = \n" << A << std::endl; + std::cout << "\tH = \n" << A.transpose() * A << std::endl; + std::cout << "\tcov_d = \n" << cov_d << std::endl; #endif }