mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 04:30:33 +00:00
Code Issues Releases Wiki Activity

Updates to integration of bayesian_estimation library into GPS L1 kalman tracking block

Browse Source
This commit is contained in:
Gerald LaMountain 2018-08-13 21:18:01 -04:00
parent 032e73e727
commit e42467a068
5 changed files with 148 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc
View File

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

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

@ -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_y(0) = d_carr_phase_error_rad; // measurement vector
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) = 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));

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

@ -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,7 +96,12 @@ private:
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,
@ -99,7 +109,12 @@ private:
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 @@ private:
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 @@ private:
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;

26
src/algorithms/tracking/libs/bayesian_estimation.cc
View File

@ -42,8 +42,14 @@
Bayesian_estimator::Bayesian_estimator()
{
int ny = 1;
mu_prior = arma::zeros(ny,1);
kappa_prior = 0;
nu_prior = 0;
Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1);
mu_est = mu_prior;
Psi_est = Psi_prior;
}
Bayesian_estimator::Bayesian_estimator(int ny)
@ -52,6 +58,9 @@ Bayesian_estimator::Bayesian_estimator(int ny)
kappa_prior = 0;
nu_prior = 0;
Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1);
mu_est = mu_prior;
Psi_est = Psi_prior;
}
Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0)
@ -60,12 +69,26 @@ Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0,
kappa_prior = kappa_prior_0;
nu_prior = nu_prior_0;
Psi_prior = Psi_prior_0;
mu_est = mu_prior;
Psi_est = Psi_prior;
}
Bayesian_estimator::~Bayesian_estimator()
{
}
void Bayesian_estimator::init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0)
{
mu_prior = mu_prior_0;
kappa_prior = kappa_prior_0;
nu_prior = nu_prior_0;
Psi_prior = Psi_prior_0;
mu_est = mu_prior;
Psi_est = Psi_prior;
}
/*
* Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in
* the class structure, and update the priors according to the computed posteriors
@ -152,10 +175,9 @@ void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0,
kappa_prior = kappa_posterior;
nu_prior = nu_posterior;
Psi_prior = Psi_posterior;
}
arma::vec Bayesian_estimator::get_mu_est()
arma::mat Bayesian_estimator::get_mu_est()
{
return mu_est;
}

5
src/algorithms/tracking/libs/bayesian_estimation.h
View File

@ -65,14 +65,15 @@ public:
Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0);
~Bayesian_estimator();
void init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0);
void update_sequential(arma::vec data);
void update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0);
arma::vec get_mu_est();
arma::mat get_mu_est();
arma::mat get_Psi_est();
private:
arma::vec mu_est;
arma::mat Psi_est;