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

wip gauss

parent 428d6382
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
......@@ -21,12 +21,15 @@ 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;
};
struct ComputePosOutput
......@@ -48,6 +51,9 @@ 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)
};
// forward declarations
......@@ -78,6 +84,50 @@ enum Combination
CARRIER_IONO_FREE ///< iono-free combination for carrier phase
};
/* 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 */
};
}
#endif
......@@ -43,6 +43,11 @@ public:
const bool &check_code,
const bool &check_carrier_phase,
const prcopt_t &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);
// Pseudo-ranges
PseudoRanges& getPseudoRanges();
......@@ -119,6 +124,15 @@ inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded
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 prcopt_t &opt)
{
return obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt);
}
inline const PseudoRanges& Snapshot::getPseudoRanges() const
{
return pranges_;
......
......@@ -57,7 +57,7 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel,
pranges_.emplace(sat,PseudoRange());
/* psudorange with code bias correction */
std::cout << "prange...\n";
//std::cout << "prange...\n";
if ((P=prange(&obs_i,&nav_->getNavigation(),azel_i,1,&opt,&vmeas))==0.0)
{
//std::cout << " error in prange\n";
......@@ -65,7 +65,7 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel,
}
/* 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 +77,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 +105,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;
......
......@@ -22,6 +22,7 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
sol_t sol;
sol = { { 0 } };
ssat_t sats_status[MAXSAT];
std::vector<double> sat_elevations(2 * _observations.size());
output.pos_stat = pntpos(_observations.data(),
......@@ -30,7 +31,7 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
&_prcopt,
&sol,
sat_elevations.data(),
NULL,
sats_status,
msg);
if (output.pos_stat == 0)
......@@ -59,6 +60,21 @@ 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;
}
......
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