From 4f30d0fb5eb0db484f65d41dcc8dc790124d2ec6 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 21 Jun 2021 19:46:06 +0200
Subject: [PATCH] changes in structure of params for tdcp
---
include/gnss_utils/gnss_utils.h | 44 +++++++++----------
include/gnss_utils/tdcp.h | 22 +++-------
src/observations.cpp | 4 +-
src/range.cpp | 10 ++---
src/tdcp.cpp | 78 +++++++++------------------------
test/gtest_tdcp.cpp | 28 ++++++------
6 files changed, 68 insertions(+), 118 deletions(-)
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 7d87ea5..cf3d2d0 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -98,17 +98,30 @@ 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;
- bool loss_function; // apply loss function in TDCP factors
+ bool loss_function; // apply loss function in TDCP factors
double sigma_atm;
double sigma_carrier;
bool use_old_nav;
bool multi_freq;
double time_window; // window of time in which we perform TDCP
+
+ // Batch TDCP params
+ bool batch; // precompute global displacement between 2 epochs
+ int min_common_sats;
+ int raim_n;
+ bool validate_residual;
+ double max_residual; // max allowed residual to be considered good solution. RAIM applied if enabled in this case.
+ bool relinearize_jacobian;
+ int max_iterations;
+ int residual_opt; // 0: Normalized RMS of residual vector. 1: Max residual in Mahalanobis squared distance
+ int sagnac_correction; // 0 deactivated, 1/2 substraction/addition
+};
+
+struct CarrierPhaseOptions
+{
+ bool corr_iono;
+ bool corr_tropo;
+ bool corr_clock;
};
struct Options
@@ -121,7 +134,7 @@ struct Options
double elmin; // min elevation (rad)
double maxgdop; // maxgdop: reject threshold of gdop
bool GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used
- TdcpOptions tdcp; // TDCP options
+ CarrierPhaseOptions carrier_opt; // Carrier phase correction options
// compute navsys int
int getNavSys() const
@@ -150,21 +163,6 @@ struct Options
}
};
-const TdcpOptions default_tdcp_options =
-{
- false, //bool enabled; // TDCP enabled
- false, //bool batch; // precompute global displacement between 2 epochs
- true, //bool corr_iono; // apply correction also in TDCP
- true, //bool corr_tropo; // apply correction also in TDCP
- true, //bool corr_clock;
- false, //bool loss_function; // apply loss function in TDCP factors
- 0.1, //double sigma_atm;
- 0.1, //double sigma_carrier;
- true, //bool use_old_nav;
- false, //bool multi_freq;
- 300 //double time_window; // window of time in which we perform TDCP
-};
-
const Options default_options =
{
EPHOPT_BRDC, // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
@@ -175,7 +173,7 @@ const Options default_options =
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
- default_tdcp_options // TDCP options
+ {true, true, true} // carrier phase options
};
// forward declarations
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 5b67886..6cd6cde 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -7,18 +7,6 @@
namespace GnssUtils
{
-struct TdcpBatchParams
-{
- TdcpOptions tdcp;
- int min_common_sats;
- int raim_n;
- bool validate_residual;
- double max_residual; // max allowed residual to be considered good solution. RAIM applied if enabled in this case.
- bool relinearize_jacobian;
- int max_iterations;
- int residual_opt; // 0: Normalized RMS of residual vector. 1: Max residual in Mahalanobis squared distance
- int sagnac_correction; // 0 deactivated, 1/2 substraction/addition
-};
struct TdcpOutput
{
@@ -36,7 +24,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
- const TdcpBatchParams& tdcp_params,
+ const TdcpOptions& tdcp_params,
const Options& opt);
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
@@ -44,7 +32,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
const Eigen::Vector3d& x_r,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
- const TdcpBatchParams& tdcp_params,
+ const TdcpOptions& tdcp_params,
const Options& opt);
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
@@ -53,7 +41,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
const std::set<int>& tracked_sats,
- const TdcpBatchParams& tdcp_params,
+ const TdcpOptions& tdcp_params,
const Options& opt);
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
@@ -61,7 +49,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
const Eigen::Vector3d& x_r,
const std::set<int>& common_sats,
const Eigen::Vector4d& d_0,
- const TdcpBatchParams& tdcp_params);
+ const TdcpOptions& tdcp_params);
TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::MatrixXd& A,
@@ -72,7 +60,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
const Eigen::Vector4d& d_0,
const double& var_tdcp,
std::set<int>& raim_discarded_rows,
- const TdcpBatchParams& tdcp_params);
+ const TdcpOptions& tdcp_params);
} // namespace GnssUtils
diff --git a/src/observations.cpp b/src/observations.cpp
index cb43c91..944da37 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -370,7 +370,7 @@ std::set<int> Observations::filter(const Satellites& sats,
opt.getNavSys(),
opt.getSnrMask(),
opt.elmin,
- opt.ionoopt == IONOOPT_IFLC or (opt.tdcp.enabled and opt.tdcp.multi_freq));
+ opt.ionoopt == IONOOPT_IFLC);
}
std::set<int> Observations::filter(const Satellites& sats,
@@ -413,7 +413,7 @@ std::set<int> Observations::filter(const Satellites& sats,
opt.getNavSys(),
opt.getSnrMask(),
opt.elmin,
- opt.ionoopt == IONOOPT_IFLC or (opt.tdcp.enabled and opt.tdcp.multi_freq));
+ opt.ionoopt == IONOOPT_IFLC);
}
std::set<int> Observations::filter(const Satellites& sats,
diff --git a/src/range.cpp b/src/range.cpp
index c490b7b..6ad98fd 100644
--- a/src/range.cpp
+++ b/src/range.cpp
@@ -119,7 +119,7 @@ Ranges Range::computeRanges(ObservationsPtr obs,
ranges[sat].L2_corrected = ranges[sat].L2;
/* ionospheric corrections */
- if (opt.tdcp.corr_iono)
+ if (opt.carrier_opt.corr_iono)
{
if (opt.ionoopt==IONOOPT_IFLC)
{
@@ -134,22 +134,22 @@ Ranges Range::computeRanges(ObservationsPtr obs,
}
/* tropospheric corrections */
- if (opt.tdcp.corr_tropo)
+ if (opt.carrier_opt.corr_tropo)
{
ranges[sat].L_corrected += ranges[sat].tropo_corr;
ranges[sat].L2_corrected += ranges[sat].tropo_corr; //FIXME: different?
}
/* sat clock corrections */
- if (opt.tdcp.corr_clock)
+ if (opt.carrier_opt.corr_clock)
{
ranges[sat].L_corrected += ranges[sat].sat_clock_corr;
ranges[sat].L2_corrected += ranges[sat].sat_clock_corr; //FIXME: different?
}
/* carrier phase variance */
- ranges[sat].L_var = opt.tdcp.sigma_carrier * opt.tdcp.sigma_carrier;
- ranges[sat].L2_var = opt.tdcp.sigma_carrier * opt.tdcp.sigma_carrier;
+ ranges[sat].L_var = 1.0; // FIXME
+ ranges[sat].L2_var = 1.0; // FIXME
//std::cout << std::endl
// << "\t\tprange: " << pranges[sat].prange << std::endl
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 5d5ff39..90e4e58 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -10,7 +10,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
- const TdcpBatchParams& tdcp_params,
+ const TdcpOptions& tdcp_params,
const Options& opt)
{
auto pos_output = computePos(*snapshot_r->getObservations(), *snapshot_r->getNavigation(), opt);
@@ -36,7 +36,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
const Eigen::Vector3d& x_r,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
- const TdcpBatchParams& tdcp_params,
+ const TdcpOptions& tdcp_params,
const Options& opt)
{
return Tdcp(snapshot_r,
@@ -55,12 +55,12 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
const std::set<int>& tracked_sats,
- const TdcpBatchParams& tdcp_params,
+ const TdcpOptions& tdcp_params,
const Options& opt)
{
// If use old nav temporary change navigation to (re)compute satellites positions
auto nav_k = snapshot_k->getNavigation();
- if (tdcp_params.tdcp.use_old_nav)
+ if (tdcp_params.use_old_nav)
{
auto new_snapshot_k = std::make_shared<Snapshot>(std::make_shared<Observations>(*snapshot_k->getObservations()),
std::make_shared<Navigation>(*snapshot_k->getNavigation()));
@@ -120,7 +120,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
const Eigen::Vector3d& x_r,
const std::set<int>& common_sats,
const Eigen::Vector4d& d_0,
- const TdcpBatchParams& tdcp_params)
+ const TdcpOptions& tdcp_params)
{
TdcpOutput output;
@@ -169,34 +169,16 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
auto&& range_k = snapshot_k->getRanges().at(sat);
// Carrier phase
- // corrected
- if (tdcp_params.tdcp.corr_iono or tdcp_params.tdcp.corr_tropo)
- {
- // L1/E1
- if (std::abs(range_r.L_corrected) > 1e-12 and std::abs(range_k.L_corrected) > 1e-12)
- row_2_sat_freq[row++] = std::make_pair(sat, 0);
-
- if (!tdcp_params.tdcp.multi_freq)
- continue;
-
- // L2 (GPS/GLO/QZS) / E5 (GAL)
- if (std::abs(range_r.L2_corrected) > 1e-12 and std::abs(range_k.L2_corrected) > 1e-12)
- row_2_sat_freq[row++] = std::make_pair(sat, 1);
- }
- // not corrected
- else
- {
- // L1/E1
- if (std::abs(range_r.L) > 1e-12 and std::abs(range_k.L) > 1e-12)
- row_2_sat_freq[row++] = std::make_pair(sat, 0);
+ // L1/E1
+ if (std::abs(range_r.L_corrected) > 1e-12 and std::abs(range_k.L_corrected) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 0);
- if (!tdcp_params.tdcp.multi_freq)
- continue;
+ if (!tdcp_params.multi_freq)
+ continue;
- // L2 (GPS/GLO/QZS) / E5 (GAL)
- if (std::abs(range_r.L2) > 1e-12 and std::abs(range_k.L2) > 1e-12)
- row_2_sat_freq[row++] = std::make_pair(sat, 1);
- }
+ // L2 (GPS/GLO/QZS) / E5 (GAL)
+ if (std::abs(range_r.L2_corrected) > 1e-12 and std::abs(range_k.L2_corrected) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 1);
}
int n_differences = row_2_sat_freq.size();
@@ -230,20 +212,10 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
s_k.col(row) = snapshot_k->getSatellites().at(sat_number).pos;
// time differenced carrier phase measurements
- if (tdcp_params.tdcp.corr_iono or tdcp_params.tdcp.corr_tropo)
- {
- if (freq == 0)
- drho_m(row) = range_k.L_corrected - range_r.L_corrected;
- else
- drho_m(row) = range_k.L2_corrected - range_r.L2_corrected;
- }
+ if (freq == 0)
+ drho_m(row) = range_k.L_corrected - range_r.L_corrected;
else
- {
- if (freq == 0)
- drho_m(row) = range_k.L - range_r.L;
- else
- drho_m(row) = range_k.L2 - range_r.L2;
- }
+ drho_m(row) = range_k.L2_corrected - range_r.L2_corrected;
// sagnac corrections
if (tdcp_params.sagnac_correction != 0)
@@ -263,16 +235,8 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
<< "\tpositions:" << std::endl
<< "\t\ts_r: " << s_r.col(row).transpose() << std::endl
<< "\t\ts_k: " << s_k.col(row).transpose() << std::endl;
- if (tdcp_params.tdcp.corr_iono or tdcp_params.tdcp.corr_tropo)
- {
- std::cout << "\trange_r.L: " << (freq == 0 ? range_r.L_corrected : range_r.L2_corrected) << std::endl;
- std::cout << "\trange_k.L: " << (freq == 0 ? range_k.L_corrected : range_k.L2_corrected) << std::endl;
- }
- else
- {
- std::cout << "\trange_r.L: " << (freq == 0 ? range_r.L : range_r.L2) << std::endl;
- std::cout << "\trange_k.L: " << (freq == 0 ? range_k.L : range_k.L2) << std::endl;
- }
+ std::cout << "\trange_r.L: " << (freq == 0 ? range_r.L_corrected : range_r.L2_corrected) << std::endl;
+ std::cout << "\trange_k.L: " << (freq == 0 ? range_k.L_corrected : range_k.L2_corrected) << std::endl;
std::cout << "\tdrho_m: " << drho_m(row) << std::endl;
#endif
@@ -289,8 +253,8 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
}
}
// Compute TDCP measurement noise variance (proportional to time)
- double var_tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
- 2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
+ double var_tdcp = (tk - tr) * tdcp_params.sigma_atm * tdcp_params.sigma_atm +
+ 2 * tdcp_params.sigma_carrier * tdcp_params.sigma_carrier;
// LEAST SQUARES SOLUTION =======================================================================
std::set<int> raim_discarded_rows;
@@ -317,7 +281,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
const Eigen::Vector4d& d_0,
const double& var_tdcp,
std::set<int>& raim_discarded_rows,
- const TdcpBatchParams& tdcp_params)
+ const TdcpOptions& tdcp_params)
{
assert(A.cols() == 4);
assert(s_k.rows() == 3);
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index 5bd4265..78811d8 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -68,7 +68,7 @@ void computeVisibleSatellite(const Vector3d& receiver_latlonalt,
TEST(Tdcp, Tdcp)
{
- TdcpBatchParams tdcp_params;
+ TdcpOptions tdcp_params;
tdcp_params.max_iterations = 5;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 0;
@@ -77,9 +77,9 @@ TEST(Tdcp, Tdcp)
tdcp_params.validate_residual = false;
tdcp_params.sagnac_correction = false;
tdcp_params.relinearize_jacobian = true;
- tdcp_params.tdcp.multi_freq = false;
- tdcp_params.tdcp.sigma_atm = 1;
- tdcp_params.tdcp.sigma_carrier = 1;
+ tdcp_params.multi_freq = false;
+ tdcp_params.sigma_atm = 1;
+ tdcp_params.sigma_carrier = 1;
Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
@@ -209,18 +209,18 @@ TEST(Tdcp, Tdcp_raim_residual_rmse)
{
int N_sats = 20;
- TdcpBatchParams tdcp_params;
+ TdcpOptions tdcp_params;
tdcp_params.max_iterations = 5;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 2;
- tdcp_params.relinearize_jacobian = true;
tdcp_params.residual_opt = 0; // Normalized RMSE
tdcp_params.max_residual = 0.1; // low threshold to detect outliers...
tdcp_params.validate_residual = false;
tdcp_params.sagnac_correction = false;
- tdcp_params.tdcp.multi_freq = false;
- tdcp_params.tdcp.sigma_atm = 1;
- tdcp_params.tdcp.sigma_carrier = 1;
+ tdcp_params.relinearize_jacobian = true;
+ tdcp_params.multi_freq = false;
+ tdcp_params.sigma_atm = 1;
+ tdcp_params.sigma_carrier = 1;
Vector3d sat_ENU, sat_ECEF;
@@ -366,18 +366,18 @@ TEST(Tdcp, Tdcp_raim_residual_max_mah)
{
int N_sats = 20;
- TdcpBatchParams tdcp_params;
+ TdcpOptions tdcp_params;
tdcp_params.max_iterations = 5;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 2;
- tdcp_params.relinearize_jacobian = true;
tdcp_params.residual_opt = 1; // Max residual in Mahalanobis distance
tdcp_params.max_residual = 3.84; // 95% of confidence
tdcp_params.validate_residual = false;
tdcp_params.sagnac_correction = false;
- tdcp_params.tdcp.multi_freq = false;
- tdcp_params.tdcp.sigma_atm = 1;
- tdcp_params.tdcp.sigma_carrier = 1;
+ tdcp_params.relinearize_jacobian = true;
+ tdcp_params.multi_freq = false;
+ tdcp_params.sigma_atm = 1;
+ tdcp_params.sigma_carrier = 1;
Vector3d sat_ENU, sat_ECEF;
--
GitLab