mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 04:00:34 +00:00
Code Issues Releases Wiki Activity

Add work on KF tracking

Browse Source
This commit is contained in:
Carles Fernandez 2022-07-22 08:15:35 +02:00
parent 14edfdf206
commit dc8e450c82
4 changed files with 229 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -18,7 +18,6 @@
* -----------------------------------------------------------------------------
*/
#include "kf_vtl_tracking.h"
#include "Beidou_B1I.h"
#include "Beidou_B3I.h"
#include "GPS_L1_CA.h"
@ -40,6 +39,7 @@
#include "gps_l2c_signal_replica.h"
#include "gps_l5_signal_replica.h"
#include "gps_sdr_signal_replica.h"
#include "kf_vtl_tracking.h"
#include "lock_detectors.h"
#include "tracking_discriminators.h"
#include "trackingcmd.h"
@ -537,6 +537,17 @@ kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
clear_tracking_vars();
if (d_trk_parameters.smoother_length > 0)
{
d_carr_ph_history.set_capacity(d_trk_parameters.smoother_length * 2);
d_code_ph_history.set_capacity(d_trk_parameters.smoother_length * 2);
}
else
{
d_carr_ph_history.set_capacity(1);
d_code_ph_history.set_capacity(1);
}
if (d_dump)
{
d_dump_filename = d_trk_parameters.dump_filename;
@ -638,6 +649,8 @@ void kf_vtl_tracking::start_tracking()
d_carrier_doppler_kf_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_kf_hz / d_trk_parameters.fs_in;
d_carrier_phase_rate_step_rad = 0.0;
d_carr_ph_history.clear();
d_code_ph_history.clear();
std::array<char, 3> Signal_{};
Signal_[0] = d_acquisition_gnss_synchro->Signal[0];
Signal_[1] = d_acquisition_gnss_synchro->Signal[1];
@ -732,7 +745,7 @@ void kf_vtl_tracking::start_tracking()
{
beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0);
// GEO Satellites use different secondary code
if ((d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) or d_acquisition_gnss_synchro->PRN > 58)
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization
d_correlation_length_ms = 1;
@ -765,7 +778,7 @@ void kf_vtl_tracking::start_tracking()
{
beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0);
// Update secondary code settings for geo satellites
if ((d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) or d_acquisition_gnss_synchro->PRN > 58)
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization
d_correlation_length_ms = 1;
@ -841,20 +854,21 @@ void kf_vtl_tracking::start_tracking()
void kf_vtl_tracking::init_kf(double acq_code_phase_chips, double acq_doppler_hz)
{
// Kalman Filter class variables
const double Ti = d_correlation_length_ms * 0.001;
// state vector: code_phase_chips, carrier_phase_rads, carrier_freq_hz,carrier_freq_rate_hz, code_freq_chips_s
d_F = arma::mat(5, 5);
d_F = {{1, 0, 0, 0, Ti},
{0, 1, 2.0 * GNSS_PI * Ti, GNSS_PI * (Ti * Ti), 0},
{0, 0, 1, Ti, 0},
{0, 0, 0, 1, 0},
{0, 0, 0, 0, 1}};
const double Ti = d_current_correlation_time_s;
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, 5);
d_H = {{1, 0, -B * Ti / 2.0, B * (Ti * Ti) / 6.0, 0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * (Ti * Ti) / 3.0, 0}};
d_F = arma::mat(4, 4);
d_F = {{1, 0, B * Ti, B * TiTi / 2},
{0, 1, 2.0 * GNSS_PI * Ti, GNSS_PI * TiTi},
{0, 0, 1, Ti},
{0, 0, 0, 1}};
d_H = arma::mat(2, 4);
d_H = {{1, 0, -B * Ti / 2.0, B * TiTi / 6.0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * TiTi / 3.0}};
// Phase noise variance
// const double CN0_lin = pow(10.0, d_trk_parameters.expected_cn0_dbhz / 10.0); // CN0 in Hz
// const double N_periods = 1; // Only 1 interval
@ -862,35 +876,42 @@ void kf_vtl_tracking::init_kf(double acq_code_phase_chips, double acq_doppler_hz
// const double Sigma2_Phase = 1.0 / (2.0 * CN0_lin * Ti) * (1.0 + 1.0 / (2.0 * CN0_lin * Ti));
// measurement covariance matrix (static)
d_R = arma::mat(2, 2);
// d_R = arma::mat(2, 2);
// d_R << Sigma2_Tau << 0 << arma::endr
// << 0 << Sigma2_Phase << arma::endr;
d_R = arma::mat(2, 2);
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)
d_Q = arma::mat(5, 5);
d_Q = {{pow(d_trk_parameters.code_phase_sd_chips, 2.0), 0, 0, 0, 0},
{0, pow(d_trk_parameters.carrier_phase_sd_rad, 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, pow(d_trk_parameters.code_rate_sd_chips_s, 2.0)}};
// system cov11ariance matrix (static)
d_Q = arma::mat(4, 4);
d_Q = {{pow(d_trk_parameters.code_phase_sd_chips, 2.0), 0, 0, 0},
{0, pow(d_trk_parameters.carrier_phase_sd_rad, 2.0), 0, 0},
{0, 0, pow(d_trk_parameters.carrier_freq_sd_hz, 2.0), 0},
{0, 0, 0, pow(d_trk_parameters.carrier_freq_rate_sd_hz_s, 2.0)}};
// initial Kalman covariance matrix
d_P_old_old = arma::mat(5, 5);
d_P_old_old = {{pow(d_trk_parameters.init_code_phase_sd_chips, 2.0), 0, 0, 0, 0},
{0, pow(d_trk_parameters.init_carrier_phase_sd_rad, 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, pow(d_trk_parameters.init_carrier_freq_rate_sd_hz_s, 2.0), 0},
{0, 0, 0, 0, pow(d_trk_parameters.init_code_rate_sd_chips_s, 2.0)}};
d_P_old_old = arma::mat(4, 4);
d_P_old_old = {{pow(d_trk_parameters.init_code_phase_sd_chips, 2.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_freq_sd_hz, 2.0), 0},
{0, 0, 0, pow(d_trk_parameters.init_carrier_freq_rate_sd_hz_s, 2.0)}};
// d_R = arma::mat(2, 2);
// d_R << Sigma2_Tau << 0 << arma::endr
// << 0 << Sigma2_Phase << arma::endr;
// init state vector
d_x_old_old = arma::vec(5);
// states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s, code_freq_rate_chips_s
d_x_old_old = {acq_code_phase_chips, 0, acq_doppler_hz, 0, 0};
// std::cout << "F: " << d_F << "\n";
// std::cout << "H: " << d_H << "\n";
// std::cout << "R: " << d_R << "\n";
// std::cout << "Q: " << d_Q << "\n";
// std::cout << "P: " << d_P_old_old << "\n";
// std::cout << "x: " << d_x_old_old << "\n";
d_x_old_old = arma::vec(4);
// 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};
std::cout << "F: " << d_F << "\n";
std::cout << "H: " << d_H << "\n";
std::cout << "R: " << d_R << "\n";
std::cout << "Q: " << d_Q << "\n";
std::cout << "P: " << d_P_old_old << "\n";
std::cout << "x: " << d_x_old_old << "\n";
}
@ -898,48 +919,76 @@ void kf_vtl_tracking::update_kf_narrow_integration_time()
{
// Kalman Filter class variables
const double Ti = d_current_correlation_time_s;
const double TiTi = Ti * Ti;
// state vector: code_phase_chips, carrier_phase_rads, carrier_freq_hz,carrier_freq_rate_hz, code_freq_chips_s
d_F = {{1, 0, 0, 0, Ti},
{0, 1, 2.0 * GNSS_PI * Ti, GNSS_PI * (Ti * Ti), 0},
{0, 0, 1, Ti, 0},
{0, 0, 0, 1, 0},
{0, 0, 0, 0, 1}};
const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor
arma::mat Qnew = arma::zeros(4, 4);
for (int i = 0; i < d_trk_parameters.extend_correlation_symbols; i++)
{
Qnew += d_F * d_Q * d_F.t();
// d_F = d_F * d_F;
d_Q = d_F * d_Q * d_F.t();
}
d_Q = Qnew;
d_H = {{1, 0, -B * Ti / 2.0, B * (Ti * Ti) / 6.0, 0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * (Ti * Ti) / 3.0, 0}};
// 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},
{0, 1, 2.0 * GNSS_PI * Ti, GNSS_PI * TiTi},
{0, 0, 1, Ti},
{0, 0, 0, 1}};
// measurement covariance matrix (static)
d_R = {{pow(d_trk_parameters.code_disc_sd_chips, 2.0), 0},
{0, pow(d_trk_parameters.carrier_disc_sd_rads, 2.0)}};
d_H = {{1, 0, -B * Ti / 2.0, B * TiTi / 6.0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * TiTi / 3.0}};
// system covariance matrix (static)
d_Q = {{pow(d_trk_parameters.narrow_code_phase_sd_chips, 2.0), 0, 0, 0, 0},
{0, pow(d_trk_parameters.narrow_carrier_phase_sd_rad, 2.0), 0, 0, 0},
{0, 0, pow(d_trk_parameters.narrow_carrier_freq_sd_hz, 2.0), 0, 0},
{0, 0, 0, pow(d_trk_parameters.narrow_carrier_freq_rate_sd_hz_s, 2.0), 0},
{0, 0, 0, 0, pow(d_trk_parameters.narrow_code_rate_sd_chips_s, 2.0)}};
// d_Q << pow(d_trk_parameters.narrow_code_phase_sd_chips, 2.0) << 0 << 0 << 0 << arma::endr
// << 0 << pow(d_trk_parameters.narrow_carrier_phase_sd_rad, 2.0) << 0 << 0 << arma::endr
// << 0 << 0 << pow(d_trk_parameters.narrow_carrier_freq_sd_hz, 2.0) << 0 << arma::endr
// << 0 << 0 << 0 << pow(d_trk_parameters.narrow_carrier_freq_rate_sd_hz_s, 2.0) << arma::endr;
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 N_periods = 1; // Only 1 interval
// const double Sigma2_Tau = 0.25 * (1.0 + 2.0 * CN0_lin * Ti) / (N_periods * pow(CN0_lin * Ti, 2.0)) * (1.0 + (1.0 + 2.0 * CN0_lin * Ti) / (pow(N_periods * (CN0_lin * Ti), 2.0)));
const double Sigma2_Phase = (1.0 / (2.0 * CN0_lin_Ti)) * (1.0 + 1.0 / (2.0 * CN0_lin_Ti));
// double prova = (1.0 / (CN0_lin * Ti)) * (0.5 + (0.5 / (1.0 - 0.5)) * (1.0 / (2.0 * CN0_lin * Ti)));
// std::cout << "Sigma2_Tau = " << Sigma2_Tau << ", prova: " << prova << std::endl;
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 = arma::mat(2, 2);
d_R = {{Sigma2_Tau, 0},
{0, Sigma2_Phase}};
// << 0 << 0 << 0 << 0 << pow(d_trk_parameters.narrow_code_rate_sd_chips_s, 2.0) << arma::endr;
std::cout << "Fu: " << d_F << "\n";
std::cout << "Hu: " << d_H << "\n";
std::cout << "Ru: " << d_R << "\n";
std::cout << "Qu: " << d_Q << "\n";
std::cout << "Pu: " << d_P_old_old << "\n";
std::cout << "xu: " << d_x_old_old << "\n";
}
void kf_vtl_tracking::update_kf_cn0(double current_cn0_dbhz)
{
// Kalman Filter class variables
const double Ti = 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 B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor
d_H = arma::mat(2, 5);
d_H = {{1, 0, -B * Ti / 2.0, B * (Ti * Ti) / 6.0, 0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * (Ti * Ti) / 3.0, 0}};
d_H = arma::mat(2, 4);
d_H = {{1, 0, -B * Ti / 2.0, B * TiTi / 6.0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * TiTi / 3.0}};
// Phase noise variance
const double CN0_lin = pow(10.0, current_cn0_dbhz / 10.0); // CN0 in Hz
const double N_periods = 1; // Only 1 interval
const double Sigma2_Tau = 0.25 * (1.0 + 2.0 * CN0_lin * Ti) / (N_periods * pow(CN0_lin * Ti, 2.0)) * (1.0 + (1.0 + 2.0 * CN0_lin * Ti) / (pow(N_periods * (CN0_lin * Ti), 2.0)));
const double Sigma2_Phase = 1.0 / (2.0 * CN0_lin * Ti) * (1.0 + 1.0 / (2.0 * CN0_lin * Ti));
// measurement covariance matrix (static)
const double CN0_lin_Ti = CN0_lin * Ti;
// const double N_periods = 1; // Only 1 interval
// const double Sigma2_Tau = 0.25 * (1.0 + 2.0 * CN0_lin * Ti) / (N_periods * pow(CN0_lin * Ti, 2.0)) * (1.0 + (1.0 + 2.0 * CN0_lin * Ti) / (pow(N_periods * (CN0_lin * Ti), 2.0)));
const double Sigma2_Phase = (1.0 / (2.0 * CN0_lin_Ti)) * (1.0 + 1.0 / (2.0 * CN0_lin_Ti));
// double prova = (1.0 / (CN0_lin * Ti)) * (0.5 + (0.5 / (1.0 - 0.5)) * (1.0 / (2.0 * CN0_lin * Ti)));
// std::cout << "Sigma2_Tau = " << Sigma2_Tau << ", prova: " << prova << std::endl;
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)));
// measurement covariance matrix (static)d_trk_parameters.spc
d_R = arma::mat(2, 2);
d_R = {{Sigma2_Tau, 0},
{0, Sigma2_Phase}};
@ -1145,17 +1194,16 @@ void kf_vtl_tracking::run_Kf()
// Prediction
d_x_new_old = d_F * d_x_old_old;
d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;
// Innovation
arma::vec z = {d_code_error_disc_chips, d_carr_phase_error_disc_hz * TWO_PI};
// Measurement update
arma::vec z = {d_code_error_disc_chips, d_carr_phase_error_disc_hz * TWO_PI};
arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R); // Kalman gain
d_x_new_new = d_x_new_old + K * z;
d_P_new_new = (arma::eye(5, 5) - K * d_H) * d_P_new_old;
d_P_new_new = (arma::eye(4, 4) - K * d_H) * d_P_new_old;
// new code phase estimation
d_code_error_kf_chips = d_x_new_new(0);
@ -1165,28 +1213,34 @@ void kf_vtl_tracking::run_Kf()
d_carrier_phase_kf_rad = d_x_new_new(1);
// New carrier Doppler frequency estimation
d_carrier_doppler_kf_hz = d_x_new_new(2); // d_carrier_loop_filter.get_carrier_error(0, static_cast<float>(d_carr_phase_error_hz), static_cast<float>(d_current_correlation_time_s));
d_carrier_doppler_kf_hz = d_x_new_new(2);
if (d_channel == 0)
{
// std::cout << "Doppler: " << d_carrier_doppler_kf_hz << '\n';
}
// d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / TWO_PI;
// d_x_new_new(2) = d_x_new_new(2) + fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / TWO_PI;
d_P_accu_old = d_P_accu;
d_carrier_doppler_rate_kf_hz_s = d_x_new_new(3);
// New code Doppler frequency estimation
if (d_trk_parameters.carrier_aiding)
{
// estimate the code rate exclusively based on the carrier Doppler
d_code_freq_kf_chips_s = d_code_chip_rate + d_carrier_doppler_kf_hz * d_code_chip_rate / d_signal_carrier_freq;
}
else
if (d_trk_parameters.high_dyn)
{
// use its own KF code rate estimation
d_code_freq_kf_chips_s -= d_x_new_new(4);
// d_x_new_new(3) = 0;
// std::cout << "d_carrier_doppler_rate_kf_hz_s: " << d_carrier_doppler_rate_kf_hz_s << '\n';
// d_carrier_phase_rate_step_rad = TWO_PI * d_carrier_doppler_rate_kf_hz_s / d_trk_parameters.fs_in;
// d_carrier_phase_rate_step_rad = d_carrier_doppler_rate_kf_hz_s / d_trk_parameters.fs_in;
}
d_x_new_new(4) = 0;
// d_x_new_new(4) = 0;
// Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler
// if (d_trk_parameters.enable_doppler_correction == true)
// {
// if (d_pull_in_transitory == false and d_corrected_doppler == false)
// {
// todo: alforithm here...
// todo: algorithm here...
// }
// }
@ -1224,6 +1278,8 @@ void kf_vtl_tracking::clear_tracking_vars()
d_Prompt_circular_buffer.clear();
d_carrier_phase_rate_step_rad = 0.0;
d_code_phase_rate_step_chips = 0.0;
d_carr_ph_history.clear();
d_code_ph_history.clear();
}
@ -1237,8 +1293,6 @@ void kf_vtl_tracking::update_tracking_vars()
// keep alignment parameters for the next input buffer
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
d_T_prn_samples = d_T_prn_seconds * d_trk_parameters.fs_in;
// d_K_blk_samples = d_T_prn_samples + d_rem_code_phase_samples + d_trk_parameters.fs_in * d_code_error_kf_chips / d_code_freq_kf_chips_s;
// KF will update d_rem_code_phase_samples
d_K_blk_samples = d_T_prn_samples + d_rem_code_phase_samples;
d_current_prn_length_samples = static_cast<int32_t>(std::floor(d_K_blk_samples)); // round to a discrete number of samples
@ -1247,16 +1301,37 @@ void kf_vtl_tracking::update_tracking_vars()
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_kf_hz / d_trk_parameters.fs_in;
// d_rem_carr_phase_rad = d_carrier_phase_kf_rad;
// remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad += static_cast<float>(d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) +
0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI);
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
if (d_trk_parameters.high_dyn)
{
d_carrier_phase_rate_step_rad = TWO_PI * d_carrier_doppler_rate_kf_hz_s / d_trk_parameters.fs_in;
// d_carrier_phase_rate_step_rad = TWO_PI * d_carrier_doppler_rate_kf_hz_s / d_trk_parameters.fs_in;
// d_carrier_doppler_rate_kf_hz_s = d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in / TWO_PI;
d_carr_ph_history.push_back(std::pair<double, double>(d_carrier_phase_step_rad, static_cast<double>(d_current_prn_length_samples)));
if (d_carr_ph_history.full())
{
double tmp_cp1 = 0.0;
double tmp_cp2 = 0.0;
double tmp_samples = 0.0;
for (unsigned int k = 0; k < d_trk_parameters.smoother_length; k++)
{
tmp_cp1 += d_carr_ph_history[k].first;
tmp_cp2 += d_carr_ph_history[d_trk_parameters.smoother_length * 2 - k - 1].first;
tmp_samples += d_carr_ph_history[d_trk_parameters.smoother_length * 2 - k - 1].second;
}
tmp_cp1 /= static_cast<double>(d_trk_parameters.smoother_length);
tmp_cp2 /= static_cast<double>(d_trk_parameters.smoother_length);
d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples;
d_x_old_old(3) = d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in / TWO_PI;
}
}
// remnant carrier phase to prevent overflow in the code NCO
auto remnant_carrier_phase = static_cast<float>(d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) +
0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
d_rem_carr_phase_rad += remnant_carrier_phase;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI);
// std::cout << d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / TWO_PI << '\n';
// remnant carrier phase to prevent overflow in the code NCO
// d_rem_carr_phase_rad += static_cast<float>(d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
@ -1266,17 +1341,31 @@ void kf_vtl_tracking::update_tracking_vars()
// double a = d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
// double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples);
// std::cout << fmod(b, TWO_PI) / fmod(a, TWO_PI) << '\n';
d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) +
0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
d_acc_carrier_phase_rad -= remnant_carrier_phase;
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
d_code_phase_step_chips = d_code_freq_kf_chips_s / d_trk_parameters.fs_in;
// todo: extend kf to estimate code rate
// if (d_trk_parameters.high_dyn)
// {
// d_code_phase_rate_step_chips = d_code_freq_kf_rate_chips_s / d_trk_parameters.fs_in;
// }
if (d_trk_parameters.high_dyn)
{
d_code_ph_history.push_back(std::pair<double, double>(d_code_phase_step_chips, static_cast<double>(d_current_prn_length_samples)));
if (d_code_ph_history.full())
{
double tmp_cp1 = 0.0;
double tmp_cp2 = 0.0;
double tmp_samples = 0.0;
for (unsigned int k = 0; k < d_trk_parameters.smoother_length; k++)
{
tmp_cp1 += d_code_ph_history[k].first;
tmp_cp2 += d_code_ph_history[d_trk_parameters.smoother_length * 2 - k - 1].first;
tmp_samples += d_code_ph_history[d_trk_parameters.smoother_length * 2 - k - 1].second;
}
tmp_cp1 /= static_cast<double>(d_trk_parameters.smoother_length);
tmp_cp2 /= static_cast<double>(d_trk_parameters.smoother_length);
d_code_phase_rate_step_chips = (tmp_cp2 - tmp_cp1) / tmp_samples;
}
}
// remnant code phase [chips]
d_rem_code_phase_samples = d_K_blk_samples - static_cast<double>(d_current_prn_length_samples); // rounding error < 1 sample
@ -1792,17 +1881,6 @@ int kf_vtl_tracking::general_work(int noutput_items __attribute__((unused)), gr_
d_cn0_smoother.reset();
d_carrier_lock_test_smoother.reset();
// init KF
// d_T_chip_seconds = 1.0 / d_code_freq_chips;
// d_T_prn_seconds = d_T_chip_seconds * static_cast<double>(samples_offset);
// // keep alignment parameters for the next input buffer
// // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
// d_T_prn_samples = d_T_prn_seconds * d_trk_parameters.fs_in;
// d_K_blk_samples = d_T_prn_samples + d_rem_code_phase_samples;
// // remnant code phase [chips]
// d_rem_code_phase_samples = d_K_blk_samples - static_cast<double>(d_current_prn_length_samples); // rounding error < 1 sample
// d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / d_trk_parameters.fs_in;
init_kf(0, d_carrier_doppler_kf_hz);
LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)";
@ -1956,7 +2034,8 @@ int kf_vtl_tracking::general_work(int noutput_items __attribute__((unused)), gr_
// perform a correlation step
do_correlation_step(in);
save_correlation_results();
update_tracking_vars();
// run_Kf();
update_tracking_vars(); // ?
if (d_current_data_symbol == 0)
{
log_data();
@ -2002,14 +2081,8 @@ int kf_vtl_tracking::general_work(int noutput_items __attribute__((unused)), gr_
d_state = 0; // loss-of-lock detected
}
else
{
if (d_trk_parameters.use_estimated_cn0 == true)
{
if (d_CN0_SNV_dB_Hz > 0)
{
update_kf_cn0(d_CN0_SNV_dB_Hz);
}
}
run_Kf();
update_tracking_vars();
check_carrier_phase_coherent_initialization();

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

@ -112,6 +112,8 @@ private:
volk_gnsssdr::vector<gr_complex> d_Prompt_buffer;
boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
boost::circular_buffer<std::pair<double, double>> d_code_ph_history;
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
const size_t d_int_type_hash_code = typeid(int).hash_code();

59
src/algorithms/tracking/libs/kf_conf.cc
View File

@ -28,24 +28,20 @@ Kf_Conf::Kf_Conf() : item_type("gr_complex"),
dump_filename("./Kf_dump.dat"),
fs_in(2000000.0),
carrier_lock_th(FLAGS_carrier_lock_th),
expected_cn0_dbhz(42.0),
code_disc_sd_chips(0.01),
carrier_disc_sd_rads(0.1),
code_phase_sd_chips(0.001),
code_rate_sd_chips_s(0.001),
carrier_phase_sd_rad(0.001),
carrier_freq_sd_hz(0.1),
carrier_freq_rate_sd_hz_s(1),
narrow_code_phase_sd_chips(0.001),
narrow_code_rate_sd_chips_s(0.001),
narrow_carrier_phase_sd_rad(0.001),
narrow_carrier_freq_sd_hz(0.1),
narrow_carrier_freq_rate_sd_hz_s(1),
init_code_phase_sd_chips(1),
init_code_rate_sd_chips_s(100),
init_carrier_phase_sd_rad(10),
init_carrier_freq_sd_hz(1000),
init_carrier_freq_rate_sd_hz_s(1000),
code_disc_sd_chips(0.2),
carrier_disc_sd_rads(0.3),
code_phase_sd_chips(0.15),
carrier_phase_sd_rad(0.25),
carrier_freq_sd_hz(0.6),
carrier_freq_rate_sd_hz_s(0.01),
// narrow_code_phase_sd_chips(0.1),
// narrow_carrier_phase_sd_rad(0.01),
// narrow_carrier_freq_sd_hz(0.01),
// narrow_carrier_freq_rate_sd_hz_s(0.1),
init_code_phase_sd_chips(0.5),
init_carrier_phase_sd_rad(0.7),
init_carrier_freq_sd_hz(5),
init_carrier_freq_rate_sd_hz_s(1),
early_late_space_chips(0.25),
very_early_late_space_chips(0.5),
early_late_space_narrow_chips(0.15),
@ -69,12 +65,9 @@ Kf_Conf::Kf_Conf() : item_type("gr_complex"),
system('G'),
track_pilot(true),
enable_doppler_correction(false),
carrier_aiding(true),
high_dyn(false),
dump(false),
dump_mat(true),
enable_dynamic_measurement_covariance(false),
use_estimated_cn0(false)
dump_mat(true)
{
signal[0] = '1';
signal[1] = 'C';
@ -112,7 +105,7 @@ void Kf_Conf::SetFromConfiguration(const ConfigurationInterface *configuration,
max_code_lock_fail = configuration->property(role + ".max_lock_fail", max_code_lock_fail);
max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", max_carrier_lock_fail);
carrier_lock_th = configuration->property(role + ".carrier_lock_th", carrier_lock_th);
carrier_aiding = configuration->property(role + ".carrier_aiding", carrier_aiding);
// carrier_aiding = configuration->property(role + ".carrier_aiding", carrier_aiding);
// tracking lock tests smoother parameters
cn0_smoother_samples = configuration->property(role + ".cn0_smoother_samples", cn0_smoother_samples);
@ -129,33 +122,33 @@ void Kf_Conf::SetFromConfiguration(const ConfigurationInterface *configuration,
// Kalman filter covariances
// Measurement covariances (R)
expected_cn0_dbhz = configuration->property(role + ".expected_cn0_dbhz", expected_cn0_dbhz);
// expected_cn0_dbhz = configuration->property(role + ".expected_cn0_dbhz", expected_cn0_dbhz);
code_disc_sd_chips = configuration->property(role + ".code_disc_sd_chips", code_disc_sd_chips);
carrier_disc_sd_rads = configuration->property(role + ".carrier_disc_sd_rads", carrier_disc_sd_rads);
enable_dynamic_measurement_covariance = configuration->property(role + ".enable_dynamic_measurement_covariance", enable_dynamic_measurement_covariance);
use_estimated_cn0 = configuration->property(role + ".use_estimated_cn0", use_estimated_cn0);
// enable_dynamic_measurement_covariance = configuration->property(role + ".enable_dynamic_measurement_covariance", enable_dynamic_measurement_covariance);
// use_estimated_cn0 = configuration->property(role + ".use_estimated_cn0", use_estimated_cn0);
// System covariances (Q)
code_phase_sd_chips = configuration->property(role + ".code_phase_sd_chips", code_phase_sd_chips);
code_rate_sd_chips_s = configuration->property(role + ".code_rate_sd_chips_s", code_rate_sd_chips_s);
// code_rate_sd_chips_s = configuration->property(role + ".code_rate_sd_chips_s", code_rate_sd_chips_s);
carrier_phase_sd_rad = configuration->property(role + ".carrier_phase_sd_rad", carrier_phase_sd_rad);
carrier_freq_sd_hz = configuration->property(role + ".carrier_freq_sd_hz", carrier_freq_sd_hz);
carrier_freq_rate_sd_hz_s = configuration->property(role + ".carrier_freq_rate_sd_hz_s", carrier_freq_rate_sd_hz_s);
// System covariances (narrow) (Q)
narrow_code_phase_sd_chips = configuration->property(role + ".narrow_code_phase_sd_chips", narrow_code_phase_sd_chips);
narrow_code_rate_sd_chips_s = configuration->property(role + ".narrow_code_rate_sd_chips_s", narrow_code_rate_sd_chips_s);
//narrow_code_phase_sd_chips = configuration->property(role + ".narrow_code_phase_sd_chips", narrow_code_phase_sd_chips);
// narrow_code_rate_sd_chips_s = configuration->property(role + ".narrow_code_rate_sd_chips_s", narrow_code_rate_sd_chips_s);
narrow_carrier_phase_sd_rad = configuration->property(role + ".narrow_carrier_phase_sd_rad", narrow_carrier_phase_sd_rad);
narrow_carrier_freq_sd_hz = configuration->property(role + ".narrow_carrier_freq_sd_hz", narrow_carrier_freq_sd_hz);
narrow_carrier_freq_rate_sd_hz_s = configuration->property(role + ".narrow_carrier_freq_rate_sd_hz_s", narrow_carrier_freq_rate_sd_hz_s);
//narrow_carrier_phase_sd_rad = configuration->property(role + ".narrow_carrier_phase_sd_rad", narrow_carrier_phase_sd_rad);
// narrow_carrier_freq_sd_hz = configuration->property(role + ".narrow_carrier_freq_sd_hz", narrow_carrier_freq_sd_hz);
// narrow_carrier_freq_rate_sd_hz_s = configuration->property(role + ".narrow_carrier_freq_rate_sd_hz_s", narrow_carrier_freq_rate_sd_hz_s);
// initial Kalman covariance matrix (P)
init_code_phase_sd_chips = configuration->property(role + ".init_code_phase_sd_chips", init_code_phase_sd_chips);
init_code_rate_sd_chips_s = configuration->property(role + ".init_code_rate_sd_chips_s", init_code_rate_sd_chips_s);
// init_code_rate_sd_chips_s = configuration->property(role + ".init_code_rate_sd_chips_s", init_code_rate_sd_chips_s);
init_carrier_phase_sd_rad = configuration->property(role + ".init_carrier_phase_sd_rad", init_carrier_phase_sd_rad);
init_carrier_freq_sd_hz = configuration->property(role + ".init_carrier_freq_sd_hz", init_carrier_freq_sd_hz);

21
src/algorithms/tracking/libs/kf_conf.h
View File

@ -40,30 +40,30 @@ public:
// KF statistics
// states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s, code_freq_rate_chips_s
// Measurement covariances (R)
double expected_cn0_dbhz;
// double expected_cn0_dbhz;
double code_disc_sd_chips;
double carrier_disc_sd_rads;
// System covariances (Q)
double code_phase_sd_chips;
double code_rate_sd_chips_s;
// double code_rate_sd_chips_s;
double carrier_phase_sd_rad;
double carrier_freq_sd_hz;
double carrier_freq_rate_sd_hz_s;
// System covariances (narrow) (Q)
double narrow_code_phase_sd_chips;
double narrow_code_rate_sd_chips_s;
// double narrow_code_phase_sd_chips;
// double narrow_code_rate_sd_chips_s;
double narrow_carrier_phase_sd_rad;
double narrow_carrier_freq_sd_hz;
double narrow_carrier_freq_rate_sd_hz_s;
// double narrow_carrier_phase_sd_rad;
// double narrow_carrier_freq_sd_hz;
//double narrow_carrier_freq_rate_sd_hz_s;
// initial Kalman covariance matrix (P)
double init_code_phase_sd_chips;
double init_code_rate_sd_chips_s;
// double init_code_rate_sd_chips_s;
double init_carrier_phase_sd_rad;
double init_carrier_freq_sd_hz;
@ -93,13 +93,12 @@ public:
char system;
bool track_pilot;
bool enable_doppler_correction;
bool carrier_aiding;
bool high_dyn;
bool dump;
bool dump_mat;
bool enable_dynamic_measurement_covariance;
bool use_estimated_cn0;
// bool enable_dynamic_measurement_covariance;
// bool use_estimated_cn0;
};
#endif