Skip to content
Permalink
Browse files

Update KF tracking to use acquisition doppler bin size for initial do…

…ppler state covariance
  • Loading branch information
glamountain committed Jul 3, 2018
1 parent 93954fb commit d565d655212a6b01ff3200d79c94d58a5af31689
@@ -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;
@@ -174,15 +174,15 @@ 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;
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));

//covariances (static)
double sigma2_carrier_phase = GPS_TWO_PI / 4;
double sigma2_doppler = 250; /// !!
double sigma2_doppler = 450;

kf_P_x_ini = arma::zeros(2, 2);
kf_P_x_ini(0, 0) = sigma2_carrier_phase;
@@ -217,6 +217,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;
@@ -135,6 +135,7 @@ class Gps_L1_Ca_Kf_Tracking_cc : public gr::block
//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 @@ class Gnss_Synchro
// 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

0 comments on commit d565d65

Please sign in to comment.
You can’t perform that action at this time.