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;