mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-10-31 15:23:04 +00:00
Adding consistency checks to the PVT solutions
This commit is contained in:
Javier Arribas
@@ -164,81 +164,87 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
|
||||
DLOG(INFO) << "satpos=" << satpos;
|
||||
DLOG(INFO) << "obs="<< obs;
|
||||
DLOG(INFO) << "W=" << W;
|
||||
try{
|
||||
// check if this is the initial position computation
|
||||
if (d_rx_dt_s == 0)
|
||||
{
|
||||
// execute Bancroft's algorithm to estimate initial receiver position and time
|
||||
DLOG(INFO) << " Executing Bancroft algorithm...";
|
||||
rx_position_and_time = bancroftPos(satpos.t(), obs);
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s = rx_position_and_time(3) / GALILEO_C_m_s; // save time for the next iteration [meters]->[seconds]
|
||||
}
|
||||
// Execute WLS using previous position as the initialization point
|
||||
rx_position_and_time = leastSquarePos(satpos, obs, W);
|
||||
|
||||
// check if this is the initial position computation
|
||||
if (d_rx_dt_s == 0)
|
||||
{
|
||||
// execute Bancroft's algorithm to estimate initial receiver position and time
|
||||
DLOG(INFO) << " Executing Bancroft algorithm...";
|
||||
rx_position_and_time = bancroftPos(satpos.t(), obs);
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s = rx_position_and_time(3) / GALILEO_C_m_s; // save time for the next iteration [meters]->[seconds]
|
||||
}
|
||||
// Execute WLS using previous position as the initialization point
|
||||
rx_position_and_time = leastSquarePos(satpos, obs, W);
|
||||
d_rx_dt_s += rx_position_and_time(3) / GALILEO_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
|
||||
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s += rx_position_and_time(3) / GALILEO_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
|
||||
// Compute Gregorian time
|
||||
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
|
||||
// get time string Gregorian calendar
|
||||
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
|
||||
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
d_position_UTC_time = p_time;
|
||||
|
||||
// Compute Gregorian time
|
||||
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
|
||||
// get time string Gregorian calendar
|
||||
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
|
||||
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
d_position_UTC_time = p_time;
|
||||
DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << rx_position_and_time;
|
||||
|
||||
DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << rx_position_and_time;
|
||||
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
|
||||
d_rx_dt_s = rx_position_and_time(3)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds
|
||||
DLOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
|
||||
d_rx_dt_s = rx_position_and_time(3)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds
|
||||
DLOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
// ###### Compute DOPs ########
|
||||
compute_DOP();
|
||||
|
||||
// ###### Compute DOPs ########
|
||||
compute_DOP();
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = galileo_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = rx_position_and_time(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = rx_position_and_time(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = rx_position_and_time(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = rx_position_and_time(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
|
||||
}
|
||||
}
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
// MOVING AVERAGE PVT
|
||||
galileo_e1_ls_pvt::pos_averaging(flag_averaging);
|
||||
}catch(const std::exception & e)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = galileo_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = rx_position_and_time(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = rx_position_and_time(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = rx_position_and_time(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = rx_position_and_time(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
|
||||
}
|
||||
d_rx_dt_s=0;//reset rx time estimation
|
||||
LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
|
||||
b_valid_position = false;
|
||||
}
|
||||
|
||||
// MOVING AVERAGE PVT
|
||||
galileo_e1_ls_pvt::pos_averaging(flag_averaging);
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@@ -49,21 +49,21 @@ gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels, std::string dump_filename, boo
|
||||
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_flag_dump_enabled == true)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
|
||||
}
|
||||
}
|
||||
try
|
||||
{
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -99,58 +99,58 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
||||
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
|
||||
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
|
||||
gnss_pseudoranges_iter++)
|
||||
{
|
||||
// 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
|
||||
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->first);
|
||||
if (gps_ephemeris_iter != gps_ephemeris_map.end())
|
||||
{
|
||||
// 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
|
||||
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->first);
|
||||
if (gps_ephemeris_iter != gps_ephemeris_map.end())
|
||||
{
|
||||
/*!
|
||||
* \todo Place here the satellite CN0 (power level, or weight factor)
|
||||
*/
|
||||
W.resize(valid_obs + 1, 1);
|
||||
W(valid_obs) = 1;
|
||||
/*!
|
||||
* \todo Place here the satellite CN0 (power level, or weight factor)
|
||||
*/
|
||||
W.resize(valid_obs + 1, 1);
|
||||
W(valid_obs) = 1;
|
||||
|
||||
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
|
||||
// first estimate of transmit time
|
||||
double Rx_time = GPS_current_time;
|
||||
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GPS_C_m_s;
|
||||
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
|
||||
// first estimate of transmit time
|
||||
double Rx_time = GPS_current_time;
|
||||
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GPS_C_m_s;
|
||||
|
||||
// 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect
|
||||
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
|
||||
// 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect
|
||||
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
|
||||
|
||||
// 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect
|
||||
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
|
||||
double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
|
||||
// 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect
|
||||
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
|
||||
double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
|
||||
|
||||
//store satellite positions in a matrix
|
||||
satpos.resize(3, valid_obs + 1);
|
||||
satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X;
|
||||
satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y;
|
||||
satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z;
|
||||
//store satellite positions in a matrix
|
||||
satpos.resize(3, valid_obs + 1);
|
||||
satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X;
|
||||
satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y;
|
||||
satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z;
|
||||
|
||||
// 4- fill the observations vector with the corrected pseudoranges
|
||||
obs.resize(valid_obs + 1, 1);
|
||||
obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + dtr * GPS_C_m_s - d_rx_dt_s * GPS_C_m_s;
|
||||
d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN;
|
||||
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
|
||||
// 4- fill the observations vector with the corrected pseudoranges
|
||||
obs.resize(valid_obs + 1, 1);
|
||||
obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + dtr * GPS_C_m_s - d_rx_dt_s * GPS_C_m_s;
|
||||
d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN;
|
||||
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
|
||||
|
||||
// SV ECEF DEBUG OUTPUT
|
||||
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
|
||||
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
|
||||
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
|
||||
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
|
||||
// SV ECEF DEBUG OUTPUT
|
||||
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
|
||||
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
|
||||
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
|
||||
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
|
||||
|
||||
valid_obs++;
|
||||
// compute the UTC time for this SV (just to print the associated UTC timestamp)
|
||||
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
|
||||
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first;
|
||||
}
|
||||
valid_obs++;
|
||||
// compute the UTC time for this SV (just to print the associated UTC timestamp)
|
||||
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
|
||||
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first;
|
||||
}
|
||||
}
|
||||
|
||||
// ********************************************************************************
|
||||
// ****** SOLVE LEAST SQUARES******************************************************
|
||||
@@ -159,21 +159,22 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
||||
LOG(INFO) << "(new)PVT: valid observations=" << valid_obs;
|
||||
|
||||
if (valid_obs >= 4)
|
||||
{
|
||||
arma::vec rx_position_and_time;
|
||||
DLOG(INFO) << "satpos=" << satpos;
|
||||
DLOG(INFO) << "obs=" << obs;
|
||||
DLOG(INFO) << "W=" << W;
|
||||
{
|
||||
arma::vec rx_position_and_time;
|
||||
DLOG(INFO) << "satpos=" << satpos;
|
||||
DLOG(INFO) << "obs=" << obs;
|
||||
DLOG(INFO) << "W=" << W;
|
||||
|
||||
try{
|
||||
// check if this is the initial position computation
|
||||
if (d_rx_dt_s == 0)
|
||||
{
|
||||
// execute Bancroft's algorithm to estimate initial receiver position and time
|
||||
DLOG(INFO) << " Executing Bancroft algorithm...";
|
||||
rx_position_and_time = bancroftPos(satpos.t(), obs);
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s = rx_position_and_time(3) / GPS_C_m_s; // save time for the next iteration [meters]->[seconds]
|
||||
}
|
||||
{
|
||||
// execute Bancroft's algorithm to estimate initial receiver position and time
|
||||
DLOG(INFO) << " Executing Bancroft algorithm...";
|
||||
rx_position_and_time = bancroftPos(satpos.t(), obs);
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s = rx_position_and_time(3) / GPS_C_m_s; // save time for the next iteration [meters]->[seconds]
|
||||
}
|
||||
|
||||
// Execute WLS using previous position as the initialization point
|
||||
rx_position_and_time = leastSquarePos(satpos, obs, W);
|
||||
@@ -193,57 +194,64 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
d_position_UTC_time = p_time;
|
||||
DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
// ###### Compute DOPs ########
|
||||
compute_DOP();
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = GPS_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = d_rx_pos(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = d_rx_pos(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = d_rx_pos(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = d_rx_dt_s;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
|
||||
}
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = GPS_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = d_rx_pos(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = d_rx_pos(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = d_rx_pos(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = d_rx_dt_s;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
|
||||
}
|
||||
}
|
||||
|
||||
// MOVING AVERAGE PVT
|
||||
pos_averaging(flag_averaging);
|
||||
}
|
||||
else
|
||||
|
||||
}catch(const std::exception & e)
|
||||
{
|
||||
d_rx_dt_s=0;//reset rx time estimation
|
||||
LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
|
||||
b_valid_position = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
b_valid_position = false;
|
||||
}
|
||||
return b_valid_position;
|
||||
}
|
||||
|
||||
|
||||
@@ -201,17 +201,17 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
||||
|
||||
// SV ECEF DEBUG OUTPUT
|
||||
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
|
||||
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
|
||||
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
|
||||
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
|
||||
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
|
||||
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
|
||||
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
|
||||
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
|
||||
|
||||
valid_obs++;
|
||||
// compute the UTC time for this SV (just to print the associated UTC timestamp)
|
||||
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
|
||||
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
|
||||
}
|
||||
@@ -288,9 +288,9 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
||||
DLOG(INFO) << "satpos=" << satpos;
|
||||
DLOG(INFO) << "obs=" << obs;
|
||||
DLOG(INFO) << "W=" << W;
|
||||
|
||||
// check if this is the initial position computation
|
||||
if (d_rx_dt_s == 0)
|
||||
try{
|
||||
// check if this is the initial position computation
|
||||
if (d_rx_dt_s == 0)
|
||||
{
|
||||
// execute Bancroft's algorithm to estimate initial receiver position and time
|
||||
DLOG(INFO) << " Executing Bancroft algorithm...";
|
||||
@@ -299,81 +299,87 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
||||
d_rx_dt_s = rx_position_and_time(3) / GPS_C_m_s; // save time for the next iteration [meters]->[seconds]
|
||||
}
|
||||
|
||||
// Execute WLS using previous position as the initialization point
|
||||
rx_position_and_time = leastSquarePos(satpos, obs, W);
|
||||
// Execute WLS using previous position as the initialization point
|
||||
rx_position_and_time = leastSquarePos(satpos, obs, W);
|
||||
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
|
||||
|
||||
DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
|
||||
DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
|
||||
DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
|
||||
DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
|
||||
|
||||
double secondsperweek = 604800.0;
|
||||
// Compute GST and Gregorian time
|
||||
if( GST != 0.0)
|
||||
{
|
||||
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
|
||||
}
|
||||
else
|
||||
{
|
||||
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
|
||||
}
|
||||
|
||||
// get time string Gregorian calendar
|
||||
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
|
||||
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
d_position_UTC_time = p_time;
|
||||
|
||||
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
|
||||
|
||||
DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
// ###### Compute DOPs ########
|
||||
hybrid_ls_pvt::compute_DOP();
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
double secondsperweek = 604800.0;
|
||||
// Compute GST and Gregorian time
|
||||
if( GST != 0.0)
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = hybrid_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = rx_position_and_time(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = rx_position_and_time(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = rx_position_and_time(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = rx_position_and_time(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
|
||||
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
|
||||
}
|
||||
}
|
||||
|
||||
// MOVING AVERAGE PVT
|
||||
pos_averaging(flag_averaging);
|
||||
// get time string Gregorian calendar
|
||||
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
|
||||
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
d_position_UTC_time = p_time;
|
||||
|
||||
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
|
||||
|
||||
DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
// ###### Compute DOPs ########
|
||||
hybrid_ls_pvt::compute_DOP();
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = hybrid_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = rx_position_and_time(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = rx_position_and_time(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = rx_position_and_time(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = rx_position_and_time(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
|
||||
}
|
||||
}
|
||||
|
||||
// MOVING AVERAGE PVT
|
||||
pos_averaging(flag_averaging);
|
||||
}catch(const std::exception & e)
|
||||
{
|
||||
d_rx_dt_s=0;//reset rx time estimation
|
||||
LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
|
||||
b_valid_position = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@@ -31,6 +31,7 @@
|
||||
|
||||
#include "ls_pvt.h"
|
||||
#include <exception>
|
||||
#include <stdexcept>
|
||||
#include "GPS_L1_CA.h"
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
@@ -81,80 +82,80 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
|
||||
arma::mat BBB;
|
||||
double traveltime = 0;
|
||||
for (int iter = 0; iter < 2; iter++)
|
||||
{
|
||||
B = B_pass;
|
||||
int m = arma::size(B,0);
|
||||
for (int i = 0; i < m; i++)
|
||||
{
|
||||
B = B_pass;
|
||||
int m = arma::size(B,0);
|
||||
for (int i = 0; i < m; i++)
|
||||
{
|
||||
int x = B(i,0);
|
||||
int y = B(i,1);
|
||||
if (iter == 0)
|
||||
{
|
||||
traveltime = 0.072;
|
||||
}
|
||||
else
|
||||
{
|
||||
int z = B(i,2);
|
||||
double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2));
|
||||
traveltime = sqrt(rho) / GPS_C_m_s;
|
||||
}
|
||||
double angle = traveltime * 7.292115147e-5;
|
||||
double cosa = cos(angle);
|
||||
double sina = sin(angle);
|
||||
B(i,0) = cosa * x + sina * y;
|
||||
B(i,1) = -sina * x + cosa * y;
|
||||
}// % i-loop
|
||||
|
||||
if (m > 3)
|
||||
{
|
||||
BBB = arma::inv(B.t() * B) * B.t();
|
||||
}
|
||||
int x = B(i,0);
|
||||
int y = B(i,1);
|
||||
if (iter == 0)
|
||||
{
|
||||
traveltime = 0.072;
|
||||
}
|
||||
else
|
||||
{
|
||||
BBB = arma::inv(B);
|
||||
}
|
||||
arma::vec e = arma::ones(m,1);
|
||||
arma::vec alpha = arma::zeros(m,1);
|
||||
for (int i = 0; i < m; i++)
|
||||
{
|
||||
alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0;
|
||||
}
|
||||
arma::mat BBBe = BBB * e;
|
||||
arma::mat BBBalpha = BBB * alpha;
|
||||
double a = lorentz(BBBe, BBBe);
|
||||
double b = lorentz(BBBe, BBBalpha) - 1;
|
||||
double c = lorentz(BBBalpha, BBBalpha);
|
||||
double root = sqrt(b * b - a * c);
|
||||
arma::vec r = {(-b - root) / a, (-b + root) / a};
|
||||
arma::mat possible_pos = arma::zeros(4,2);
|
||||
{
|
||||
int z = B(i,2);
|
||||
double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2));
|
||||
traveltime = sqrt(rho) / GPS_C_m_s;
|
||||
}
|
||||
double angle = traveltime * 7.292115147e-5;
|
||||
double cosa = cos(angle);
|
||||
double sina = sin(angle);
|
||||
B(i,0) = cosa * x + sina * y;
|
||||
B(i,1) = -sina * x + cosa * y;
|
||||
}// % i-loop
|
||||
|
||||
if (m > 3)
|
||||
{
|
||||
BBB = arma::inv(B.t() * B) * B.t();
|
||||
}
|
||||
else
|
||||
{
|
||||
BBB = arma::inv(B);
|
||||
}
|
||||
arma::vec e = arma::ones(m,1);
|
||||
arma::vec alpha = arma::zeros(m,1);
|
||||
for (int i = 0; i < m; i++)
|
||||
{
|
||||
alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0;
|
||||
}
|
||||
arma::mat BBBe = BBB * e;
|
||||
arma::mat BBBalpha = BBB * alpha;
|
||||
double a = lorentz(BBBe, BBBe);
|
||||
double b = lorentz(BBBe, BBBalpha) - 1;
|
||||
double c = lorentz(BBBalpha, BBBalpha);
|
||||
double root = sqrt(b * b - a * c);
|
||||
arma::vec r = {(-b - root) / a, (-b + root) / a};
|
||||
arma::mat possible_pos = arma::zeros(4,2);
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
possible_pos.col(i) = r(i) * BBBe + BBBalpha;
|
||||
possible_pos(3,i) = -possible_pos(3,i);
|
||||
}
|
||||
|
||||
arma::vec abs_omc = arma::zeros(2,1);
|
||||
for (int j = 0; j < m; j++)
|
||||
{
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
possible_pos.col(i) = r(i) * BBBe + BBBalpha;
|
||||
possible_pos(3,i) = -possible_pos(3,i);
|
||||
}
|
||||
{
|
||||
double c_dt = possible_pos(3,i);
|
||||
double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0,2)) + c_dt;
|
||||
double omc = obs(j) - calc;
|
||||
abs_omc(i) = std::abs(omc);
|
||||
}
|
||||
}// % j-loop
|
||||
|
||||
arma::vec abs_omc = arma::zeros(2,1);
|
||||
for (int j = 0; j < m; j++)
|
||||
{
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
double c_dt = possible_pos(3,i);
|
||||
double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0,2)) + c_dt;
|
||||
double omc = obs(j) - calc;
|
||||
abs_omc(i) = std::abs(omc);
|
||||
}
|
||||
}// % j-loop
|
||||
|
||||
//discrimination between roots
|
||||
if (abs_omc(0) > abs_omc(1))
|
||||
{
|
||||
pos = possible_pos.col(1);
|
||||
}
|
||||
else
|
||||
{
|
||||
pos = possible_pos.col(0);
|
||||
}
|
||||
}// % iter loop
|
||||
//discrimination between roots
|
||||
if (abs_omc(0) > abs_omc(1))
|
||||
{
|
||||
pos = possible_pos.col(1);
|
||||
}
|
||||
else
|
||||
{
|
||||
pos = possible_pos.col(0);
|
||||
}
|
||||
}// % iter loop
|
||||
return pos;
|
||||
}
|
||||
|
||||
@@ -217,83 +218,82 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
|
||||
|
||||
//=== Iteratively find receiver position ===================================
|
||||
for (int iter = 0; iter < nmbOfIterations; iter++)
|
||||
{
|
||||
for (int i = 0; i < nmbOfSatellites; i++)
|
||||
{
|
||||
for (int i = 0; i < nmbOfSatellites; i++)
|
||||
{
|
||||
if (iter == 0)
|
||||
{
|
||||
//--- Initialize variables at the first iteration --------------
|
||||
Rot_X = X.col(i); //Armadillo
|
||||
trop = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
//--- Update equations -----------------------------------------
|
||||
rho2 = (X(0, i) - pos(0)) *
|
||||
(X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
|
||||
(X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
|
||||
(X(2, i) - pos(2));
|
||||
traveltime = sqrt(rho2) / GPS_C_m_s;
|
||||
|
||||
//--- Correct satellite position (do to earth rotation) --------
|
||||
Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
|
||||
|
||||
//--- Find DOA and range of satellites
|
||||
Ls_Pvt::topocent(&d_visible_satellites_Az[i],
|
||||
&d_visible_satellites_El[i],
|
||||
&d_visible_satellites_Distance[i],
|
||||
pos.subvec(0,2),
|
||||
Rot_X - pos.subvec(0, 2));
|
||||
if(traveltime < 0.1 && nmbOfSatellites > 3)
|
||||
{
|
||||
//--- Find receiver's height
|
||||
Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
|
||||
// Add troposphere correction if the receiver is below the troposphere
|
||||
if (h > 15000)
|
||||
{
|
||||
//receiver is above the troposphere
|
||||
trop = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
//--- Find delay due to troposphere (in meters)
|
||||
Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
|
||||
if(trop > 5.0 ) trop = 0.0; //check for erratic values
|
||||
}
|
||||
}
|
||||
}
|
||||
//--- Apply the corrections ----------------------------------------
|
||||
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
|
||||
|
||||
//--- Construct the A matrix ---------------------------------------
|
||||
//Armadillo
|
||||
A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
|
||||
A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
|
||||
A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
|
||||
A(i,3) = 1.0;
|
||||
}
|
||||
|
||||
//--- Find position update ---------------------------------------------
|
||||
x = arma::solve(w*A, w*omc); // Armadillo
|
||||
|
||||
//--- Apply position update --------------------------------------------
|
||||
pos = pos + x;
|
||||
if (arma::norm(x,2) < 1e-4)
|
||||
if (iter == 0)
|
||||
{
|
||||
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
|
||||
//--- Initialize variables at the first iteration --------------
|
||||
Rot_X = X.col(i); //Armadillo
|
||||
trop = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
//--- Update equations -----------------------------------------
|
||||
rho2 = (X(0, i) - pos(0)) *
|
||||
(X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
|
||||
(X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
|
||||
(X(2, i) - pos(2));
|
||||
traveltime = sqrt(rho2) / GPS_C_m_s;
|
||||
|
||||
//--- Correct satellite position (do to earth rotation) --------
|
||||
Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
|
||||
|
||||
//--- Find DOA and range of satellites
|
||||
Ls_Pvt::topocent(&d_visible_satellites_Az[i],
|
||||
&d_visible_satellites_El[i],
|
||||
&d_visible_satellites_Distance[i],
|
||||
pos.subvec(0,2),
|
||||
Rot_X - pos.subvec(0, 2));
|
||||
if(traveltime < 0.1 && nmbOfSatellites > 3)
|
||||
{
|
||||
//--- Find receiver's height
|
||||
Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
|
||||
// Add troposphere correction if the receiver is below the troposphere
|
||||
if (h > 15000)
|
||||
{
|
||||
//receiver is above the troposphere
|
||||
trop = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
//--- Find delay due to troposphere (in meters)
|
||||
Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
|
||||
if(trop > 5.0 ) trop = 0.0; //check for erratic values
|
||||
}
|
||||
}
|
||||
}
|
||||
//--- Apply the corrections ----------------------------------------
|
||||
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
|
||||
|
||||
//--- Construct the A matrix ---------------------------------------
|
||||
//Armadillo
|
||||
A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
|
||||
A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
|
||||
A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
|
||||
A(i,3) = 1.0;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
//-- compute the Dilution Of Precision values
|
||||
d_Q = arma::inv(arma::htrans(A) * A);
|
||||
}
|
||||
catch(std::exception& e)
|
||||
{
|
||||
d_Q = arma::zeros(4,4);
|
||||
//--- Find position update ---------------------------------------------
|
||||
x = arma::solve(w*A, w*omc); // Armadillo
|
||||
|
||||
//--- Apply position update --------------------------------------------
|
||||
pos = pos + x;
|
||||
if (arma::norm(x,2) < 1e-4)
|
||||
{
|
||||
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
|
||||
}
|
||||
}
|
||||
|
||||
//-- compute the Dilution Of Precision values
|
||||
d_Q = arma::inv(arma::htrans(A) * A);
|
||||
|
||||
// check the consistency of the PVT solution
|
||||
if (((fabs(pos(3))*1000.0)/ GPS_C_m_s)>GPS_STARTOFFSET_ms*2)
|
||||
{
|
||||
LOG(WARNING)<<"Receiver time offset out of range! Estimated RX Time error [s]:"<<pos(3)/ GPS_C_m_s;
|
||||
throw std::runtime_error("Receiver time offset out of range!");
|
||||
}
|
||||
return pos;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user