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

Merge branch 'glamountain-kf' into kf

Browse Source
This commit is contained in:
Carles Fernandez 2018-08-21 11:03:25 +02:00
commit f5c67d0c7d
11 changed files with 717 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
conf/gnss-sdr-kalman-bayes.conf Normal file
View File

@ -0,0 +1,63 @@
[GNSS-SDR]
;######### GLOBAL OPTIONS ##################
;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second].
GNSS-SDR.internal_fs_sps=2000000
GNSS-SDR.internal_fs_hz=2000000
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=File_Signal_Source
SignalSource.filename=/home/glamountain/gnss-sdr/data/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN.dat
SignalSource.item_type=ishort
SignalSource.sampling_frequency=4000000
SignalSource.freq=1575420000
SignalSource.samples=0
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Signal_Conditioner
DataTypeAdapter.implementation=Ishort_To_Complex
InputFilter.implementation=Pass_Through
InputFilter.item_type=gr_complex
Resampler.implementation=Direct_Resampler
Resampler.sample_freq_in=4000000
Resampler.sample_freq_out=2000000
Resampler.item_type=gr_complex
;######### CHANNELS GLOBAL CONFIG ############
Channels_1C.count=8
Channels.in_acquisition=1
Channel.signal=1C
;######### ACQUISITION GLOBAL CONFIG ############
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition_1C.item_type=gr_complex
Acquisition_1C.threshold=0.008
Acquisition_1C.doppler_max=10000
Acquisition_1C.doppler_step=250
Acquisition_1C.dump=false
Acquisition_1C.dump_filename=../data/kalman/acq_dump
;######### TRACKING GLOBAL CONFIG ############
Tracking_1C.implementation=GPS_L1_CA_KF_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.pll_bw_hz=40.0;
Tracking_1C.dll_bw_hz=4.0;
Tracking_1C.order=3;
Tracking_1C.dump=true
Tracking_1C.dump_filename=../data/kalman/epl_tracking_ch_
Tracking_1C.bce_run = true;
Tracking_1C.p_transient = 0;
Tracking_1C.s_transient = 100;
;######### TELEMETRY DECODER GPS CONFIG ############
TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder
;######### OBSERVABLES CONFIG ############
Observables.implementation=GPS_L1_CA_Observables
;######### PVT CONFIG ############
PVT.implementation=GPS_L1_CA_PVT
PVT.averaging_depth=100
PVT.flag_averaging=true
PVT.output_rate_ms=10
PVT.display_rate_ms=500

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

