Skip to content
Snippets Groups Projects
Commit 8035c5b4 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

added GnssUtils::Options

parent f5a87d8c
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
......@@ -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
......@@ -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,
......
......@@ -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);
}
......
......@@ -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_ */
......@@ -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,
......
......@@ -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);
}
......
......@@ -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
......
......@@ -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();
......
......@@ -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)
// {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment