From d565d655212a6b01ff3200d79c94d58a5af31689 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Tue, 3 Jul 2018 13:31:53 -0400 Subject: [PATCH 1/2] Update KF tracking to use acquisition doppler bin size for initial doppler state covariance --- .../acquisition/gnuradio_blocks/pcps_acquisition.cc | 2 ++ .../gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc | 11 +++++++++-- .../gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h | 1 + src/core/system_parameters/gnss_synchro.h | 1 + 4 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index 3513a41da..f4aea64d8 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -222,6 +222,7 @@ void pcps_acquisition::init() d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; + d_gnss_synchro->Acq_doppler_step = d_doppler_step; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0; @@ -282,6 +283,7 @@ void pcps_acquisition::set_state(int state) { d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; + d_gnss_synchro->Acq_doppler_step = 0; d_gnss_synchro->Acq_samplestamp_samples = 0; d_well_count = 0; d_mag = 0.0; 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 cc05dde31..ee6d29e4f 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 @@ -174,7 +174,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( // Kalman filter initialization (receiver initialization) - double CN_dB_Hz = 40; + double CN_dB_Hz = 30; double CN_lin = pow(10, CN_dB_Hz / 10.0); double sigma2_phase_detector_cycles2; @@ -182,7 +182,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 = 250; /// !! + double sigma2_doppler = 450; kf_P_x_ini = arma::zeros(2, 2); kf_P_x_ini(0, 0) = sigma2_carrier_phase; @@ -217,6 +217,13 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + d_acq_carrier_doppler_step_hz = static_cast(d_acquisition_gnss_synchro->Acq_doppler_step); + + // 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); + } long int acq_trk_diff_samples; double acq_trk_diff_seconds; 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 3e33491af..0d4ed779d 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 @@ -135,6 +135,7 @@ private: //Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition + double d_acq_carrier_doppler_step_hz; double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; // correlator diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 7d572dffa..90dd378d3 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -49,6 +49,7 @@ public: // Acquisition double Acq_delay_samples; //!< Set by Acquisition processing block double Acq_doppler_hz; //!< Set by Acquisition processing block + unsigned int Acq_doppler_step; //!< Set by Acquisition processing block unsigned long int Acq_samplestamp_samples; //!< Set by Acquisition processing block bool Flag_valid_acquisition; //!< Set by Acquisition processing block //Tracking From 0dd99e3c5d0242603039471eefa4c1476aee146a Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Tue, 3 Jul 2018 14:47:17 -0400 Subject: [PATCH 2/2] Add configurable model order to KF tracking (order 2, order 3) --- .../adapters/gps_l1_ca_kf_tracking.cc | 4 ++ .../gps_l1_ca_kf_tracking_cc.cc | 39 ++++++++++++++++--- .../gps_l1_ca_kf_tracking_cc.h | 10 +++-- 3 files changed, 45 insertions(+), 8 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 d1a6d56d0..e124d411a 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -53,6 +53,7 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## + int order; int fs_in; int vector_length; int f_if; @@ -63,7 +64,9 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( float pll_bw_hz; float dll_bw_hz; float early_late_space_chips; + item_type = configuration->property(role + ".item_type", default_item_type); + order = configuration->property(role + ".order", 2); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); f_if = configuration->property(role + ".if", 0); @@ -80,6 +83,7 @@ GpsL1CaKfTracking::GpsL1CaKfTracking( { item_size_ = sizeof(gr_complex); tracking_ = gps_l1_ca_kf_make_tracking_cc( + order, f_if, fs_in, vector_length, 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 ee6d29e4f..14d0bd16b 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 @@ -59,6 +59,7 @@ using google::LogMessage; gps_l1_ca_kf_tracking_cc_sptr gps_l1_ca_kf_make_tracking_cc( + unsigned int order, long if_freq, long fs_in, unsigned int vector_length, @@ -67,7 +68,7 @@ gps_l1_ca_kf_make_tracking_cc( float dll_bw_hz, float early_late_space_chips) { - return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(if_freq, + 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)); } @@ -83,6 +84,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::forecast(int noutput_items, Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( + unsigned int order, long if_freq, long fs_in, unsigned int vector_length, @@ -97,6 +99,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( this->message_port_register_out(pmt::mp("events")); // initialize internal vars + d_order = order; d_dump = dump; d_if_freq = if_freq; d_fs_in = fs_in; @@ -182,7 +185,8 @@ 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 = 450; + double sigma2_doppler_rate = 1.0 / 24.0; kf_P_x_ini = arma::zeros(2, 2); kf_P_x_ini(0, 0) = sigma2_carrier_phase; @@ -192,8 +196,8 @@ 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) = 1e-14; - kf_Q(1, 1) = 1e-2; + kf_Q(0, 0) = pow(4, GPS_L1_CA_CODE_PERIOD); + kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD; kf_F = arma::zeros(2, 2); kf_F(0, 0) = 1.0; @@ -206,6 +210,31 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( kf_x = arma::zeros(2, 1); kf_y = arma::zeros(1, 1); + + // order three + if (d_order == 3) + { + kf_P_x_ini = arma::resize(kf_P_x_ini, 3, 3); + 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_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(1, 2) = GPS_L1_CA_CODE_PERIOD; + kf_F(2, 0) = 0.0; + kf_F(2, 1) = 0.0; + kf_F(2, 2) = 1.0; + + kf_H = arma::resize(kf_H, 1, 3); + kf_H(0, 2) = 0.0; + + kf_x = arma::resize(kf_x, 3, 1); + kf_x(2, 0) = -0.25; + } } @@ -674,7 +703,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus kf_y(0) = carr_phase_error_rad; // measurement vector kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation - kf_P_x = (arma::eye(2, 2) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix + kf_P_x = (arma::eye(size(kf_P_x_pre)) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix 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 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 0d4ed779d..12f80ac3d 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 @@ -56,7 +56,8 @@ typedef boost::shared_ptr gps_l1_ca_kf_tracking_cc_sptr; gps_l1_ca_kf_tracking_cc_sptr -gps_l1_ca_kf_make_tracking_cc(long if_freq, +gps_l1_ca_kf_make_tracking_cc(unsigned int order, + long if_freq, long fs_in, unsigned int vector_length, bool dump, std::string dump_filename, @@ -83,14 +84,16 @@ public: private: friend gps_l1_ca_kf_tracking_cc_sptr - gps_l1_ca_kf_make_tracking_cc(long if_freq, + gps_l1_ca_kf_make_tracking_cc(unsigned int order, + long if_freq, long fs_in, unsigned int vector_length, bool dump, std::string dump_filename, float dll_bw_hz, float early_late_space_chips); - Gps_L1_Ca_Kf_Tracking_cc(long if_freq, + Gps_L1_Ca_Kf_Tracking_cc(unsigned int order, + long if_freq, long fs_in, unsigned int vector_length, bool dump, std::string dump_filename, @@ -98,6 +101,7 @@ private: float early_late_space_chips); // tracking configuration vars + unsigned int d_order; unsigned int d_vector_length; bool d_dump;