mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 12:40:35 +00:00
Merge branch 'glamountain-kf' into kf
This commit is contained in:
commit
f5c67d0c7d
63
conf/gnss-sdr-kalman-bayes.conf
Normal file
63
conf/gnss-sdr-kalman-bayes.conf
Normal 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
|
@ -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
|
||||||
{
|
{
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
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_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_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
|
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));
|
||||||
|
@ -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;
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
@ -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}
|
||||||
|
@ -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
93
src/utils/matlab/gps_l1_ca_kf_plot_sample.m
Normal 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
158
src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m
Normal 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
135
src/utils/matlab/libs/plotKalman.m
Normal 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
|
Loading…
Reference in New Issue
Block a user