Skip to content
Permalink
Browse files

Add configurable model order to KF tracking (order 2, order 3)

  • Loading branch information
glamountain committed Jul 3, 2018
1 parent d565d65 commit 0dd99e3c5d0242603039471eefa4c1476aee146a
@@ -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;
@@ -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 = 450;
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;
}
}


@@ -674,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,21 +84,24 @@ class Gps_L1_Ca_Kf_Tracking_cc : public gr::block

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,
float dll_bw_hz,
float early_late_space_chips);

// tracking configuration vars
unsigned int d_order;
unsigned int d_vector_length;
bool d_dump;

0 comments on commit 0dd99e3

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