mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-04 01:03:04 +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
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_code_phase_samples = 0.0;
d_acq_carrier_doppler_hz = 0.0; d_acq_carrier_doppler_hz = 0.0;
d_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_acc_carrier_phase_rad = 0.0;
d_code_phase_samples = 0.0; d_code_phase_samples = 0.0;
d_rem_code_phase_chips = 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) //covariances (static)
double sigma2_carrier_phase = GPS_TWO_PI / 4; double sigma2_carrier_phase = GPS_TWO_PI / 4;
double sigma2_doppler = 450; 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 = arma::zeros(2, 2);
kf_P_x_ini(0, 0) = sigma2_carrier_phase; 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_R(0, 0) = sigma2_phase_detector_cycles2;
kf_Q = arma::zeros(2, 2); 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_Q(1, 1) = GPS_L1_CA_CODE_PERIOD;
kf_F = arma::zeros(2, 2); 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_x = arma::zeros(2, 1);
kf_y = arma::zeros(1, 1); kf_y = arma::zeros(1, 1);
kf_P_y = arma::zeros(1, 1);
// order three // order three
if (d_order == 3) 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_P_x_ini(2, 2) = sigma2_doppler_rate;
kf_Q = arma::zeros(3, 3); kf_Q = arma::zeros(3, 3);
kf_Q(0, 0) = pow(6, GPS_L1_CA_CODE_PERIOD); kf_Q(0, 0) = pow(GPS_L1_CA_CODE_PERIOD, 4);
kf_Q(1, 1) = pow(4, GPS_L1_CA_CODE_PERIOD); kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD;
kf_Q(2, 2) = pow(2, GPS_L1_CA_CODE_PERIOD); kf_Q(2, 2) = GPS_L1_CA_CODE_PERIOD;
kf_F = arma::resize(kf_F, 3, 3); 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(1, 2) = GPS_L1_CA_CODE_PERIOD;
kf_F(2, 0) = 0.0; kf_F(2, 0) = 0.0;
kf_F(2, 1) = 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_H(0, 2) = 0.0;
kf_x = arma::resize(kf_x, 3, 1); 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() 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) // Correct Kalman filter covariance according to acq doppler step size (3 sigma)
if (d_acquisition_gnss_synchro->Acq_doppler_step > 0) 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; 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_acq_code_phase_samples = corrected_acq_phase_samples;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz; 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); d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// DLL filter initialization // DLL filter initialization
@@ -372,7 +375,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
// READ DUMP FILE // READ DUMP FILE
std::ifstream::pos_type size; std::ifstream::pos_type size;
int number_of_double_vars = 1; 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 + int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(unsigned int); sizeof(float) * number_of_float_vars + sizeof(unsigned int);
std::ifstream dump_file; 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]; unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch];
float *acc_carrier_phase_rad = new float[num_epoch]; float *acc_carrier_phase_rad = new float[num_epoch];
float *carrier_doppler_hz = 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 *code_freq_chips = new float[num_epoch];
float *carr_error_hz = new float[num_epoch]; float *carr_error_hz = new float[num_epoch];
float *carr_error_filt_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 *>(&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 *>(&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_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 *>(&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_hz[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carr_error_filt_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[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad; delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz; delete[] carrier_doppler_hz;
delete[] carrier_dopplerrate_hz2;
delete[] code_freq_chips; delete[] code_freq_chips;
delete[] carr_error_hz; delete[] carr_error_hz;
delete[] carr_error_filt_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_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); 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); 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_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
@@ -576,6 +586,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
delete[] PRN_start_sample_count; delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad; delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz; delete[] carrier_doppler_hz;
delete[] carrier_dopplerrate_hz2;
delete[] code_freq_chips; delete[] code_freq_chips;
delete[] carr_error_hz; delete[] carr_error_hz;
delete[] carr_error_filt_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; kf_P_x = kf_P_x_ini;
// Update Kalman states based on acquisition information // Update Kalman states based on acquisition information
kf_x(0) = d_carrier_phase_step_rad * samples_offset; 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 consume_each(samples_offset); // shift input to perform alignment with local replica
return 1; 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 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_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 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 ########################################################## // ################## DLL ##########################################################
// New code Doppler frequency estimation based on carrier frequency estimation // 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 // carrier and code frequency
tmp_float = d_carrier_doppler_hz; tmp_float = d_carrier_doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float)); 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; tmp_float = d_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float)); 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); tmp_float = static_cast<float>(carr_phase_error_rad * GPS_TWO_PI);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float)); d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = static_cast<float>(d_rem_carr_phase_rad * GPS_TWO_PI); 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_freq_chips;
double d_code_phase_step_chips; double d_code_phase_step_chips;
double d_carrier_doppler_hz; double d_carrier_doppler_hz;
double d_carrier_dopplerrate_hz2;
double d_carrier_phase_step_rad; double d_carrier_phase_step_rad;
double d_acc_carrier_phase_rad; double d_acc_carrier_phase_rad;
double d_code_phase_samples; double d_code_phase_samples;