@ -64,6 +64,11 @@ GpsL1CaKfTracking::GpsL1CaKfTracking(
float pll_bw_hz; float pll_bw_hz;
float dll_bw_hz; 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;
item_type = configuration->property(role + ".item_type", default_item_type); item_type = configuration->property(role + ".item_type", default_item_type);
order = configuration->property(role + ".order", 2); order = configuration->property(role + ".order", 2);
@ -78,6 +83,12 @@ GpsL1CaKfTracking::GpsL1CaKfTracking(
dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); 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)); 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 ################### //################# MAKE TRACKING GNURadio object ###################
if (item_type.compare("gr_complex") == 0) if (item_type.compare("gr_complex") == 0)
{ {
@ -90,7 +101,12 @@ GpsL1CaKfTracking::GpsL1CaKfTracking(
dump, dump,
dump_filename, dump_filename,
dll_bw_hz, dll_bw_hz,
early_late_space_chips); early_late_space_chips,
bce_run,
bce_ptrans,
bce_strans,
bce_nu,
bce_kappa);
} }
else else
{ {

134
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, bool dump,
std::string dump_filename, std::string dump_filename,
float dll_bw_hz, 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, 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, bool dump,
std::string dump_filename, std::string dump_filename,
float dll_bw_hz, 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))) gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{ {
// Telemetry bit synchronization message port input // 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; d_rem_code_phase_samples = 0.0;
// define residual carrier phase // define residual carrier phase
d_rem_carr_phase_rad = 0.0; d_rem_carr_phase_rad = 0.0;
// define residual carrier phase covariance
d_carr_phase_sigma2 = 0.0;
// sample synchronization // sample synchronization
d_sample_counter = 0; d_sample_counter = 0;
@ -165,6 +178,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 +200,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 +210,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 +224,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 +233,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 +248,21 @@ 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;
} }
}
// 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() void Gps_L1_Ca_Kf_Tracking_cc::start_tracking()
{ {
@ -251,7 +277,8 @@ 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);
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; long int acq_trk_diff_samples;
@ -288,6 +315,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
@ -307,6 +335,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking()
d_rem_carr_phase_rad = 0.0; d_rem_carr_phase_rad = 0.0;
d_rem_code_phase_chips = 0.0; d_rem_code_phase_chips = 0.0;
d_acc_carrier_phase_rad = 0.0; d_acc_carrier_phase_rad = 0.0;
d_carr_phase_sigma2 = 0.0;
d_code_phase_samples = d_acq_code_phase_samples; d_code_phase_samples = d_acq_code_phase_samples;
@ -372,7 +401,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 = 19;
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,8 +437,10 @@ 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_noise_sigma2 = new float[num_epoch];
float *carr_error_filt_hz = new float[num_epoch]; float *carr_error_filt_hz = new float[num_epoch];
float *code_error_chips = new float[num_epoch]; float *code_error_chips = new float[num_epoch];
float *code_error_filt_chips = new float[num_epoch]; float *code_error_filt_chips = new float[num_epoch];
@ -435,8 +466,10 @@ 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_noise_sigma2[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));
dump_file.read(reinterpret_cast<char *>(&code_error_chips[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)); dump_file.read(reinterpret_cast<char *>(&code_error_filt_chips[i]), sizeof(float));
@ -462,8 +495,10 @@ 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_noise_sigma2;
delete[] carr_error_filt_hz; delete[] carr_error_filt_hz;
delete[] code_error_chips; delete[] code_error_chips;
delete[] code_error_filt_chips; delete[] code_error_filt_chips;
@ -525,6 +560,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);
@ -533,6 +572,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("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); 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_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
@ -576,8 +619,10 @@ 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_noise_sigma2;
delete[] carr_error_filt_hz; delete[] carr_error_filt_hz;
delete[] code_error_chips; delete[] code_error_chips;
delete[] code_error_filt_chips; delete[] code_error_filt_chips;
@ -627,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) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
// process vars // 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_chips = 0.0;
double code_error_filt_chips = 0.0; double code_error_filt_chips = 0.0;
@ -667,7 +712,15 @@ 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;
}
// 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 consume_each(samples_offset); // shift input to perform alignment with local replica
return 1; return 1;
@ -689,24 +742,49 @@ 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 kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q; //state error covariance prediction
// Update discriminator [rads/Ti] // 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) // Kalman estimation (measurement update)
double sigma2_phase_detector_cycles2; double sigma2_phase_detector_cycles2;
double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0); 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)); 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_R(0, 0) = sigma2_phase_detector_cycles2;
kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix if (bayes_run && (kf_iter >= bayes_ptrans))
kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain {
bayes_estimator.update_sequential(kf_y);
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_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 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;
}
d_carr_phase_sigma2 = kf_R_est(0, 0);
// ################## DLL ########################################################## // ################## DLL ##########################################################
// New code Doppler frequency estimation based on carrier frequency estimation // New code Doppler frequency estimation based on carrier frequency estimation
@ -756,7 +834,10 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus
// Loss of lock detection // Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) 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++; d_carrier_lock_fail_counter++;
//nfail++;
} }
else else
{ {
@ -781,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.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms = 1; current_synchro_data.correlation_length_ms = 1;
kf_iter++;
} }
else else
{ {
@ -833,10 +917,14 @@ 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>(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)); 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);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float)); d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));

