mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-18 21:23:02 +00:00
Code Issues Releases Wiki Activity

Updates to 3-state Kalman filter parameters to improve tracking

Browse Source
This commit is contained in:
Gerald LaMountain 2018-08-13 21:15:58 -04:00
parent ca4614811c
commit 032e73e727
2 changed files with 39 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc
View File

@ -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<double>(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<char *>(&PRN_start_sample_count[i]), sizeof(unsigned long int));
dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz[i]), sizeof(float));
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_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<char *>(&tmp_float), sizeof(float));
tmp_float = d_carrier_dopplerrate_hz2;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// PLL commands
// Kalman commands
tmp_float = static_cast<float>(carr_phase_error_rad * GPS_TWO_PI);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = static_cast<float>(d_rem_carr_phase_rad * GPS_TWO_PI);

1
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h
View File

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