mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-16 23:04:57 +00:00
Code Issues Releases Wiki Activity

Avoid extrapolation

Browse Source
This commit is contained in:
Antonio Ramos 2018-02-22 11:14:57 +01:00
parent df5f51ba91
commit ca9a551a86
2 changed files with 40 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -58,7 +58,6 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned
{ {
set_max_noutput_items(1); set_max_noutput_items(1);
d_dump = dump; d_dump = dump;
set_T_rx_s = false;
d_nchannels = nchannels_out; d_nchannels = nchannels_out;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
d_dump_filename_in = d_dump_filename; d_dump_filename_in = d_dump_filename;
@ -376,7 +375,7 @@ void hybrid_observables_cc::clean_history(std::deque<Gnss_Synchro>& data)
} }
} }
std::pair<Gnss_Synchro, Gnss_Synchro> hybrid_observables_cc::find_closest(std::deque<Gnss_Synchro>& data) std::pair<Gnss_Synchro, Gnss_Synchro> hybrid_observables_cc::find_closest(std::deque<Gnss_Synchro>& data, const double& ti)
{ {
std::pair<Gnss_Synchro, Gnss_Synchro> result; std::pair<Gnss_Synchro, Gnss_Synchro> result;
unsigned int index = 0; unsigned int index = 0;
@ -385,7 +384,7 @@ std::pair<Gnss_Synchro, Gnss_Synchro> hybrid_observables_cc::find_closest(std::d
unsigned int aux = 0; unsigned int aux = 0;
for(it = data.begin(); it != data.end(); it++) for(it = data.begin(); it != data.end(); it++)
{ {
double instant_delta = std::fabs(T_rx_s - it->RX_time); double instant_delta = std::fabs(ti - it->RX_time);
if(instant_delta < delta_t) if(instant_delta < delta_t)
{ {
delta_t = instant_delta; delta_t = instant_delta;
@ -393,50 +392,31 @@ std::pair<Gnss_Synchro, Gnss_Synchro> hybrid_observables_cc::find_closest(std::d
} }
aux++; aux++;
} }
try delta_t = ti - data.at(index).RX_time;
if( (index == 0) or (index == (data.size() - 1)) )
{ {
delta_t = T_rx_s - data.at(index).RX_time; Gnss_Synchro invalid_data;
if(index == 0) invalid_data.Flag_valid_pseudorange = false;
{ result.first = invalid_data;
result.first = data.at(1); result.second = invalid_data;
result.second = data.at(0);
}
else if((index == (data.size() - 1)) or (delta_t < 0.0))
{
result.first = data.at(index);
result.second = data.at(index - 1);
}
else
{
result.first = data.at(index + 1);
result.second = data.at(index);
}
} }
catch(const std::exception& e) else if(delta_t < 0.0)
{ {
result.first = Gnss_Synchro(); result.first = data.at(index);
result.second = Gnss_Synchro(); result.first.Flag_valid_pseudorange = true;
LOG(WARNING) << "Exception computing observables " << e.what(); result.second = data.at(index - 1);
result.second.Flag_valid_pseudorange = true;
}
else
{
result.first = data.at(index + 1);
result.first.Flag_valid_pseudorange = true;
result.second = data.at(index);
result.second.Flag_valid_pseudorange = true;
} }
return result; return result;
} }
double hybrid_observables_cc::find_min_RX_time()
{
if(d_num_valid_channels == 0) { return 0.0; }
std::vector<std::deque<Gnss_Synchro>>::iterator it = d_gnss_synchro_history.begin();
double result = std::numeric_limits<double>::max();
for(unsigned int i = 0; i < d_nchannels; i++)
{
if(valid_channels[i])
{
if(it->front().RX_time < result) { result = it->front().RX_time; }
}
it++;
}
return(floor(result * 1000.0) / 1000.0);
}
void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data) void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data)
{ {
@ -466,16 +446,15 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
int total_input_items = 0; int total_input_items = 0;
for(i = 0; i < d_nchannels; i++) { total_input_items += ninput_items[i]; } for(i = 0; i < d_nchannels; i++) { total_input_items += ninput_items[i]; }
consume(d_nchannels, 1); consume(d_nchannels, 1);
T_rx_s += T_rx_step_s;
////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////
if((total_input_items == 0) and (d_num_valid_channels == 0)) if((total_input_items == 0) and (d_num_valid_channels == 0))
{ {
return 0; return 0;
} }
if(set_T_rx_s) { T_rx_s += T_rx_step_s; }
////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////
std::vector<std::deque<Gnss_Synchro>>::iterator it; std::vector<std::deque<Gnss_Synchro>>::iterator it;
if (total_input_items > 0) if (total_input_items > 0)
{ {
@ -498,7 +477,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
{ {
if(it->front().PRN != it->back().PRN) { it->clear(); __dump = false; } if(it->front().PRN != it->back().PRN) { it->clear(); __dump = false; }
} }
if(d_dump && __dump) if(d_dump and __dump)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
try try
@ -529,17 +508,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
} }
d_num_valid_channels = valid_channels.count(); d_num_valid_channels = valid_channels.count();
// Check if there is any valid channel after reading the new incoming Gnss_Synchro data // Check if there is any valid channel after reading the new incoming Gnss_Synchro data
if(d_num_valid_channels == 0) if(d_num_valid_channels == 0) { return 0; }
{
set_T_rx_s = false;
return 0;
}
if(!set_T_rx_s) //Find the lowest RX_time among the valid observables in the history
{
T_rx_s = find_min_RX_time();
set_T_rx_s = true;
}
for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold
{ {
@ -552,11 +521,8 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
// Check if there is any valid channel after computing the time distance between the Gnss_Synchro data and the receiver time // Check if there is any valid channel after computing the time distance between the Gnss_Synchro data and the receiver time
d_num_valid_channels = valid_channels.count(); d_num_valid_channels = valid_channels.count();
if(d_num_valid_channels == 0) double T_rx_s_out = T_rx_s - (max_delta / 2.0);
{ if((d_num_valid_channels == 0) or (T_rx_s_out < 0.0)) { return 0; }
set_T_rx_s = false;
return 0;
}
std::vector<Gnss_Synchro> epoch_data; std::vector<Gnss_Synchro> epoch_data;
i = 0; i = 0;
@ -564,14 +530,17 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
{ {
if(valid_channels[i]) if(valid_channels[i])
{ {
std::pair<Gnss_Synchro, Gnss_Synchro> gnss_pair = find_closest(*it); std::pair<Gnss_Synchro, Gnss_Synchro> gnss_pair = find_closest(*it, T_rx_s_out);
Gnss_Synchro interpolated_gnss_synchro = gnss_pair.second; Gnss_Synchro interpolated_gnss_synchro = gnss_pair.second;
if(interpolated_gnss_synchro.Flag_valid_pseudorange)
{
interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s_out, 0);
interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s_out, 1);
interpolated_gnss_synchro.TOW_at_current_symbol_s = interpolate_data(gnss_pair, T_rx_s_out, 2);
interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s, 0); epoch_data.push_back(interpolated_gnss_synchro);
interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s, 1); }
interpolated_gnss_synchro.TOW_at_current_symbol_s = interpolate_data(gnss_pair, T_rx_s, 2); else { valid_channels[i] = false; }
epoch_data.push_back(interpolated_gnss_synchro);
} }
i++; i++;
} }