36
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, bool dump,
std::string dump_filename, std::string dump_filename,
float pll_bw_hz, 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, bool dump,
std::string dump_filename, std::string dump_filename,
float dll_bw_hz, 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, Gps_L1_Ca_Kf_Tracking_cc(unsigned int order,
long if_freq, long if_freq,
@ -99,7 +109,12 @@ private:
bool dump, bool dump,
std::string dump_filename, std::string dump_filename,
float dll_bw_hz, 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 // tracking configuration vars
unsigned int d_order; unsigned int d_order;
@ -124,19 +139,27 @@ private:
arma::mat kf_P_x; //state error covariance matrix arma::mat kf_P_x; //state error covariance matrix
arma::mat kf_P_x_pre; //Predicted 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_P_y; //innovation covariance matrix
arma::mat kf_F; //state transition matrix arma::mat kf_F; //state transition matrix
arma::mat kf_H; //system matrix arma::mat kf_H; //system matrix
arma::mat kf_R; //measurement error covariance matrix arma::mat kf_R; //measurement error covariance matrix
arma::mat kf_Q; //system error covariance matrix arma::mat kf_Q; //system error covariance matrix
arma::colvec kf_x; //state vector arma::colvec kf_x; //state vector
arma::colvec kf_x_pre; //predicted state vector arma::colvec kf_x_pre; //predicted state vector
arma::colvec kf_y; //measurement vector arma::colvec kf_y; //measurement vector
arma::colvec kf_y_pre; //measurement vector
arma::mat kf_K; //Kalman gain matrix arma::mat kf_K; //Kalman gain matrix
// Bayesian estimator // 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 // PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_DLL_filter d_code_loop_filter;
@ -157,8 +180,11 @@ 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_carr_phase_error_rad;
double d_carr_phase_sigma2;
double d_code_phase_samples; double d_code_phase_samples;
double code_error_chips; double code_error_chips;
double code_error_filt_chips; double code_error_filt_chips;

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

@ -42,8 +42,14 @@
Bayesian_estimator::Bayesian_estimator() Bayesian_estimator::Bayesian_estimator()
{ {
int ny = 1;
mu_prior = arma::zeros(ny,1);
kappa_prior = 0; kappa_prior = 0;
nu_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) Bayesian_estimator::Bayesian_estimator(int ny)
@ -52,6 +58,9 @@ Bayesian_estimator::Bayesian_estimator(int ny)
kappa_prior = 0; kappa_prior = 0;
nu_prior = 0; nu_prior = 0;
Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); 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) 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; kappa_prior = kappa_prior_0;
nu_prior = nu_prior_0; nu_prior = nu_prior_0;
Psi_prior = Psi_prior_0; Psi_prior = Psi_prior_0;
mu_est = mu_prior;
Psi_est = Psi_prior;
} }
Bayesian_estimator::~Bayesian_estimator() 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 * Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in
* the class structure, and update the priors according to the computed posteriors * 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; kappa_prior = kappa_posterior;
nu_prior = nu_posterior; nu_prior = nu_posterior;
Psi_prior = Psi_posterior; Psi_prior = Psi_posterior;
} }
arma::vec Bayesian_estimator::get_mu_est() arma::mat Bayesian_estimator::get_mu_est()
{ {
return 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(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0);
~Bayesian_estimator(); ~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);
void update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); 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(); arma::mat get_Psi_est();
private: private:
arma::vec mu_est; arma::vec mu_est;
arma::mat Psi_est; arma::mat Psi_est;

3
src/tests/CMakeLists.txt
View File

