mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 04:30:33 +00:00
Add work on KF tracking
This commit is contained in:
parent
ea4af27796
commit
01fc2b1120
@ -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<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 accumulator
|
||||
// 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 -= remnant_carrier_phase;
|
||||
|
||||
// ################### DLL COMMANDS #################################################
|
||||
|
@ -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);
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user