mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-06-20 15:34:09 +00:00
Code Issues Releases Wiki Activity

Beta as a private member

Browse Source
This commit is contained in:
Carles Fernandez 2022-07-30 10:07:06 +02:00
parent 0e6740de15
commit a7a59fe9c1
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
2 changed files with 36 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc
View File

@ -109,6 +109,7 @@ kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
d_code_phase_rate_step_chips(0.0), d_code_phase_rate_step_chips(0.0),
d_rem_code_phase_chips(0.0), d_rem_code_phase_chips(0.0),
d_rem_code_phase_samples(0.0), d_rem_code_phase_samples(0.0),
d_beta(0.0),
d_sample_counter(0ULL), d_sample_counter(0ULL),
d_acq_sample_stamp(0ULL), d_acq_sample_stamp(0ULL),
d_rem_carr_phase_rad(0.0), d_rem_carr_phase_rad(0.0),
@ -437,13 +438,15 @@ kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
std::cerr << "Invalid System argument when instantiating tracking blocks\n"; std::cerr << "Invalid System argument when instantiating tracking blocks\n";
d_correlation_length_ms = 1; d_correlation_length_ms = 1;
d_secondary = false; d_secondary = false;
d_signal_carrier_freq = 0.0; d_signal_carrier_freq = 1.0;
d_code_period = 0.0; d_code_period = 0.0;
d_code_length_chips = 0; d_code_length_chips = 0;
d_code_samples_per_chip = 0U; d_code_samples_per_chip = 0U;
d_symbols_per_bit = 0; d_symbols_per_bit = 0;
} }
d_beta = d_code_chip_rate / d_signal_carrier_freq;
// Initialization of local code replica // Initialization of local code replica
// Get space for a vector with the sinboc(1,1) replica sampled 2x/chip // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip
d_tracking_code.resize(2 * d_code_length_chips, 0.0); d_tracking_code.resize(2 * d_code_length_chips, 0.0);
@ -857,33 +860,31 @@ void kf_vtl_tracking::init_kf(double acq_code_phase_chips, double acq_doppler_hz
const double Ti = d_current_correlation_time_s; const double Ti = d_current_correlation_time_s;
const double TiTi = Ti * Ti; const double TiTi = Ti * Ti;
const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor d_F = {{1.0, 0.0, d_beta * Ti, d_beta * TiTi / 2.0},
{0.0, 1.0, 2.0 * GNSS_PI * Ti, GNSS_PI * TiTi},
{0.0, 0.0, 1.0, Ti},
{0.0, 0.0, 0.0, 1.0}};
d_F = {{1, 0, B * Ti, B * TiTi / 2}, d_H = {{1.0, 0.0, -d_beta * Ti / 2.0, d_beta * TiTi / 6.0},
{0, 1, 2.0 * GNSS_PI * Ti, GNSS_PI * TiTi}, {0.0, 1.0, -GNSS_PI * Ti, GNSS_PI * TiTi / 3.0}};
{0, 0, 1, Ti},
{0, 0, 0, 1}};
d_H = {{1, 0, -B * Ti / 2.0, B * TiTi / 6.0}, d_R = {{pow(d_trk_parameters.code_disc_sd_chips, 2.0), 0.0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * TiTi / 3.0}}; {0.0, pow(d_trk_parameters.carrier_disc_sd_rads, 2.0)}};
d_R = {{pow(d_trk_parameters.code_disc_sd_chips, 2.0), 0},
{0, pow(d_trk_parameters.carrier_disc_sd_rads, 2.0)}};
// system covariance matrix (static) // system covariance matrix (static)
d_Q = {{pow(d_trk_parameters.code_phase_sd_chips, 2.0), 0, 0, 0}, d_Q = {{pow(d_trk_parameters.code_phase_sd_chips, 2.0), 0.0, 0.0, 0.0},
{0, pow(d_trk_parameters.carrier_phase_sd_rad, 2.0), 0, 0}, {0.0, pow(d_trk_parameters.carrier_phase_sd_rad, 2.0), 0.0, 0.0},
{0, 0, pow(d_trk_parameters.carrier_freq_sd_hz, 2.0), 0}, {0.0, 0.0, pow(d_trk_parameters.carrier_freq_sd_hz, 2.0), 0.0},
{0, 0, 0, pow(d_trk_parameters.carrier_freq_rate_sd_hz_s, 2.0)}}; {0.0, 0.0, 0.0, pow(d_trk_parameters.carrier_freq_rate_sd_hz_s, 2.0)}};
// initial Kalman covariance matrix // initial Kalman covariance matrix
d_P_old_old = {{pow(d_trk_parameters.init_code_phase_sd_chips, 2.0), 0, 0, 0}, d_P_old_old = {{pow(d_trk_parameters.init_code_phase_sd_chips, 2.0), 0.0, 0.0, 0.0},
{0, pow(d_trk_parameters.init_carrier_phase_sd_rad, 2.0), 0, 0}, {0.0, pow(d_trk_parameters.init_carrier_phase_sd_rad, 2.0), 0.0, 0.0},
{0, 0, pow(d_trk_parameters.init_carrier_freq_sd_hz, 2.0), 0}, {0.0, 0.0, pow(d_trk_parameters.init_carrier_freq_sd_hz, 2.0), 0.0},
{0, 0, 0, pow(d_trk_parameters.init_carrier_freq_rate_sd_hz_s, 2.0)}}; {0.0, 0.0, 0.0, pow(d_trk_parameters.init_carrier_freq_rate_sd_hz_s, 2.0)}};
// states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s
d_x_old_old = {acq_code_phase_chips, 0, acq_doppler_hz, 0}; d_x_old_old = {acq_code_phase_chips, 0.0, acq_doppler_hz, 0.0};
std::cout << "F: " << d_F << "\n"; std::cout << "F: " << d_F << "\n";
std::cout << "H: " << d_H << "\n"; std::cout << "H: " << d_H << "\n";
@ -900,7 +901,6 @@ void kf_vtl_tracking::update_kf_narrow_integration_time()
const double Ti = d_current_correlation_time_s; const double Ti = d_current_correlation_time_s;
const double TiTi = Ti * Ti; const double TiTi = Ti * Ti;
const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor
arma::mat Qnew = arma::zeros(4, 4); arma::mat Qnew = arma::zeros(4, 4);
for (int i = 0; i < d_trk_parameters.extend_correlation_symbols; i++) for (int i = 0; i < d_trk_parameters.extend_correlation_symbols; i++)
{ {
@ -910,21 +910,21 @@ void kf_vtl_tracking::update_kf_narrow_integration_time()
d_Q = Qnew; d_Q = Qnew;
// state vector: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz // state vector: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz
d_F = {{1, 0, B * Ti, B * TiTi / 2.0}, d_F = {{1.0, 0.0, d_beta * Ti, d_beta * TiTi / 2.0},
{0, 1, 2.0 * GNSS_PI * Ti, GNSS_PI * TiTi}, {0.0, 1.0, 2.0 * GNSS_PI * Ti, GNSS_PI * TiTi},
{0, 0, 1, Ti}, {0.0, 0.0, 1.0, Ti},
{0, 0, 0, 1}}; {0.0, 0.0, 0.0, 1.0}};
d_H = {{1, 0, -B * Ti / 2.0, B * TiTi / 6.0}, d_H = {{1.0, 0.0, -d_beta * Ti / 2.0, d_beta * TiTi / 6.0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * TiTi / 3.0}}; {0.0, 1.0, -GNSS_PI * Ti, GNSS_PI * TiTi / 3.0}};
const double CN0_lin = pow(10.0, d_CN0_SNV_dB_Hz / 10.0); // CN0 in Hz const double CN0_lin = pow(10.0, d_CN0_SNV_dB_Hz / 10.0); // CN0 in Hz
const double CN0_lin_Ti = CN0_lin * Ti; const double CN0_lin_Ti = CN0_lin * Ti;
const double Sigma2_Phase = (1.0 / (2.0 * CN0_lin_Ti)) * (1.0 + 1.0 / (2.0 * CN0_lin_Ti)); const double Sigma2_Phase = (1.0 / (2.0 * CN0_lin_Ti)) * (1.0 + 1.0 / (2.0 * CN0_lin_Ti));
const double Sigma2_Tau = (1.0 / CN0_lin_Ti) * (d_trk_parameters.spc + (d_trk_parameters.spc / (1.0 - d_trk_parameters.spc)) * (1.0 / (2.0 * CN0_lin_Ti))); const double Sigma2_Tau = (1.0 / CN0_lin_Ti) * (d_trk_parameters.spc + (d_trk_parameters.spc / (1.0 - d_trk_parameters.spc)) * (1.0 / (2.0 * CN0_lin_Ti)));
d_R = {{Sigma2_Tau, 0}, d_R = {{Sigma2_Tau, 0.0},
{0, Sigma2_Phase}}; {0.0, Sigma2_Phase}};
std::cout << "Fu: " << d_F << "\n"; std::cout << "Fu: " << d_F << "\n";
std::cout << "Hu: " << d_H << "\n"; std::cout << "Hu: " << d_H << "\n";
@ -940,11 +940,9 @@ void kf_vtl_tracking::update_kf_cn0(double current_cn0_dbhz)
// Kalman Filter class variables // Kalman Filter class variables
const double Ti = d_current_correlation_time_s; // d_correlation_length_ms * 0.001; const double Ti = d_current_correlation_time_s; // d_correlation_length_ms * 0.001;
const double TiTi = Ti * Ti; const double TiTi = Ti * Ti;
const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor
d_H = arma::mat(2, 4); d_H = {{1.0, 0.0, -d_beta * Ti / 2.0, d_beta * TiTi / 6.0},
d_H = {{1, 0, -B * Ti / 2.0, B * TiTi / 6.0}, {0.0, 1.0, -GNSS_PI * Ti, GNSS_PI * TiTi / 3.0}};
{0, 1, -GNSS_PI * Ti, GNSS_PI * TiTi / 3.0}};
// Phase noise variance // Phase noise variance
const double CN0_lin = pow(10.0, current_cn0_dbhz / 10.0); // CN0 in Hz const double CN0_lin = pow(10.0, current_cn0_dbhz / 10.0); // CN0 in Hz
@ -952,8 +950,8 @@ void kf_vtl_tracking::update_kf_cn0(double current_cn0_dbhz)
const double Sigma2_Phase = (1.0 / (2.0 * CN0_lin_Ti)) * (1.0 + 1.0 / (2.0 * CN0_lin_Ti)); const double Sigma2_Phase = (1.0 / (2.0 * CN0_lin_Ti)) * (1.0 + 1.0 / (2.0 * CN0_lin_Ti));
const double Sigma2_Tau = (1.0 / CN0_lin_Ti) * (d_trk_parameters.spc + (d_trk_parameters.spc / (1.0 - d_trk_parameters.spc)) * (1.0 / (2.0 * CN0_lin_Ti))); const double Sigma2_Tau = (1.0 / CN0_lin_Ti) * (d_trk_parameters.spc + (d_trk_parameters.spc / (1.0 - d_trk_parameters.spc)) * (1.0 / (2.0 * CN0_lin_Ti)));
d_R = {{Sigma2_Tau, 0}, d_R = {{Sigma2_Tau, 0.0},
{0, Sigma2_Phase}}; {0.0, Sigma2_Phase}};
} }
@ -1833,7 +1831,7 @@ int kf_vtl_tracking::general_work(int noutput_items __attribute__((unused)), gr_
d_cn0_smoother.reset(); d_cn0_smoother.reset();
d_carrier_lock_test_smoother.reset(); d_carrier_lock_test_smoother.reset();
init_kf(0, d_carrier_doppler_kf_hz); init_kf(0.0, d_carrier_doppler_kf_hz);
LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)";
DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_kf_hz DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_kf_hz

2
src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.h
View File

@ -195,6 +195,8 @@ private:
double d_rem_code_phase_chips; double d_rem_code_phase_chips;
double d_rem_code_phase_samples; double d_rem_code_phase_samples;
double d_beta;
uint64_t d_sample_counter; uint64_t d_sample_counter;
uint64_t d_acq_sample_stamp; uint64_t d_acq_sample_stamp;