Skip to content
Permalink
Browse files

Updates to integration of bayesian_estimation library into GPS L1 kal…

…man tracking block
  • Loading branch information
glamountain committed Aug 14, 2018
1 parent 032e73e commit e42467a06868561449328d926a75057aee46c431
@@ -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
{
@@ -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_R(0, 0) = sigma2_phase_detector_cycles2;

kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix
kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain
kf_y(0) = d_carr_phase_error_rad; // measurement vector
kf_R(0, 0) = sigma2_phase_detector_cycles2;

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_estimator.update_sequential(kf_y);
}
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));
@@ -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,15 +96,25 @@ class Gps_L1_Ca_Kf_Tracking_cc : public gr::block
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,
long fs_in, unsigned int vector_length,
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 @@ class Gps_L1_Ca_Kf_Tracking_cc : public gr::block
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 @@ class Gps_L1_Ca_Kf_Tracking_cc : public gr::block
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;

0 comments on commit e42467a

Please sign in to comment.
You can’t perform that action at this time.