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

satellite pos using SBAS also using BRDC for the rest

parent 08598be6
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
......@@ -35,6 +35,7 @@ namespace GnssUtils
double clock_bias;
double clock_drift;
int svh;
int eph_mode;
};
} // namespace GnssUtils
......
......@@ -16,69 +16,121 @@ namespace GnssUtils
{
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
{
// ecef 2 geodetic
Eigen::Vector3d receiver_geo;
ecef2pos(receiver_ecef.data(), receiver_geo.data());
// receiver-sat vector ecef
Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;
// receiver-sat vector enu (receiver ecef as origin)
Eigen::Vector3d receiver_sat_enu;
ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z}
receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
// elevation
return atan2(receiver_sat_enu(2),
sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
// ecef 2 geodetic
Eigen::Vector3d receiver_geo;
ecef2pos(receiver_ecef.data(), receiver_geo.data());
// receiver-sat vector ecef
Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;
// receiver-sat vector enu (receiver ecef as origin)
Eigen::Vector3d receiver_sat_enu;
ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z}
receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
// elevation
return atan2(receiver_sat_enu(2),
sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
}
Satellites computeSatellites(const Observations& obs,
const Navigation& nav,
const int& eph_opt)
{
double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
int svh[obs.size()];
// std::cout << "computing position of sats: ";
// for (auto&& obs_ref : obs.getObservations())
// std::cout << (int)obs_ref.sat << " ";
// std::cout << std::endl;
// compute positions
satposs(
obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), eph_opt, rs, dts, var, svh);
// fill Satellites
Satellites sats;
//std::cout << "filling satellites: \n";
for (int i = 0; i < obs.size(); i++)
{
auto sat_pair = sats.emplace(obs.getObservationByIdx(i).sat, // Key
Satellite({satsys(obs.getObservationByIdx(i).sat,NULL),
obs.getObservationByIdx(i).sat, // Constructor...
(Eigen::Vector3d() << rs[6*i], rs[6*i+1], rs[6*i+2]).finished(),
(Eigen::Vector3d() << rs[6*i+3], rs[6*i+4], rs[6*i+5]).finished(),
var[i],
dts[2*i],
dts[2*i+1],
(svh[i]==-2 ? 0 : svh[i])})); // -2 = ephemeris without SBAS correction (remove negative error code)
assert(sat_pair.second && "satellite already computed");
//std::cout << "\tsat: " << sat_pair.first->second.sat << std::endl
// << "\t\tpos: " << sat_pair.first->second.pos.transpose() << std::endl
// << "\t\tvel: " << sat_pair.first->second.vel.transpose() << std::endl
// << "\t\tvar: " << sat_pair.first->second.var << std::endl
// << "\t\tclock bias: " << sat_pair.first->second.clock_bias << std::endl
// << "\t\tclock drift: " << sat_pair.first->second.clock_drift << std::endl
// << "\t\tsvh: " << sat_pair.first->second.svh << std::endl;
// if (sat_pair.first->second.pos_ == Eigen::Vector3d::Zero())
// std::cout << "ephemeris not available for sat " << sat_pair.first->second.sat_ << std::endl;
}
return sats;
double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
double rs_brdc[6 * obs.size()], dts_brdc[2 * obs.size()], var_brdc[obs.size()];
int svh[obs.size()];
int svh_brdc[obs.size()];
// std::cout << "computing position of sats: ";
// for (auto&& obs_ref : obs.getObservations())
// std::cout << (int)obs_ref.sat << " ";
// std::cout << std::endl;
// compute positions
// first compute all broadcasted ephemeris
satposs(obs.getObservations().front().time,
obs.data(),
obs.size(),
&nav.getNavigation(),
0,
rs_brdc,
dts_brdc,
var_brdc,
svh_brdc);
/* DEBUG
std::cout << "Sats positions with broadcasted ephemeris: \n";
for (int i = 0; i < obs.size(); i++)
{
if (rs_brdc[6*i] == 0 and rs_brdc[6*i+1] == 0 and rs_brdc[6*i+2] == 0)
std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << " ephemeris not available" << std::endl;
else
{
std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << std::endl
<< "\t\tpos: " << rs_brdc[6*i] << " " << rs_brdc[6*i+1] << " " << rs_brdc[6*i+2] << std::endl
<< "\t\tvel: " << rs_brdc[6*i+3] << " " << rs_brdc[6*i+4] << " " << rs_brdc[6*i+5] << std::endl
<< "\t\tvar: " << var_brdc[i] << std::endl
<< "\t\tclock bias: " << dts_brdc[2*i] << std::endl
<< "\t\tclock drift: " << dts_brdc[2*i+1] << std::endl
<< "\t\tsvh: " << svh_brdc[i] << std::endl;
}
}
//*/
// compute SBAS ephemeris
if (eph_opt != 0)
satposs(obs.getObservations().front().time,
obs.data(),
obs.size(),
&nav.getNavigation(),
eph_opt,
rs,
dts,
var,
svh);
// fill Satellites
Satellites sats;
//std::cout << "computeSatellites: filling satellites: \n";
for (int i = 0; i < obs.size(); i++)
{
bool brdc_eph = eph_opt == 0 or (rs[6*i] == 0 and rs[6*i+1] == 0 and rs[6*i+2] == 0);
auto sat_pair = sats.emplace(obs.getObservationByIdx(i).sat, // Key
Satellite({satsys(obs.getObservationByIdx(i).sat,NULL),
obs.getObservationByIdx(i).sat, // Constructor...
brdc_eph ?
(Eigen::Vector3d() << rs_brdc[6*i],rs_brdc[6*i+1],rs_brdc[6*i+2]).finished() :
(Eigen::Vector3d() << rs[6*i],rs[6*i+1],rs[6*i+2]).finished(),
brdc_eph ?
(Eigen::Vector3d() << rs_brdc[6*i+3], rs_brdc[6*i+4], rs_brdc[6*i+5]).finished() :
(Eigen::Vector3d() << rs[6*i+3], rs[6*i+4], rs[6*i+5]).finished() ,
brdc_eph ? var_brdc[i] : var[i],
brdc_eph ? dts_brdc[2*i] : dts[2*i],
brdc_eph ? dts_brdc[2*i+1] : dts[2*i+1],
brdc_eph ? svh_brdc[i] : svh[i],
brdc_eph ? 0 : 2}));
assert(sat_pair.second && "satellite already computed");
/* DEBUG
if (sat_pair.first->second.pos == Eigen::Vector3d::Zero())
std::cout << "\tsat: " << sat_pair.first->second.sat << " ephemeris not available" << std::endl;
else
{
std::cout << "\tsat: " << sat_pair.first->second.sat << std::endl
<< "\t\tpos: " << sat_pair.first->second.pos.transpose() << std::endl
<< "\t\tvel: " << sat_pair.first->second.vel.transpose() << std::endl
<< "\t\tvar: " << sat_pair.first->second.var << std::endl
<< "\t\tclock bias: " << sat_pair.first->second.clock_bias << std::endl
<< "\t\tclock drift: " << sat_pair.first->second.clock_drift << std::endl
<< "\t\tsvh: " << sat_pair.first->second.svh << std::endl
<< "\t\teph: " << (sat_pair.first->second.eph_mode == 2 ? "SBAS" : "BRDC") << std::endl;
}//*/
}
return sats;
}
} // namespace GnssUtils
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