@ -659,7 +659,8 @@ endif(NOT ${GTEST_DIR_LOCAL})
add_executable(trk_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc add_executable(trk_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc ) ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc )
target_link_libraries(trk_test ${Boost_LIBRARIES} target_link_libraries(trk_test ${Boost_LIBRARIES}
${GFlags_LIBS} ${GFlags_LIBS}

80
src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc Normal file
View File

@ -0,0 +1,80 @@
/*!
* \file bayesian_estimation_positivity_test.cc
* \brief This file implements timing tests for the Bayesian covariance estimator
* \author Gerald LaMountain, 20168. gerald(at)ece.neu.edu
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include <chrono>
#include <complex>
#include <random>
#include <thread>
#include <armadillo>
#include <gtest/gtest.h>
#include <gflags/gflags.h>
#include <gnuradio/gr_complex.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include "bayesian_estimation.h"
#define BAYESIAN_TEST_N_TRIALS 100
#define BAYESIAN_TEST_ITER 10000
TEST(BayesianEstimationPositivityTest, BayesianPositivityTest)
{
Bayesian_estimator bayes;
std::chrono::time_point<std::chrono::system_clock> start, end;
std::chrono::duration<double> elapsed_seconds(0);
arma::vec bayes_mu = arma::zeros(1, 1);
int bayes_nu = 0;
int bayes_kappa = 0;
arma::mat bayes_Psi = arma::ones(1, 1);
arma::vec input = arma::zeros(1, 1);
arma::mat output = arma::ones(1, 1);
//--- Perform initializations ------------------------------
std::random_device r;
std::default_random_engine e1(r());
std::normal_distribution<float> normal_dist(0, 5);
for (int k = 0; k < BAYESIAN_TEST_N_TRIALS; k++)
{
bayes.init(bayes_mu, bayes_kappa, bayes_nu, bayes_Psi);
for (int n = 0; n < BAYESIAN_TEST_ITER; n++)
{
input(0) = (double)(normal_dist(e1));
bayes.update_sequential(input);
output = bayes.get_Psi_est();
ASSERT_EQ(output(0) > 0, true);
}
}
}

93
src/utils/matlab/gps_l1_ca_kf_plot_sample.m Normal file
View File

@ -0,0 +1,93 @@
% Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es
% -------------------------------------------------------------------------
%
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
%
% GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver
%
% This file is part of GNSS-SDR.
%
% GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% at your option) any later version.
%
% GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
%
% -------------------------------------------------------------------------
%
close all;
clear all;
if ~exist('dll_pll_veml_read_tracking_dump.m', 'file')
addpath('./libs')
end
samplingFreq = 6625000; %[Hz]
channels = 8;
first_channel = 0;
code_period = 0.001;
path = '/archive/'; %% CHANGE THIS PATH
figpath = [path];
for N=1:1:channels
tracking_log_path = [path 'epl_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename
GNSS_tracking(N) = gps_l1_ca_kf_read_tracking_dump(tracking_log_path);
end
% GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels
trackResults(N).status = 'T'; %fake track
trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.';
trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.';
trackResults(N).carrFreqRate = GNSS_tracking(N).carrier_dopplerrate_hz2.';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error.';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.';
trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.';
trackResults(N).I_P = GNSS_tracking(N).prompt_I.';
trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.';
trackResults(N).I_E = GNSS_tracking(N).E.';
trackResults(N).I_L = GNSS_tracking(N).L.';
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).PRN = GNSS_tracking(N).PRN.';
trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
kalmanResults(N).PRN = GNSS_tracking(N).PRN.';
kalmanResults(N).innovation = GNSS_tracking(N).carr_error.';
kalmanResults(N).state1 = GNSS_tracking(N).carr_nco.';
kalmanResults(N).state2 = GNSS_tracking(N).carrier_doppler_hz.';
kalmanResults(N).state3 = GNSS_tracking(N).carrier_dopplerrate_hz2.';
kalmanResults(N).r_noise_cov = GNSS_tracking(N).carr_noise_sigma2.';
kalmanResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
% Use original MATLAB tracking plot function
settings.numberOfChannels = channels;
settings.msToProcess = length(GNSS_tracking(N).E);
settings.codePeriod = code_period;
settings.timeStartInSeconds = 20;
%plotTracking(N, trackResults, settings)
plotKalman(N, kalmanResults, settings)
saveas(gcf, [figpath 'epl_tracking_ch_' num2str(N) '_PRN_' num2str(trackResults(N).PRN(end)) '.png'], 'png')
end

158
src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m Normal file
View File

@ -0,0 +1,158 @@
% Usage: gps_l1_ca_kf_read_tracking_dump (filename, [count])
%
% Opens GNSS-SDR tracking binary log file .dat and returns the contents
% Read GNSS-SDR Tracking dump binary file into MATLAB.
% Javier Arribas, 2011. jarribas(at)cttc.es
% -------------------------------------------------------------------------
%
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
%
% GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver
%
% This file is part of GNSS-SDR.
%
% GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% at your option) any later version.
%
% GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
%
% -------------------------------------------------------------------------
%
function [GNSS_tracking] = gps_l1_ca_kf_read_tracking_dump (filename, count)
m = nargchk (1,2,nargin);
num_float_vars = 19;
num_unsigned_long_int_vars = 1;
num_double_vars = 1;
num_unsigned_int_vars = 1;
if(~isempty(strfind(computer('arch'), '64')))
% 64-bit computer
double_size_bytes = 8;
unsigned_long_int_size_bytes = 8;
float_size_bytes = 4;
unsigned_int_size_bytes = 4;
else
double_size_bytes = 8;
unsigned_long_int_size_bytes = 4;
float_size_bytes = 4;
unsigned_int_size_bytes = 4;
end
skip_bytes_each_read = float_size_bytes * num_float_vars + unsigned_long_int_size_bytes * num_unsigned_long_int_vars + ...
double_size_bytes * num_double_vars + num_unsigned_int_vars*unsigned_int_size_bytes;
bytes_shift = 0;
if (m)
usage (m);
end
if (nargin < 2)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
v1 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v2 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v3 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v4 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v5 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v6 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v7 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v8 = fread (f, count, 'long', skip_bytes_each_read - unsigned_long_int_size_bytes);
bytes_shift = bytes_shift + unsigned_long_int_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v9 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v10 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v11 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v12 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v13 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v14 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v15 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v18 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v19 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v20 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next double
v21 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes);
bytes_shift = bytes_shift + double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next unsigned int
v22 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes);
fclose (f);
GNSS_tracking.VE = v1;
GNSS_tracking.E = v2;
GNSS_tracking.P = v3;
GNSS_tracking.L = v4;
GNSS_tracking.VL = v5;
GNSS_tracking.prompt_I = v6;
GNSS_tracking.prompt_Q = v7;
GNSS_tracking.PRN_start_sample = v8;
GNSS_tracking.acc_carrier_phase_rad = v9;
GNSS_tracking.carrier_doppler_hz = v10;
GNSS_tracking.carrier_dopplerrate_hz2 = v11;
GNSS_tracking.code_freq_hz = v12;
GNSS_tracking.carr_error = v13;
GNSS_tracking.carr_noise_sigma2 = v14;
GNSS_tracking.carr_nco = v15;
GNSS_tracking.code_error = v16;
GNSS_tracking.code_nco = v17;
GNSS_tracking.CN0_SNV_dB_Hz = v18;
GNSS_tracking.carrier_lock_test = v19;
GNSS_tracking.var1 = v20;
GNSS_tracking.var2 = v21;
GNSS_tracking.PRN = v22;
end

135
src/utils/matlab/libs/plotKalman.m Normal file
View File

@ -0,0 +1,135 @@
function plotKalman(channelList, trackResults, settings)
% This function plots the tracking results for the given channel list.
%
% plotTracking(channelList, trackResults, settings)
%
% Inputs:
% channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function.
% settings - receiver settings.
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
%USA.
%--------------------------------------------------------------------------
% Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ==============================================
for channelNr = channelList
%% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by
% this function.
figure(channelNr +200);
clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ...
num2str(trackResults(channelNr).PRN(end-1)), ...
') results']);
timeStart = settings.timeStartInSeconds;
%% Draw axes ==============================================================
% Row 1
handles(1, 1) = subplot(4, 2, 1);
handles(1, 2) = subplot(4, 2, 2);
% Row 2
handles(2, 1) = subplot(4, 2, 3);
handles(2, 2) = subplot(4, 2, 4);
% Row 3
handles(3, 1) = subplot(4, 2, [5 6]);
% Row 4
handles(4, 1) = subplot(4, 2, [7 8]);
%% Plot all figures =======================================================
timeAxisInSeconds = (1:settings.msToProcess)/1000;
%----- CNo for signal----------------------------------
plot (handles(1, 1), timeAxisInSeconds, ...
trackResults(channelNr).CNo(1:settings.msToProcess), 'b');
grid (handles(1, 1));
axis (handles(1, 1), 'tight');
xlabel(handles(1, 1), 'Time (s)');
ylabel(handles(1, 1), 'CNo (dB-Hz)');
title (handles(1, 1), 'Carrier to Noise Ratio');
%----- PLL discriminator filtered----------------------------------
plot (handles(1, 2), timeAxisInSeconds, ...
trackResults(channelNr).state1(1:settings.msToProcess), 'b');
grid (handles(1, 2));
axis (handles(1, 2), 'tight');
xlim (handles(1, 2), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(1, 2), 'Time (s)');
ylabel(handles(1, 2), 'Phase Amplitude');
title (handles(1, 2), 'Filtered Carrier Phase');
%----- Carrier Frequency --------------------------------
plot (handles(2, 1), timeAxisInSeconds(2:end), ...
trackResults(channelNr).state2(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]);
grid (handles(2, 1));
axis (handles(2, 1));
xlim (handles(2, 1), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(2, 1), 'Time (s)');
ylabel(handles(2, 1), 'Freq (hz)');
title (handles(2, 1), 'Filtered Doppler Frequency');
%----- Carrier Frequency Rate --------------------------------
plot (handles(2, 2), timeAxisInSeconds(2:end), ...
trackResults(channelNr).state3(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]);
grid (handles(2, 2));
axis (handles(2, 2));
xlim (handles(2, 2), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(2, 2), 'Time (s)');
ylabel(handles(2, 2), 'Freq (hz)');
title (handles(2, 2), 'Filtered Doppler Frequency Rate');
%----- PLL discriminator unfiltered--------------------------------
plot (handles(3, 1), timeAxisInSeconds, ...
trackResults(channelNr).innovation, 'r');
grid (handles(3, 1));
axis (handles(3, 1), 'auto');
xlim (handles(3, 1), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(3, 1), 'Time (s)');
ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Raw PLL discriminator (Innovation)');
%----- PLL discriminator covariance --------------------------------
plot (handles(4, 1), timeAxisInSeconds, ...
trackResults(channelNr).r_noise_cov, 'r');
grid (handles(4, 1));
axis (handles(4, 1), 'auto');
xlim (handles(4, 1), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(4, 1), 'Time (s)');
ylabel(handles(4, 1), 'Variance');
title (handles(4, 1), 'Estimated Noise Variance');
end % for channelNr = channelList