From 032e73e727fe8e17c788173531500b44fe048539 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Mon, 13 Aug 2018 21:15:58 -0400 Subject: [PATCH 1/4] Updates to 3-state Kalman filter parameters to improve tracking --- .../gps_l1_ca_kf_tracking_cc.cc | 50 ++++++++++++++----- .../gps_l1_ca_kf_tracking_cc.h | 1 + 2 files changed, 39 insertions(+), 12 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index 14d0bd16b..b3af2680f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -165,6 +165,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( d_acq_code_phase_samples = 0.0; d_acq_carrier_doppler_hz = 0.0; d_carrier_doppler_hz = 0.0; + d_carrier_dopplerrate_hz2 = 0.0; d_acc_carrier_phase_rad = 0.0; d_code_phase_samples = 0.0; d_rem_code_phase_chips = 0.0; @@ -186,7 +187,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( //covariances (static) double sigma2_carrier_phase = GPS_TWO_PI / 4; double sigma2_doppler = 450; - double sigma2_doppler_rate = 1.0 / 24.0; + double sigma2_doppler_rate = pow(4.0 * GPS_TWO_PI, 2) / 12.0; kf_P_x_ini = arma::zeros(2, 2); kf_P_x_ini(0, 0) = sigma2_carrier_phase; @@ -196,7 +197,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_R(0, 0) = sigma2_phase_detector_cycles2; kf_Q = arma::zeros(2, 2); - kf_Q(0, 0) = pow(4, GPS_L1_CA_CODE_PERIOD); + kf_Q(0, 0) = pow(GPS_L1_CA_CODE_PERIOD, 4); kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD; kf_F = arma::zeros(2, 2); @@ -210,6 +211,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_x = arma::zeros(2, 1); kf_y = arma::zeros(1, 1); + kf_P_y = arma::zeros(1, 1); // order three if (d_order == 3) @@ -218,12 +220,12 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_P_x_ini(2, 2) = sigma2_doppler_rate; kf_Q = arma::zeros(3, 3); - kf_Q(0, 0) = pow(6, GPS_L1_CA_CODE_PERIOD); - kf_Q(1, 1) = pow(4, GPS_L1_CA_CODE_PERIOD); - kf_Q(2, 2) = pow(2, GPS_L1_CA_CODE_PERIOD); + kf_Q(0, 0) = pow(GPS_L1_CA_CODE_PERIOD, 4); + kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD; + kf_Q(2, 2) = GPS_L1_CA_CODE_PERIOD; kf_F = arma::resize(kf_F, 3, 3); - kf_F(0, 2) = 0.25 * GPS_TWO_PI * pow(2, GPS_L1_CA_CODE_PERIOD); + kf_F(0, 2) = 0.5 * GPS_TWO_PI * pow(GPS_L1_CA_CODE_PERIOD, 2); kf_F(1, 2) = GPS_L1_CA_CODE_PERIOD; kf_F(2, 0) = 0.0; kf_F(2, 1) = 0.0; @@ -233,10 +235,10 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_H(0, 2) = 0.0; kf_x = arma::resize(kf_x, 3, 1); - kf_x(2, 0) = -0.25; + kf_x(2, 0) = 0.0; } -} +} void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() { @@ -251,7 +253,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() // Correct Kalman filter covariance according to acq doppler step size (3 sigma) if (d_acquisition_gnss_synchro->Acq_doppler_step > 0) { - kf_P_x_ini(1, 1) = pow(2, d_acq_carrier_doppler_step_hz / 3.0); + kf_P_x_ini(1, 1) = pow(d_acq_carrier_doppler_step_hz / 3.0, 2); } long int acq_trk_diff_samples; @@ -288,6 +290,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_acq_code_phase_samples = corrected_acq_phase_samples; d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_dopplerrate_hz2 = 0; d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); // DLL filter initialization @@ -372,7 +375,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() // READ DUMP FILE std::ifstream::pos_type size; int number_of_double_vars = 1; - int number_of_float_vars = 17; + int number_of_float_vars = 18; int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(unsigned int); std::ifstream dump_file; @@ -408,6 +411,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch]; float *acc_carrier_phase_rad = new float[num_epoch]; float *carrier_doppler_hz = new float[num_epoch]; + float *carrier_dopplerrate_hz2 = new float[num_epoch]; float *code_freq_chips = new float[num_epoch]; float *carr_error_hz = new float[num_epoch]; float *carr_error_filt_hz = new float[num_epoch]; @@ -435,6 +439,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_dopplerrate_hz2[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); @@ -462,6 +467,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() delete[] PRN_start_sample_count; delete[] acc_carrier_phase_rad; delete[] carrier_doppler_hz; + delete[] carrier_dopplerrate_hz2; delete[] code_freq_chips; delete[] carr_error_hz; delete[] carr_error_filt_hz; @@ -525,6 +531,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("carrier_dopplerrate_hz2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_dopplerrate_hz2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -576,6 +586,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() delete[] PRN_start_sample_count; delete[] acc_carrier_phase_rad; delete[] carrier_doppler_hz; + delete[] carrier_dopplerrate_hz2; delete[] code_freq_chips; delete[] carr_error_hz; delete[] carr_error_filt_hz; @@ -667,7 +678,11 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_P_x = kf_P_x_ini; // Update Kalman states based on acquisition information kf_x(0) = d_carrier_phase_step_rad * samples_offset; - kf_x(1) = current_synchro_data.Carrier_Doppler_hz; + kf_x(1) = d_carrier_doppler_hz; + if (kf_x.n_elem > 2) + { + kf_x(2) = d_carrier_dopplerrate_hz2; + } consume_each(samples_offset); // shift input to perform alignment with local replica return 1; @@ -705,8 +720,17 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_P_x = (arma::eye(size(kf_P_x_pre)) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix + // Store Kalman filter results d_rem_carr_phase_rad = kf_x(0); // set a new carrier Phase estimation to the NCO d_carrier_doppler_hz = kf_x(1); // set a new carrier Doppler estimation to the NCO + if (kf_x.n_elem > 2) + { + d_carrier_dopplerrate_hz2 = kf_x(2); + } + else + { + d_carrier_dopplerrate_hz2 = 0; + } // ################## DLL ########################################################## // New code Doppler frequency estimation based on carrier frequency estimation @@ -833,9 +857,11 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus // carrier and code frequency tmp_float = d_carrier_doppler_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_dopplerrate_hz2; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_code_freq_chips; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // PLL commands + // Kalman commands tmp_float = static_cast(carr_phase_error_rad * GPS_TWO_PI); d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = static_cast(d_rem_carr_phase_rad * GPS_TWO_PI); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index db690e52b..e6b0e87ce 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -157,6 +157,7 @@ private: double d_code_freq_chips; double d_code_phase_step_chips; double d_carrier_doppler_hz; + double d_carrier_dopplerrate_hz2; double d_carrier_phase_step_rad; double d_acc_carrier_phase_rad; double d_code_phase_samples; From e42467a06868561449328d926a75057aee46c431 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Mon, 13 Aug 2018 21:18:01 -0400 Subject: [PATCH 2/4] Updates to integration of bayesian_estimation library into GPS L1 kalman tracking block --- .../adapters/gps_l1_ca_kf_tracking.cc | 18 +++- .../gps_l1_ca_kf_tracking_cc.cc | 86 ++++++++++++++++--- .../gps_l1_ca_kf_tracking_cc.h | 35 ++++++-- .../tracking/libs/bayesian_estimation.cc | 26 +++++- .../tracking/libs/bayesian_estimation.h | 5 +- 5 files changed, 148 insertions(+), 22 deletions(-) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc index e124d411a..4c06d5580 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -64,6 +64,11 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( float pll_bw_hz; float dll_bw_hz; float early_late_space_chips; + bool bce_run; + unsigned int bce_ptrans; + unsigned int bce_strans; + int bce_nu; + int bce_kappa; item_type = configuration->property(role + ".item_type", default_item_type); order = configuration->property(role + ".order", 2); @@ -78,6 +83,12 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); + bce_run = configuration->property(role + ".bce_run", false); + bce_ptrans = configuration->property(role + ".p_transient", 0); + bce_strans = configuration->property(role + ".s_transient", 0); + bce_nu = configuration->property(role + ".bce_nu", 0); + bce_kappa = configuration->property(role + ".bce_kappa", 0); + //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { @@ -90,7 +101,12 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( dump, dump_filename, dll_bw_hz, - early_late_space_chips); + early_late_space_chips, + bce_run, + bce_ptrans, + bce_strans, + bce_nu, + bce_kappa); } else { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index b3af2680f..0f6b3d9d5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -66,10 +66,16 @@ gps_l1_ca_kf_make_tracking_cc( bool dump, std::string dump_filename, float dll_bw_hz, - float early_late_space_chips) + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa) { return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(order, if_freq, - fs_in, vector_length, dump, dump_filename, dll_bw_hz, early_late_space_chips)); + fs_in, vector_length, dump, dump_filename, dll_bw_hz, early_late_space_chips, + bce_run, bce_ptrans, bce_strans, bce_nu, bce_kappa)); } @@ -91,7 +97,12 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( bool dump, std::string dump_filename, float dll_bw_hz, - float early_late_space_chips) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input @@ -140,6 +151,8 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( d_rem_code_phase_samples = 0.0; // define residual carrier phase d_rem_carr_phase_rad = 0.0; + // define residual carrier phase covariance + d_carr_phase_sigma2 = 0.0; // sample synchronization d_sample_counter = 0; @@ -238,6 +251,17 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_x(2, 0) = 0.0; } + // Bayesian covariance estimator initialization + kf_iter = 0; + bayes_run = bce_run; + bayes_ptrans = bce_ptrans; + bayes_strans = bce_strans; + + bayes_kappa = bce_kappa; + bayes_nu = bce_nu; + kf_R_est = kf_R; + + bayes_estimator.init(arma::zeros(1,1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R)*(bayes_nu + 2)); } void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() @@ -254,6 +278,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() if (d_acquisition_gnss_synchro->Acq_doppler_step > 0) { kf_P_x_ini(1, 1) = pow(d_acq_carrier_doppler_step_hz / 3.0, 2); + bayes_estimator.init(arma::zeros(1,1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R)*(bayes_nu + 2)); } long int acq_trk_diff_samples; @@ -310,6 +335,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_rem_carr_phase_rad = 0.0; d_rem_code_phase_chips = 0.0; d_acc_carrier_phase_rad = 0.0; + d_carr_phase_sigma2 = 0.0; d_code_phase_samples = d_acq_code_phase_samples; @@ -375,7 +401,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() // READ DUMP FILE std::ifstream::pos_type size; int number_of_double_vars = 1; - int number_of_float_vars = 18; + int number_of_float_vars = 19; int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(unsigned int); std::ifstream dump_file; @@ -414,6 +440,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() float *carrier_dopplerrate_hz2 = new float[num_epoch]; float *code_freq_chips = new float[num_epoch]; float *carr_error_hz = new float[num_epoch]; + float *carr_noise_sigma2 = new float[num_epoch]; float *carr_error_filt_hz = new float[num_epoch]; float *code_error_chips = new float[num_epoch]; float *code_error_filt_chips = new float[num_epoch]; @@ -442,6 +469,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() dump_file.read(reinterpret_cast(&carrier_dopplerrate_hz2[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_noise_sigma2[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); @@ -470,6 +498,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() delete[] carrier_dopplerrate_hz2; delete[] code_freq_chips; delete[] carr_error_hz; + delete[] carr_noise_sigma2; delete[] carr_error_filt_hz; delete[] code_error_chips; delete[] code_error_filt_chips; @@ -543,6 +572,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("carr_noise_sigma2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_noise_sigma2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -589,6 +622,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile() delete[] carrier_dopplerrate_hz2; delete[] code_freq_chips; delete[] carr_error_hz; + delete[] carr_noise_sigma2; delete[] carr_error_filt_hz; delete[] code_error_chips; delete[] code_error_filt_chips; @@ -638,7 +672,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - double carr_phase_error_rad = 0.0; + d_carr_phase_error_rad = 0.0; double code_error_chips = 0.0; double code_error_filt_chips = 0.0; @@ -684,6 +718,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_x(2) = d_carrier_dopplerrate_hz2; } + // Covariance estimation initialization reset + kf_iter = 0; + bayes_estimator.init(arma::zeros(1,1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R)*(bayes_nu + 2)); + consume_each(samples_offset); // shift input to perform alignment with local replica return 1; } @@ -704,20 +742,35 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q; //state error covariance prediction // Update discriminator [rads/Ti] - carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output + d_carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output // Kalman estimation (measurement update) double sigma2_phase_detector_cycles2; double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0); sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)); + + kf_y(0) = d_carr_phase_error_rad; // measurement vector kf_R(0, 0) = sigma2_phase_detector_cycles2; - kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix - kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain - - kf_y(0) = carr_phase_error_rad; // measurement vector - kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation + if (bayes_run && (kf_iter >= bayes_ptrans)) + { + bayes_estimator.update_sequential(kf_y); + } + if (bayes_run && (kf_iter >= (bayes_ptrans + bayes_strans))) + { + // TODO: Resolve segmentation fault + kf_P_y = bayes_estimator.get_Psi_est(); + kf_R_est = kf_P_y - kf_H * kf_P_x_pre * kf_H.t(); + } + else + { + kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix + kf_R_est = kf_R; + } + // Kalman filter update step + kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain + kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation kf_P_x = (arma::eye(size(kf_P_x_pre)) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix // Store Kalman filter results @@ -731,6 +784,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus { d_carrier_dopplerrate_hz2 = 0; } + d_carr_phase_sigma2 = kf_R_est(0, 0); // ################## DLL ########################################################## // New code Doppler frequency estimation based on carrier frequency estimation @@ -780,7 +834,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { + //if (d_channel == 1) + //std::cout << "Carrier Lock Test Fail in channel " << d_channel << ": " << d_carrier_lock_test << " < " << d_carrier_lock_threshold << "," << nfail++ << std::endl; d_carrier_lock_fail_counter++; + //nfail++; } else { @@ -805,6 +862,9 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.correlation_length_ms = 1; + + kf_iter++; + } else { @@ -862,7 +922,9 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus tmp_float = d_code_freq_chips; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // Kalman commands - tmp_float = static_cast(carr_phase_error_rad * GPS_TWO_PI); + tmp_float = static_cast(d_carr_phase_error_rad * GPS_TWO_PI); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = static_cast(d_carr_phase_sigma2); d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = static_cast(d_rem_carr_phase_rad * GPS_TWO_PI); d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index e6b0e87ce..718613820 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -63,7 +63,12 @@ gps_l1_ca_kf_make_tracking_cc(unsigned int order, bool dump, std::string dump_filename, float pll_bw_hz, - float early_late_space_chips); + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa); /*! @@ -91,7 +96,12 @@ private: bool dump, std::string dump_filename, float dll_bw_hz, - float early_late_space_chips); + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa); Gps_L1_Ca_Kf_Tracking_cc(unsigned int order, long if_freq, @@ -99,7 +109,12 @@ private: bool dump, std::string dump_filename, float dll_bw_hz, - float early_late_space_chips); + float early_late_space_chips, + bool bce_run, + unsigned int bce_ptrans, + unsigned int bce_strans, + int bce_nu, + int bce_kappa); // tracking configuration vars unsigned int d_order; @@ -124,19 +139,27 @@ private: arma::mat kf_P_x; //state error covariance matrix arma::mat kf_P_x_pre; //Predicted state error covariance matrix arma::mat kf_P_y; //innovation covariance matrix + arma::mat kf_F; //state transition matrix arma::mat kf_H; //system matrix arma::mat kf_R; //measurement error covariance matrix arma::mat kf_Q; //system error covariance matrix + arma::colvec kf_x; //state vector arma::colvec kf_x_pre; //predicted state vector arma::colvec kf_y; //measurement vector - arma::colvec kf_y_pre; //measurement vector arma::mat kf_K; //Kalman gain matrix // Bayesian estimator - Bayesian_estimator cov_est; + Bayesian_estimator bayes_estimator; + arma::mat kf_R_est; //measurement error covariance + unsigned int bayes_ptrans; + unsigned int bayes_strans; + int bayes_nu; + int bayes_kappa; + bool bayes_run; + unsigned int kf_iter; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; @@ -160,6 +183,8 @@ private: double d_carrier_dopplerrate_hz2; double d_carrier_phase_step_rad; double d_acc_carrier_phase_rad; + double d_carr_phase_error_rad; + double d_carr_phase_sigma2; double d_code_phase_samples; double code_error_chips; double code_error_filt_chips; diff --git a/src/algorithms/tracking/libs/bayesian_estimation.cc b/src/algorithms/tracking/libs/bayesian_estimation.cc index abeb0b575..ed1065d44 100644 --- a/src/algorithms/tracking/libs/bayesian_estimation.cc +++ b/src/algorithms/tracking/libs/bayesian_estimation.cc @@ -42,8 +42,14 @@ Bayesian_estimator::Bayesian_estimator() { + int ny = 1; + mu_prior = arma::zeros(ny,1); kappa_prior = 0; nu_prior = 0; + Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); + + mu_est = mu_prior; + Psi_est = Psi_prior; } Bayesian_estimator::Bayesian_estimator(int ny) @@ -52,6 +58,9 @@ Bayesian_estimator::Bayesian_estimator(int ny) kappa_prior = 0; nu_prior = 0; Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); + + mu_est = mu_prior; + Psi_est = Psi_prior; } Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) @@ -60,12 +69,26 @@ Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, kappa_prior = kappa_prior_0; nu_prior = nu_prior_0; Psi_prior = Psi_prior_0; + + mu_est = mu_prior; + Psi_est = Psi_prior; } Bayesian_estimator::~Bayesian_estimator() { } +void Bayesian_estimator::init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) +{ + mu_prior = mu_prior_0; + kappa_prior = kappa_prior_0; + nu_prior = nu_prior_0; + Psi_prior = Psi_prior_0; + + mu_est = mu_prior; + Psi_est = Psi_prior; +} + /* * Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in * the class structure, and update the priors according to the computed posteriors @@ -152,10 +175,9 @@ void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0, kappa_prior = kappa_posterior; nu_prior = nu_posterior; Psi_prior = Psi_posterior; - } -arma::vec Bayesian_estimator::get_mu_est() +arma::mat Bayesian_estimator::get_mu_est() { return mu_est; } diff --git a/src/algorithms/tracking/libs/bayesian_estimation.h b/src/algorithms/tracking/libs/bayesian_estimation.h index 5cc524e44..44370cacb 100644 --- a/src/algorithms/tracking/libs/bayesian_estimation.h +++ b/src/algorithms/tracking/libs/bayesian_estimation.h @@ -65,14 +65,15 @@ public: Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); ~Bayesian_estimator(); + void init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); + void update_sequential(arma::vec data); void update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); - arma::vec get_mu_est(); + arma::mat get_mu_est(); arma::mat get_Psi_est(); private: - arma::vec mu_est; arma::mat Psi_est; From 0fd98b037920ba3b20e3a9541ee80649416409c7 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Mon, 13 Aug 2018 21:21:24 -0400 Subject: [PATCH 3/4] Add Matlab parsing and plotting functions for Kalman filter tracking block --- conf/gnss-sdr-kalman-bayes.conf | 63 +++++++ src/utils/matlab/gps_l1_ca_kf_plot_sample.m | 93 +++++++++++ .../libs/gps_l1_ca_kf_read_tracking_dump.m | 158 ++++++++++++++++++ src/utils/matlab/libs/plotKalman.m | 135 +++++++++++++++ 4 files changed, 449 insertions(+) create mode 100644 conf/gnss-sdr-kalman-bayes.conf create mode 100644 src/utils/matlab/gps_l1_ca_kf_plot_sample.m create mode 100644 src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m create mode 100644 src/utils/matlab/libs/plotKalman.m diff --git a/conf/gnss-sdr-kalman-bayes.conf b/conf/gnss-sdr-kalman-bayes.conf new file mode 100644 index 000000000..051d080de --- /dev/null +++ b/conf/gnss-sdr-kalman-bayes.conf @@ -0,0 +1,63 @@ +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second]. +GNSS-SDR.internal_fs_sps=2000000 +GNSS-SDR.internal_fs_hz=2000000 + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=File_Signal_Source +SignalSource.filename=/home/glamountain/gnss-sdr/data/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN.dat +SignalSource.item_type=ishort +SignalSource.sampling_frequency=4000000 +SignalSource.freq=1575420000 +SignalSource.samples=0 + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner +DataTypeAdapter.implementation=Ishort_To_Complex +InputFilter.implementation=Pass_Through +InputFilter.item_type=gr_complex +Resampler.implementation=Direct_Resampler +Resampler.sample_freq_in=4000000 +Resampler.sample_freq_out=2000000 +Resampler.item_type=gr_complex + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1C.count=8 +Channels.in_acquisition=1 +Channel.signal=1C + +;######### ACQUISITION GLOBAL CONFIG ############ +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.item_type=gr_complex +Acquisition_1C.threshold=0.008 +Acquisition_1C.doppler_max=10000 +Acquisition_1C.doppler_step=250 +Acquisition_1C.dump=false +Acquisition_1C.dump_filename=../data/kalman/acq_dump + +;######### TRACKING GLOBAL CONFIG ############ +Tracking_1C.implementation=GPS_L1_CA_KF_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.pll_bw_hz=40.0; +Tracking_1C.dll_bw_hz=4.0; +Tracking_1C.order=3; +Tracking_1C.dump=true +Tracking_1C.dump_filename=../data/kalman/epl_tracking_ch_ +Tracking_1C.bce_run = true; +Tracking_1C.p_transient = 0; +Tracking_1C.s_transient = 100; + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder + +;######### OBSERVABLES CONFIG ############ +Observables.implementation=GPS_L1_CA_Observables + +;######### PVT CONFIG ############ +PVT.implementation=GPS_L1_CA_PVT +PVT.averaging_depth=100 +PVT.flag_averaging=true +PVT.output_rate_ms=10 +PVT.display_rate_ms=500 diff --git a/src/utils/matlab/gps_l1_ca_kf_plot_sample.m b/src/utils/matlab/gps_l1_ca_kf_plot_sample.m new file mode 100644 index 000000000..f8fcb0ed7 --- /dev/null +++ b/src/utils/matlab/gps_l1_ca_kf_plot_sample.m @@ -0,0 +1,93 @@ +% Reads GNSS-SDR Tracking dump binary file using the provided +% function and plots some internal variables +% Javier Arribas, 2011. jarribas(at)cttc.es +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% +% GNSS-SDR is a software defined Global Navigation +% Satellite Systems receiver +% +% This file is part of GNSS-SDR. +% +% GNSS-SDR is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +close all; +clear all; + +if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') + addpath('./libs') +end + + +samplingFreq = 6625000; %[Hz] +channels = 8; +first_channel = 0; +code_period = 0.001; + +path = '/archive/'; %% CHANGE THIS PATH +figpath = [path]; + +for N=1:1:channels + tracking_log_path = [path 'epl_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename + GNSS_tracking(N) = gps_l1_ca_kf_read_tracking_dump(tracking_log_path); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).carrFreqRate = GNSS_tracking(N).carrier_dopplerrate_hz2.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; + trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; + + trackResults(N).I_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).PRN = GNSS_tracking(N).PRN.'; + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + + kalmanResults(N).PRN = GNSS_tracking(N).PRN.'; + kalmanResults(N).innovation = GNSS_tracking(N).carr_error.'; + kalmanResults(N).state1 = GNSS_tracking(N).carr_nco.'; + kalmanResults(N).state2 = GNSS_tracking(N).carrier_doppler_hz.'; + kalmanResults(N).state3 = GNSS_tracking(N).carrier_dopplerrate_hz2.'; + kalmanResults(N).r_noise_cov = GNSS_tracking(N).carr_noise_sigma2.'; + kalmanResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E); + settings.codePeriod = code_period; + settings.timeStartInSeconds = 20; + + %plotTracking(N, trackResults, settings) + plotKalman(N, kalmanResults, settings) + + saveas(gcf, [figpath 'epl_tracking_ch_' num2str(N) '_PRN_' num2str(trackResults(N).PRN(end)) '.png'], 'png') +end + + diff --git a/src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m b/src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m new file mode 100644 index 000000000..e1f1c8687 --- /dev/null +++ b/src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m @@ -0,0 +1,158 @@ +% Usage: gps_l1_ca_kf_read_tracking_dump (filename, [count]) +% +% Opens GNSS-SDR tracking binary log file .dat and returns the contents + +% Read GNSS-SDR Tracking dump binary file into MATLAB. +% Javier Arribas, 2011. jarribas(at)cttc.es +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% +% GNSS-SDR is a software defined Global Navigation +% Satellite Systems receiver +% +% This file is part of GNSS-SDR. +% +% GNSS-SDR is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +function [GNSS_tracking] = gps_l1_ca_kf_read_tracking_dump (filename, count) + +m = nargchk (1,2,nargin); + +num_float_vars = 19; +num_unsigned_long_int_vars = 1; +num_double_vars = 1; +num_unsigned_int_vars = 1; + +if(~isempty(strfind(computer('arch'), '64'))) + % 64-bit computer + double_size_bytes = 8; + unsigned_long_int_size_bytes = 8; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; +else + double_size_bytes = 8; + unsigned_long_int_size_bytes = 4; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; +end + +skip_bytes_each_read = float_size_bytes * num_float_vars + unsigned_long_int_size_bytes * num_unsigned_long_int_vars + ... + double_size_bytes * num_double_vars + num_unsigned_int_vars*unsigned_int_size_bytes; + +bytes_shift = 0; + +if (m) + usage (m); +end + +if (nargin < 2) + count = Inf; +end +%loops_counter = fread (f, count, 'uint32',4*12); +f = fopen (filename, 'rb'); +if (f < 0) +else + v1 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v2 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v3 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v4 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v5 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v6 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v7 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v8 = fread (f, count, 'long', skip_bytes_each_read - unsigned_long_int_size_bytes); + bytes_shift = bytes_shift + unsigned_long_int_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v9 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v10 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v11 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v12 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v13 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v14 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v15 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v18 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v19 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v20 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next double + v21 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes); + bytes_shift = bytes_shift + double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next unsigned int + v22 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes); + fclose (f); + + GNSS_tracking.VE = v1; + GNSS_tracking.E = v2; + GNSS_tracking.P = v3; + GNSS_tracking.L = v4; + GNSS_tracking.VL = v5; + GNSS_tracking.prompt_I = v6; + GNSS_tracking.prompt_Q = v7; + GNSS_tracking.PRN_start_sample = v8; + GNSS_tracking.acc_carrier_phase_rad = v9; + GNSS_tracking.carrier_doppler_hz = v10; + GNSS_tracking.carrier_dopplerrate_hz2 = v11; + GNSS_tracking.code_freq_hz = v12; + GNSS_tracking.carr_error = v13; + GNSS_tracking.carr_noise_sigma2 = v14; + GNSS_tracking.carr_nco = v15; + GNSS_tracking.code_error = v16; + GNSS_tracking.code_nco = v17; + GNSS_tracking.CN0_SNV_dB_Hz = v18; + GNSS_tracking.carrier_lock_test = v19; + GNSS_tracking.var1 = v20; + GNSS_tracking.var2 = v21; + GNSS_tracking.PRN = v22; +end \ No newline at end of file diff --git a/src/utils/matlab/libs/plotKalman.m b/src/utils/matlab/libs/plotKalman.m new file mode 100644 index 000000000..e05fd0d52 --- /dev/null +++ b/src/utils/matlab/libs/plotKalman.m @@ -0,0 +1,135 @@ +function plotKalman(channelList, trackResults, settings) +% This function plots the tracking results for the given channel list. +% +% plotTracking(channelList, trackResults, settings) +% +% Inputs: +% channelList - list of channels to be plotted. +% trackResults - tracking results from the tracking function. +% settings - receiver settings. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% Protection - if the list contains incorrect channel numbers +channelList = intersect(channelList, 1:settings.numberOfChannels); + +%=== For all listed channels ============================================== +for channelNr = channelList + + %% Select (or create) and clear the figure ================================ + % The number 200 is added just for more convenient handling of the open + % figure windows, when many figures are closed and reopened. + % Figures drawn or opened by the user, will not be "overwritten" by + % this function. + + figure(channelNr +200); + clf(channelNr +200); + set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... + ' (PRN ', ... + num2str(trackResults(channelNr).PRN(end-1)), ... + ') results']); + + timeStart = settings.timeStartInSeconds; + + %% Draw axes ============================================================== + % Row 1 + handles(1, 1) = subplot(4, 2, 1); + handles(1, 2) = subplot(4, 2, 2); + % Row 2 + handles(2, 1) = subplot(4, 2, 3); + handles(2, 2) = subplot(4, 2, 4); + % Row 3 + handles(3, 1) = subplot(4, 2, [5 6]); + % Row 4 + handles(4, 1) = subplot(4, 2, [7 8]); + + %% Plot all figures ======================================================= + + timeAxisInSeconds = (1:settings.msToProcess)/1000; + + %----- CNo for signal---------------------------------- + plot (handles(1, 1), timeAxisInSeconds, ... + trackResults(channelNr).CNo(1:settings.msToProcess), 'b'); + + grid (handles(1, 1)); + axis (handles(1, 1), 'tight'); + xlabel(handles(1, 1), 'Time (s)'); + ylabel(handles(1, 1), 'CNo (dB-Hz)'); + title (handles(1, 1), 'Carrier to Noise Ratio'); + + %----- PLL discriminator filtered---------------------------------- + plot (handles(1, 2), timeAxisInSeconds, ... + trackResults(channelNr).state1(1:settings.msToProcess), 'b'); + + grid (handles(1, 2)); + axis (handles(1, 2), 'tight'); + xlim (handles(1, 2), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(1, 2), 'Time (s)'); + ylabel(handles(1, 2), 'Phase Amplitude'); + title (handles(1, 2), 'Filtered Carrier Phase'); + + %----- Carrier Frequency -------------------------------- + plot (handles(2, 1), timeAxisInSeconds(2:end), ... + trackResults(channelNr).state2(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]); + + grid (handles(2, 1)); + axis (handles(2, 1)); + xlim (handles(2, 1), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(2, 1), 'Time (s)'); + ylabel(handles(2, 1), 'Freq (hz)'); + title (handles(2, 1), 'Filtered Doppler Frequency'); + + %----- Carrier Frequency Rate -------------------------------- + plot (handles(2, 2), timeAxisInSeconds(2:end), ... + trackResults(channelNr).state3(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]); + + grid (handles(2, 2)); + axis (handles(2, 2)); + xlim (handles(2, 2), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(2, 2), 'Time (s)'); + ylabel(handles(2, 2), 'Freq (hz)'); + title (handles(2, 2), 'Filtered Doppler Frequency Rate'); + + %----- PLL discriminator unfiltered-------------------------------- + plot (handles(3, 1), timeAxisInSeconds, ... + trackResults(channelNr).innovation, 'r'); + + grid (handles(3, 1)); + axis (handles(3, 1), 'auto'); + xlim (handles(3, 1), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(3, 1), 'Time (s)'); + ylabel(handles(3, 1), 'Amplitude'); + title (handles(3, 1), 'Raw PLL discriminator (Innovation)'); + + + %----- PLL discriminator covariance -------------------------------- + plot (handles(4, 1), timeAxisInSeconds, ... + trackResults(channelNr).r_noise_cov, 'r'); + + grid (handles(4, 1)); + axis (handles(4, 1), 'auto'); + xlim (handles(4, 1), [timeStart, timeAxisInSeconds(end)]); + xlabel(handles(4, 1), 'Time (s)'); + ylabel(handles(4, 1), 'Variance'); + title (handles(4, 1), 'Estimated Noise Variance'); +end % for channelNr = channelList From 05aea34e7b8e888ad668609c4ca766ada304cbac Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Tue, 14 Aug 2018 00:54:00 -0400 Subject: [PATCH 4/4] Add a unit test for bayesian_estimation.cc as part of trk_test which tests that all outputs are positive over a large number of iterations --- src/tests/CMakeLists.txt | 3 +- .../tracking/bayesian_estimation_test.cc | 80 +++++++++++++++++++ 2 files changed, 82 insertions(+), 1 deletion(-) create mode 100644 src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 54b3ac685..359f9d9fb 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -659,7 +659,8 @@ endif(NOT ${GTEST_DIR_LOCAL}) add_executable(trk_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc ) + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc ) target_link_libraries(trk_test ${Boost_LIBRARIES} ${GFlags_LIBS} diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc new file mode 100644 index 000000000..b9f9c9356 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc @@ -0,0 +1,80 @@ +/*! + * \file bayesian_estimation_positivity_test.cc + * \brief This file implements timing tests for the Bayesian covariance estimator + * \author Gerald LaMountain, 20168. gerald(at)ece.neu.edu + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "bayesian_estimation.h" + +#define BAYESIAN_TEST_N_TRIALS 100 +#define BAYESIAN_TEST_ITER 10000 + +TEST(BayesianEstimationPositivityTest, BayesianPositivityTest) +{ + Bayesian_estimator bayes; + std::chrono::time_point start, end; + std::chrono::duration elapsed_seconds(0); + + arma::vec bayes_mu = arma::zeros(1, 1); + int bayes_nu = 0; + int bayes_kappa = 0; + arma::mat bayes_Psi = arma::ones(1, 1); + + arma::vec input = arma::zeros(1, 1); + arma::mat output = arma::ones(1, 1); + + //--- Perform initializations ------------------------------ + + std::random_device r; + std::default_random_engine e1(r()); + std::normal_distribution normal_dist(0, 5); + + for (int k = 0; k < BAYESIAN_TEST_N_TRIALS; k++) + { + bayes.init(bayes_mu, bayes_kappa, bayes_nu, bayes_Psi); + + + for (int n = 0; n < BAYESIAN_TEST_ITER; n++) + { + input(0) = (double)(normal_dist(e1)); + bayes.update_sequential(input); + + output = bayes.get_Psi_est(); + ASSERT_EQ(output(0) > 0, true); + } + } +}