Updates to integration of bayesian_estimation library into GPS L1 kalman tracking block

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
         {

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<char *>(&carrier_dopplerrate_hz2[i]), sizeof(float));
                             dump_file.read(reinterpret_cast<char *>(&code_freq_chips[i]), sizeof(float));
                             dump_file.read(reinterpret_cast<char *>(&carr_error_hz[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&carr_noise_sigma2[i]), sizeof(float));
                             dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz[i]), sizeof(float));
                             dump_file.read(reinterpret_cast<char *>(&code_error_chips[i]), sizeof(float));
                             dump_file.read(reinterpret_cast<char *>(&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
+            if (bayes_run && (kf_iter >= bayes_ptrans))
-            kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y);  // Kalman gain
+                {
-
+                    bayes_estimator.update_sequential(kf_y);
-            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_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<char *>(&tmp_float), sizeof(float));
                     // Kalman commands
-                    tmp_float = static_cast<float>(carr_phase_error_rad * GPS_TWO_PI);
+                    tmp_float = static_cast<float>(d_carr_phase_error_rad * GPS_TWO_PI);
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
+                    tmp_float = static_cast<float>(d_carr_phase_sigma2);
                     d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
                     tmp_float = static_cast<float>(d_rem_carr_phase_rad * GPS_TWO_PI);
                     d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));

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;

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;
 }

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;