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
29dc1730c4
@ -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;
|
||||
|
@ -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,
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user