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-07-04 13:06:48 +02:00
commit 29dc1730c4
5 changed files with 57 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@ -222,6 +222,7 @@ void pcps_acquisition::init()
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
@ -282,6 +283,7 @@ void pcps_acquisition::set_state(int state)
{
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_doppler_step = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_well_count = 0;
d_mag = 0.0;

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

@ -53,6 +53,7 @@ GpsL1CaKfTracking::GpsL1CaKfTracking(
{
DLOG(INFO) << "role " << role;
//################# CONFIGURATION PARAMETERS ########################
int order;
int fs_in;
int vector_length;
int f_if;
@ -63,7 +64,9 @@ GpsL1CaKfTracking::GpsL1CaKfTracking(
float pll_bw_hz;
float dll_bw_hz;
float early_late_space_chips;
item_type = configuration->property(role + ".item_type", default_item_type);
order = configuration->property(role + ".order", 2);
int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
f_if = configuration->property(role + ".if", 0);
@ -80,6 +83,7 @@ GpsL1CaKfTracking::GpsL1CaKfTracking(
{
item_size_ = sizeof(gr_complex);
tracking_ = gps_l1_ca_kf_make_tracking_cc(
order,
f_if,
fs_in,
vector_length,

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

@ -59,6 +59,7 @@ using google::LogMessage;
gps_l1_ca_kf_tracking_cc_sptr
gps_l1_ca_kf_make_tracking_cc(
unsigned int order,
long if_freq,
long fs_in,
unsigned int vector_length,
@ -67,7 +68,7 @@ gps_l1_ca_kf_make_tracking_cc(
float dll_bw_hz,
float early_late_space_chips)
{
return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(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));
}
@ -83,6 +84,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::forecast(int noutput_items,
Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
unsigned int order,
long if_freq,
long fs_in,
unsigned int vector_length,
@ -97,6 +99,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
this->message_port_register_out(pmt::mp("events"));
// initialize internal vars
d_order = order;
d_dump = dump;
d_if_freq = if_freq;
d_fs_in = fs_in;
@ -174,7 +177,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
// Kalman filter initialization (receiver initialization)
double CN_dB_Hz = 40;
double CN_dB_Hz = 30;
double CN_lin = pow(10, CN_dB_Hz / 10.0);
double sigma2_phase_detector_cycles2;
@ -182,7 +185,8 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
//covariances (static)
double sigma2_carrier_phase = GPS_TWO_PI / 4;
double sigma2_doppler = 250; /// !!
double sigma2_doppler = 450;
double sigma2_doppler_rate = 1.0 / 24.0;
kf_P_x_ini = arma::zeros(2, 2);
kf_P_x_ini(0, 0) = sigma2_carrier_phase;
@ -192,8 +196,8 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
kf_R(0, 0) = sigma2_phase_detector_cycles2;
kf_Q = arma::zeros(2, 2);
kf_Q(0, 0) = 1e-14;
kf_Q(1, 1) = 1e-2;
kf_Q(0, 0) = pow(4, GPS_L1_CA_CODE_PERIOD);
kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD;
kf_F = arma::zeros(2, 2);
kf_F(0, 0) = 1.0;
@ -206,6 +210,31 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
kf_x = arma::zeros(2, 1);
kf_y = arma::zeros(1, 1);
// order three
if (d_order == 3)
{
kf_P_x_ini = arma::resize(kf_P_x_ini, 3, 3);
kf_P_x_ini(2, 2) = sigma2_doppler_rate;
kf_Q = arma::zeros(3, 3);
kf_Q(0, 0) = pow(6, GPS_L1_CA_CODE_PERIOD);
kf_Q(1, 1) = pow(4, GPS_L1_CA_CODE_PERIOD);
kf_Q(2, 2) = pow(2, GPS_L1_CA_CODE_PERIOD);
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(1, 2) = GPS_L1_CA_CODE_PERIOD;
kf_F(2, 0) = 0.0;
kf_F(2, 1) = 0.0;
kf_F(2, 2) = 1.0;
kf_H = arma::resize(kf_H, 1, 3);
kf_H(0, 2) = 0.0;
kf_x = arma::resize(kf_x, 3, 1);
kf_x(2, 0) = -0.25;
}
}
@ -217,6 +246,13 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking()
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
d_acq_carrier_doppler_step_hz = static_cast<double>(d_acquisition_gnss_synchro->Acq_doppler_step);
// Correct Kalman filter covariance according to acq doppler step size (3 sigma)
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);
}
long int acq_trk_diff_samples;
double acq_trk_diff_seconds;
@ -667,7 +703,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus
kf_y(0) = carr_phase_error_rad; // measurement vector
kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation
kf_P_x = (arma::eye(2, 2) - 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
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

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

@ -56,7 +56,8 @@ typedef boost::shared_ptr<Gps_L1_Ca_Kf_Tracking_cc>
gps_l1_ca_kf_tracking_cc_sptr;
gps_l1_ca_kf_tracking_cc_sptr
gps_l1_ca_kf_make_tracking_cc(long if_freq,
gps_l1_ca_kf_make_tracking_cc(unsigned int order,
long if_freq,
long fs_in, unsigned int vector_length,
bool dump,
std::string dump_filename,
@ -83,14 +84,16 @@ public:
private:
friend gps_l1_ca_kf_tracking_cc_sptr
gps_l1_ca_kf_make_tracking_cc(long if_freq,
gps_l1_ca_kf_make_tracking_cc(unsigned int order,
long if_freq,
long fs_in, unsigned int vector_length,
bool dump,
std::string dump_filename,
float dll_bw_hz,
float early_late_space_chips);
Gps_L1_Ca_Kf_Tracking_cc(long if_freq,
Gps_L1_Ca_Kf_Tracking_cc(unsigned int order,
long if_freq,
long fs_in, unsigned int vector_length,
bool dump,
std::string dump_filename,
@ -98,6 +101,7 @@ private:
float early_late_space_chips);
// tracking configuration vars
unsigned int d_order;
unsigned int d_vector_length;
bool d_dump;
@ -135,6 +139,7 @@ private:
//Tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
double d_acq_carrier_doppler_step_hz;
double d_acq_code_phase_samples;
double d_acq_carrier_doppler_hz;
// correlator

1
src/core/system_parameters/gnss_synchro.h
View File

@ -49,6 +49,7 @@ public:
// Acquisition
double Acq_delay_samples; //!< Set by Acquisition processing block
double Acq_doppler_hz; //!< Set by Acquisition processing block
unsigned int Acq_doppler_step; //!< Set by Acquisition processing block
unsigned long int Acq_samplestamp_samples; //!< Set by Acquisition processing block
bool Flag_valid_acquisition; //!< Set by Acquisition processing block
//Tracking