From 8035c5b40817a35999cbc62ca088cf8fbff1df59 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 22 May 2020 10:41:06 +0200
Subject: [PATCH] added GnssUtils::Options
---
include/gnss_utils/gnss_utils.h | 112 ++++++++++++++++++------
include/gnss_utils/observations.h | 4 +-
include/gnss_utils/snapshot.h | 10 +--
include/gnss_utils/tdcp.h | 53 +++++------
include/gnss_utils/utils/rcv_position.h | 2 +-
src/observations.cpp | 12 +--
src/snapshot.cpp | 9 +-
src/tdcp.cpp | 106 ++++++++++------------
src/utils/rcv_position.cpp | 15 ++--
9 files changed, 184 insertions(+), 139 deletions(-)
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index b795d2d..8fc1dd9 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -38,6 +38,7 @@ struct PseudoRange
double sat_clock_corr = 0;
double L = -1;
double carrier_range = -1;
+ double carrier_range_var = 1;
};
struct ComputePosOutput
@@ -64,34 +65,6 @@ struct ComputePosOutput
std::map<int,double> prange_residuals; // residuals of used pseudoranges (applying RAIM if the case and discarding wrong data)
};
-// forward declarations
-class Observations;
-class Navigation;
-class Snapshot;
-struct Satellite;
-
-// Typedefs
-typedef std::map<int,Satellite> Satellites;
-typedef std::map<int,PseudoRange> PseudoRanges;
-
-// pointer typedefs
-typedef std::shared_ptr<Observations> ObservationsPtr;
-typedef std::shared_ptr<const Observations> ObservationsConstPtr;
-
-typedef std::shared_ptr<Navigation> NavigationPtr;
-typedef std::shared_ptr<const Navigation> NavigationConstPtr;
-
-typedef std::shared_ptr<Snapshot> SnapshotPtr;
-typedef std::shared_ptr<const Snapshot> SnapshotConstPtr;
-
-enum Combination
-{
- CODE_L1, ///< only L1 code
- CARRIER_L1, ///< only L1 carrier phase
- CODE_IONO_FREE, ///< iono-free combination for code
- CARRIER_IONO_FREE ///< iono-free combination for carrier phase
-};
-
/* defaults processing options */
const prcopt_t opt_default = {
PMODE_SINGLE, /* mode: positioning mode (PMODE_???) */
@@ -136,6 +109,89 @@ const prcopt_t opt_default = {
{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
+class Observations;
+class Navigation;
+class Snapshot;
+struct Satellite;
+
+// Typedefs
+typedef std::map<int,Satellite> Satellites;
+typedef std::map<int,PseudoRange> PseudoRanges;
+
+// pointer typedefs
+typedef std::shared_ptr<Observations> ObservationsPtr;
+typedef std::shared_ptr<const Observations> ObservationsConstPtr;
+
+typedef std::shared_ptr<Navigation> NavigationPtr;
+typedef std::shared_ptr<const Navigation> NavigationConstPtr;
+
+typedef std::shared_ptr<Snapshot> SnapshotPtr;
+typedef std::shared_ptr<const Snapshot> SnapshotConstPtr;
+
+enum Combination
+{
+ CODE_L1, ///< only L1 code
+ CARRIER_L1, ///< only L1 carrier phase
+ CODE_IONO_FREE, ///< iono-free combination for code
+ 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 f4abefb..92a03fe 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -54,7 +54,7 @@ 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> filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
@@ -68,7 +68,7 @@ public:
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,
diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h
index ca8bf12..04e2d6d 100644
--- a/include/gnss_utils/snapshot.h
+++ b/include/gnss_utils/snapshot.h
@@ -42,12 +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 prcopt_t &opt);
+ const Options &opt);
// Pseudo-ranges
PseudoRanges& getPseudoRanges();
@@ -55,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();
@@ -119,7 +119,7 @@ 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);
}
@@ -128,7 +128,7 @@ inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded
const std::map<int,Eigen::Vector2d>& azels,
const bool &check_code,
const bool &check_carrier_phase,
- const prcopt_t &opt)
+ const Options &opt)
{
return obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt);
}
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 45504e0..f5f8a82 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 9ac529c..962d0ba 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/observations.cpp b/src/observations.cpp
index 4569585..cb77c68 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -388,15 +388,15 @@ 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.getNavSys(),
+ opt.getSnrMask(),
opt.elmin);
}
@@ -458,15 +458,15 @@ 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.getNavSys(),
+ opt.getSnrMask(),
opt.elmin);
}
diff --git a/src/snapshot.cpp b/src/snapshot.cpp
index 355cfb4..5ff3285 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";
@@ -58,7 +59,7 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel,
/* psudorange with code bias correction */
//std::cout << "prange...\n";
- if ((P=prange(&obs_i,&nav_->getNavigation(),azel_i,1,&opt,&vmeas))==0.0)
+ if ((P=prange(&obs_i,&nav_->getNavigation(),azel_i,1,&prcopt,&vmeas))==0.0)
{
//std::cout << " error in prange\n";
continue;
@@ -119,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 cd1b82c..9207f4c 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 fb69311..4e3501e 100644
--- a/src/utils/rcv_position.cpp
+++ b/src/utils/rcv_position.cpp
@@ -13,27 +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(),
sats_status,
msg);
+ // Fill output struct
if (output.pos_stat == 0)
std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
@@ -80,7 +85,7 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
// ComputePosOutput computePosOwn(const GnssUtils::Observations & _observations,
// GnssUtils::Navigation & _navigation,
-// const prcopt_t & _prcopt)
+// const prcopt_t & prcopt)
// {
// // Remove duplicated satellites
@@ -95,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)
// {
--
GitLab