From e42467a06868561449328d926a75057aee46c431 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Mon, 13 Aug 2018 21:18:01 -0400 Subject: [PATCH] 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;