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

MOD: debug kf_tracking

Browse Source
This commit is contained in:
M.A.Gomez 2022-12-17 18:47:24 +00:00
parent 2a182bfbad
commit aa9608b11a
2 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/algorithms/PVT/libs/vtl_engine.cc
View File

@ -232,6 +232,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
kf_yerr = kf_H * kf_xerr; kf_yerr = kf_H * kf_xerr;
// Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): // Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
TrackingCmd trk_cmd;
for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector
{ {
rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered
@ -243,7 +245,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
//TODO: Fill the tracking commands outputs //TODO: Fill the tracking commands outputs
// Notice: keep the same satellite order as in the Vtl_Data matrices // Notice: keep the same satellite order as in the Vtl_Data matrices
// sample code // sample code
TrackingCmd trk_cmd;
trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_phase_rads = 0; // difficult of calculation
trk_cmd.carrier_freq_hz = doppler_hz_filt(channel) + kf_x(7)/ Lambda_GPS_L1;//+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction trk_cmd.carrier_freq_hz = doppler_hz_filt(channel) + kf_x(7)/ Lambda_GPS_L1;//+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction
trk_cmd.carrier_freq_rate_hz_s = 0; trk_cmd.carrier_freq_rate_hz_s = 0;

20
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
View File

@ -634,20 +634,23 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr<TrackingCmd>).hash_code()) if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr<TrackingCmd>).hash_code())
{ {
const auto cmd = wht::any_cast<const std::shared_ptr<TrackingCmd>>(pmt::any_ref(msg)); const auto cmd = wht::any_cast<const std::shared_ptr<TrackingCmd>>(pmt::any_ref(msg));
//std::cout<< "test cast CH "<<cmd->sample_counter <<"\n";
if (cmd->channel_id == this->d_channel) if (cmd->channel_id == this->d_channel)
{ {
arma::vec x_tmp;
arma::mat F_tmp;
gr::thread::scoped_lock lock(d_setlock); gr::thread::scoped_lock lock(d_setlock);
//To.Do: apply VTL corrections to the KF states //To.Do: apply VTL corrections to the KF states
double delta_t_s = static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in; double delta_t_s = static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in;
arma::mat F_tmp; // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s
x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s};
//ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces //ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces
F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0}, F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0},
{0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)}, {0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)},
{0.0, 0.0, 1.0, delta_t_s}, {0.0, 0.0, 1.0, delta_t_s},
{0.0, 0.0, 0.0, 1.0}}; {0.0, 0.0, 0.0, 1.0}};
arma::vec x_tmp;
// states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s
x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s};
// TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz)
//arma::vec tmp_x = F_tmp * x_tmp; //arma::vec tmp_x = F_tmp * x_tmp;
double old_doppler = d_x_old_old(2); double old_doppler = d_x_old_old(2);
@ -661,13 +664,14 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
// << " SampleCounter origin: " << cmd->sample_counter // << " SampleCounter origin: " << cmd->sample_counter
// << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]"
// << " [s]\n"; // << " [s]\n";
if(this->d_channel ==0) if(cmd->channel_id ==0)
{ {
std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: "
<< static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in << delta_t_s << "[s]"
<< " SampleCounter origin: " << cmd->sample_counter << " SampleCounter origin: " << cmd->sample_counter
<< " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]"
<< " [s]\n"; << "\n";
std::cout << "use count " <<cmd.use_count()<<"\r";
} }
std::fstream dump_tracking_file; std::fstream dump_tracking_file;