From 01fc2b1120fecb98b0c2e40dbe7c149e925fadc0 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 22 Jul 2022 13:20:25 +0200 Subject: [PATCH] Add work on KF tracking --- .../gnuradio_blocks/kf_vtl_tracking.cc | 53 ++----------------- src/algorithms/tracking/libs/kf_conf.cc | 22 -------- src/algorithms/tracking/libs/kf_conf.h | 18 ------- 3 files changed, 3 insertions(+), 90 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc index b24fe18e5..94eb6bc1e 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc @@ -18,6 +18,7 @@ * ----------------------------------------------------------------------------- */ +#include "kf_vtl_tracking.h" #include "Beidou_B1I.h" #include "Beidou_B3I.h" #include "GPS_L1_CA.h" @@ -39,7 +40,6 @@ #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" @@ -859,50 +859,29 @@ void kf_vtl_tracking::init_kf(double acq_code_phase_chips, double acq_doppler_hz const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor - 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 - // 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) - // 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 cov11ariance matrix (static) - d_Q = arma::mat(4, 4); + // system covariance matrix (static) 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(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(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}; @@ -940,25 +919,14 @@ void kf_vtl_tracking::update_kf_narrow_integration_time() 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 << 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"; @@ -982,14 +950,9 @@ void kf_vtl_tracking::update_kf_cn0(double current_cn0_dbhz) // Phase noise variance const double CN0_lin = pow(10.0, current_cn0_dbhz / 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))); - // measurement covariance matrix (static)d_trk_parameters.spc - d_R = arma::mat(2, 2); + d_R = {{Sigma2_Tau, 0}, {0, Sigma2_Phase}}; } @@ -1331,16 +1294,6 @@ void kf_vtl_tracking::update_tracking_vars() 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(d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); - // d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI); - - // carrier phase accumulator - // double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); - // double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); - // std::cout << fmod(b, TWO_PI) / fmod(a, TWO_PI) << '\n'; d_acc_carrier_phase_rad -= remnant_carrier_phase; // ################### DLL COMMANDS ################################################# diff --git a/src/algorithms/tracking/libs/kf_conf.cc b/src/algorithms/tracking/libs/kf_conf.cc index ab3c5a6df..80b049c38 100644 --- a/src/algorithms/tracking/libs/kf_conf.cc +++ b/src/algorithms/tracking/libs/kf_conf.cc @@ -34,10 +34,6 @@ Kf_Conf::Kf_Conf() : item_type("gr_complex"), 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), @@ -105,7 +101,6 @@ 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); // tracking lock tests smoother parameters cn0_smoother_samples = configuration->property(role + ".cn0_smoother_samples", cn0_smoother_samples); @@ -122,34 +117,17 @@ 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); - 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); - // 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); - 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_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_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); init_carrier_freq_rate_sd_hz_s = configuration->property(role + ".init_carrier_freq_rate_sd_hz_s", init_carrier_freq_rate_sd_hz_s); diff --git a/src/algorithms/tracking/libs/kf_conf.h b/src/algorithms/tracking/libs/kf_conf.h index 1c8789fb4..fe7d620b3 100644 --- a/src/algorithms/tracking/libs/kf_conf.h +++ b/src/algorithms/tracking/libs/kf_conf.h @@ -38,33 +38,18 @@ public: double carrier_lock_th; // 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 code_disc_sd_chips; double carrier_disc_sd_rads; // System covariances (Q) double code_phase_sd_chips; - // 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_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_carrier_phase_sd_rad; double init_carrier_freq_sd_hz; double init_carrier_freq_rate_sd_hz_s; @@ -96,9 +81,6 @@ public: bool high_dyn; bool dump; bool dump_mat; - - // bool enable_dynamic_measurement_covariance; - // bool use_estimated_cn0; }; #endif