9
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
View File

@ -68,8 +68,7 @@ private:
void clean_history(std::deque<Gnss_Synchro>& data); void clean_history(std::deque<Gnss_Synchro>& data);
double compute_T_rx_s(const Gnss_Synchro& a); double compute_T_rx_s(const Gnss_Synchro& a);
double interpolate_data(const std::pair<Gnss_Synchro, Gnss_Synchro>& a, const double& ti, int parameter); double interpolate_data(const std::pair<Gnss_Synchro, Gnss_Synchro>& a, const double& ti, int parameter);
double find_min_RX_time(); std::pair<Gnss_Synchro, Gnss_Synchro> find_closest(std::deque<Gnss_Synchro>& data, const double& ti);
std::pair<Gnss_Synchro, Gnss_Synchro> find_closest(std::deque<Gnss_Synchro>& data);
void correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data); void correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data);
//Tracking observable history //Tracking observable history
@ -79,7 +78,6 @@ private:
double T_rx_step_s; double T_rx_step_s;
double max_delta; double max_delta;
bool d_dump; bool d_dump;
bool set_T_rx_s;
unsigned int d_nchannels; unsigned int d_nchannels;
unsigned int d_num_valid_channels; unsigned int d_num_valid_channels;
std::string d_dump_filename; std::string d_dump_filename;
@ -87,6 +85,11 @@ private:
std::ofstream d_dump_file; std::ofstream d_dump_file;
std::ofstream d_dump_in; std::ofstream d_dump_in;
std::string text_red = "\033[31m";
std::string text_green = "\033[32m";
std::string text_reset = "\033[0m";
int save_matfile(); int save_matfile();
}; };