mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-04-16 15:53:21 +00:00
Merge branch 'contrib' of https://github.com/odrisci/gnss-sdr into
odrisci-contrib # Conflicts: # src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc # src/algorithms/tracking/libs/tracking_discriminators.cc
This commit is contained in:
commit
74d42250d8
@ -364,6 +364,9 @@ if(NOT GNURADIO_RUNTIME_FOUND)
|
||||
message("You can install it easily via Macports.")
|
||||
message("Open a terminal and type:")
|
||||
message("sudo port install gnuradio ")
|
||||
message("Alternatively you can use homebrew.")
|
||||
message("brew tap odrisci/gnuradio")
|
||||
message("brew install gnuradio" )
|
||||
message(FATAL_ERROR "GNU Radio 3.7.3 or later is required to build gnss-sdr")
|
||||
endif(OS_IS_MACOSX)
|
||||
endif(NOT GNURADIO_RUNTIME_FOUND)
|
||||
|
39
README.md
39
README.md
@ -390,7 +390,11 @@ Agree to Xcode license:
|
||||
$ sudo xcodebuild -license
|
||||
~~~~~~
|
||||
|
||||
Then, you need a package manager. For example, you can [install Macports](http://www.macports.org/install.php "Macports"). If you are upgrading from a previous installation, please follow the [migration rules](http://trac.macports.org/wiki/Migration).
|
||||
Software pre-requisites can be installed using either [Macports](#macports) or [Homebrew](#homebrew).
|
||||
|
||||
####<a name"macports">Macports</a>
|
||||
|
||||
First, [install Macports](http://www.macports.org/install.php). If you are upgrading from a previous installation, please follow the [migration rules](http://trac.macports.org/wiki/Migration).
|
||||
|
||||
In a terminal, type:
|
||||
|
||||
@ -416,12 +420,43 @@ and you can activate a certain version (2.7 works well) by typing:
|
||||
$ sudo port select --set python python27
|
||||
~~~~~~
|
||||
|
||||
#### <a name="homebrew">Homebrew</a>
|
||||
|
||||
Instructions for installing gnuradio using [homebrew](http://www.brew.sh) can be found [here](http://github.com/odrisci/homebrew-gnuradio) - please ensure to install all dependencies as required.
|
||||
|
||||
Install Armadillo and dependencies:
|
||||
|
||||
~~~~~~
|
||||
$ brew tap homebrew/science
|
||||
$ brew install cmake hdf5 arpack superlu
|
||||
$ brew install armadillo
|
||||
$ brew install glog gflags
|
||||
~~~~~~
|
||||
|
||||
#### Build GNSS-SDR
|
||||
|
||||
Finally, you are ready to clone the GNSS-SDR repository and build the software:
|
||||
|
||||
~~~~~~
|
||||
$ git clone https://github.com/gnss-sdr/gnss-sdr
|
||||
$ cd gnss-sdr/build
|
||||
~~~~~~
|
||||
|
||||
If using Macports, run:
|
||||
|
||||
~~~~~~
|
||||
$ cmake -DCMAKE_CXX_COMPILER=/usr/bin/clang++ -DCMAKE_PREFIX_PATH=/opt/local -DUSE_MACPORTS_PYTHON=/opt/local/bin/python ../
|
||||
~~~~~~
|
||||
|
||||
If using homebrew, run:
|
||||
|
||||
~~~~~~
|
||||
$ cmake ../
|
||||
~~~~~~
|
||||
|
||||
Finally, run:
|
||||
|
||||
~~~~~~
|
||||
$ make
|
||||
~~~~~~
|
||||
|
||||
@ -431,7 +466,7 @@ This will create three executables at gnss-sdr/install, namely ```gnss-sdr```, `
|
||||
$ sudo make install
|
||||
~~~~~~
|
||||
|
||||
|
||||
Note, it is advisable not to run the install step in a homebrew environment.
|
||||
|
||||
The documentation can be built by:
|
||||
|
||||
|
@ -97,6 +97,10 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
|
||||
|
||||
vector_length_ = sampled_ms_ * samples_per_ms;
|
||||
|
||||
if( bit_transition_flag_ ){
|
||||
vector_length_ *= 2;
|
||||
}
|
||||
|
||||
code_ = new gr_complex[vector_length_];
|
||||
|
||||
if (item_type_.compare("gr_complex") == 0)
|
||||
|
@ -40,6 +40,7 @@
|
||||
#include <volk/volk.h>
|
||||
#include "gnss_signal_processing.h"
|
||||
#include "control_message_factory.h"
|
||||
#include <boost/filesystem.hpp>
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
@ -65,8 +66,8 @@ pcps_acquisition_cc::pcps_acquisition_cc(
|
||||
gr::msg_queue::sptr queue, bool dump,
|
||||
std::string dump_filename) :
|
||||
gr::block("pcps_acquisition_cc",
|
||||
gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms),
|
||||
gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms))
|
||||
gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms * ( bit_transition_flag ? 2 : 1 )),
|
||||
gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms * ( bit_transition_flag ? 2 : 1 )) )
|
||||
{
|
||||
d_sample_counter = 0; // SAMPLE COUNTER
|
||||
d_active = false;
|
||||
@ -92,6 +93,24 @@ pcps_acquisition_cc::pcps_acquisition_cc(
|
||||
d_channel = 0;
|
||||
d_doppler_freq = 0.0;
|
||||
|
||||
//set_relative_rate( 1.0/d_fft_size );
|
||||
|
||||
// COD:
|
||||
// Experimenting with the overlap/save technique for handling bit trannsitions
|
||||
// The problem: Circular correlation is asynchronous with the received code.
|
||||
// In effect the first code phase used in the correlation is the current
|
||||
// estimate of the code phase at the start of the input buffer. If this is 1/2
|
||||
// of the code period a bit transition would move all the signal energy into
|
||||
// adjacent frequency bands at +/- 1/T where T is the integration time.
|
||||
//
|
||||
// We can avoid this by doing linear correlation, effectively doubling the
|
||||
// size of the input buffer and padding the code with zeros.
|
||||
if( d_bit_transition_flag )
|
||||
{
|
||||
d_fft_size *= 2;
|
||||
d_max_dwells = 1;
|
||||
}
|
||||
|
||||
d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
@ -135,7 +154,17 @@ pcps_acquisition_cc::~pcps_acquisition_cc()
|
||||
|
||||
void pcps_acquisition_cc::set_local_code(std::complex<float> * code)
|
||||
{
|
||||
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
|
||||
// COD
|
||||
// Here we want to create a buffer that looks like this:
|
||||
// [ 0 0 0 ... 0 c_0 c_1 ... c_L]
|
||||
// where c_i is the local code and there are L zeros and L chips
|
||||
int offset = 0;
|
||||
if( d_bit_transition_flag )
|
||||
{
|
||||
std::fill_n( d_fft_if->get_inbuf(), d_samples_per_code, gr_complex( 0.0, 0.0 ) );
|
||||
offset = d_samples_per_code;
|
||||
}
|
||||
memcpy(d_fft_if->get_inbuf() + offset, code, sizeof(gr_complex) * d_samples_per_code);
|
||||
d_fft_if->execute(); // We need the FFT of local code
|
||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
||||
}
|
||||
@ -157,7 +186,7 @@ void pcps_acquisition_cc::init()
|
||||
{
|
||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||
complex_exp_gen(d_grid_doppler_wipeoffs[doppler_index], d_freq - doppler, d_fs_in, d_fft_size);
|
||||
complex_exp_gen(d_grid_doppler_wipeoffs[doppler_index], -d_freq - doppler, d_fs_in, d_fft_size);
|
||||
}
|
||||
}
|
||||
|
||||
@ -222,6 +251,8 @@ int pcps_acquisition_cc::general_work(int noutput_items,
|
||||
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
|
||||
consume_each(ninput_items[0]);
|
||||
|
||||
//DLOG(INFO) << "Consumed " << ninput_items[0] << " items";
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
@ -232,7 +263,13 @@ int pcps_acquisition_cc::general_work(int noutput_items,
|
||||
unsigned int indext = 0;
|
||||
float magt = 0.0;
|
||||
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
|
||||
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
||||
|
||||
int effective_fft_size = ( d_bit_transition_flag ? d_fft_size/2 : d_fft_size );
|
||||
size_t offset = ( d_bit_transition_flag ? effective_fft_size : 0 );
|
||||
|
||||
float fft_normalization_factor = static_cast<float>(d_fft_size)
|
||||
* static_cast<float>(d_fft_size);
|
||||
|
||||
d_input_power = 0.0;
|
||||
d_mag = 0.0;
|
||||
|
||||
@ -273,8 +310,9 @@ int pcps_acquisition_cc::general_work(int noutput_items,
|
||||
d_ifft->execute();
|
||||
|
||||
// Search maximum
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u(&indext, d_magnitude, d_fft_size);
|
||||
size_t offset = ( d_bit_transition_flag ? effective_fft_size : 0 );
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size);
|
||||
volk_32f_index_max_16u(&indext, d_magnitude, effective_fft_size);
|
||||
|
||||
// Normalize the maximum value to correct the scale factor introduced by FFTW
|
||||
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
|
||||
@ -309,9 +347,19 @@ int pcps_acquisition_cc::general_work(int noutput_items,
|
||||
std::stringstream filename;
|
||||
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
||||
filename.str("");
|
||||
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
||||
|
||||
boost::filesystem::path p = d_dump_filename;
|
||||
filename << p.parent_path().string()
|
||||
<< boost::filesystem::path::preferred_separator
|
||||
<< p.stem().string()
|
||||
<< "_" << d_gnss_synchro->System
|
||||
<<"_" << d_gnss_synchro->Signal << "_sat_"
|
||||
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
||||
<< d_gnss_synchro->PRN << "_doppler_"
|
||||
<< doppler
|
||||
<< p.extension().string();
|
||||
|
||||
DLOG(INFO) << "Writing ACQ out to " << filename.str();
|
||||
|
||||
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
||||
d_dump_file.write((char*)d_ifft->get_outbuf(), n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
d_dump_file.close();
|
||||
@ -346,6 +394,8 @@ int pcps_acquisition_cc::general_work(int noutput_items,
|
||||
|
||||
consume_each(1);
|
||||
|
||||
DLOG(INFO) << "Done. Consumed 1 item.";
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
@ -402,3 +452,18 @@ int pcps_acquisition_cc::general_work(int noutput_items,
|
||||
|
||||
return noutput_items;
|
||||
}
|
||||
|
||||
|
||||
//void pcps_acquisition_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
|
||||
//{
|
||||
//// COD:
|
||||
//// For zero-padded case we need one extra code period
|
||||
//if( d_bit_transition_flag )
|
||||
//{
|
||||
//ninput_items_required[0] = noutput_items*(d_samples_per_code * d_max_dwells + d_samples_per_code);
|
||||
//}
|
||||
//else
|
||||
//{
|
||||
//ninput_items_required[0] = noutput_items*d_fft_size*d_max_dwells;
|
||||
//}
|
||||
//}
|
||||
|
@ -201,7 +201,7 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
|
||||
doppler_hz = d_config_doppler_min + d_doppler_step*doppler_index;
|
||||
// doppler search steps
|
||||
// compute the carrier doppler wipe-off signal and store it
|
||||
phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler_hz / static_cast<float>(d_fs_in);
|
||||
phase_step_rad = static_cast<float>(GPS_TWO_PI) * ( d_freq + doppler_hz ) / static_cast<float>(d_fs_in);
|
||||
d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size];
|
||||
fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size,0, phase_step_rad);
|
||||
}
|
||||
@ -316,7 +316,7 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
|
||||
{
|
||||
|
||||
// Direct FFT
|
||||
int zero_padding_factor = 16;
|
||||
int zero_padding_factor = 2;
|
||||
int fft_size_extended = d_fft_size * zero_padding_factor;
|
||||
gr::fft::fft_complex *fft_operator = new gr::fft::fft_complex(fft_size_extended, true);
|
||||
|
||||
|
@ -58,6 +58,8 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
|
||||
std::string default_item_type = "short";
|
||||
std::string default_dump_filename = "./my_capture.dat";
|
||||
|
||||
double default_seconds_to_skip = 0.0;
|
||||
size_t header_size = 0;
|
||||
samples_ = configuration->property(role + ".samples", 0);
|
||||
sampling_frequency_ = configuration->property(role + ".sampling_frequency", 0);
|
||||
filename_ = configuration->property(role + ".filename", default_filename);
|
||||
@ -72,6 +74,11 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
|
||||
enable_throttle_control_ = configuration->property(role + ".enable_throttle_control", false);
|
||||
std::string s = "InputFilter";
|
||||
//double IF = configuration->property(s + ".IF", 0.0);
|
||||
double seconds_to_skip = configuration->property(role + ".seconds_to_skip", default_seconds_to_skip );
|
||||
header_size = configuration->property( role + ".header_size", 0 );
|
||||
long samples_to_skip = 0;
|
||||
|
||||
bool is_complex = false;
|
||||
|
||||
if (item_type_.compare("gr_complex") == 0)
|
||||
{
|
||||
@ -88,6 +95,7 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
|
||||
else if (item_type_.compare("ishort") == 0)
|
||||
{
|
||||
item_size_ = sizeof(int16_t);
|
||||
is_complex = true;
|
||||
}
|
||||
else if (item_type_.compare("byte") == 0)
|
||||
{
|
||||
@ -96,6 +104,7 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
|
||||
else if (item_type_.compare("ibyte") == 0)
|
||||
{
|
||||
item_size_ = sizeof(int8_t);
|
||||
is_complex = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -107,6 +116,30 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
|
||||
{
|
||||
file_source_ = gr::blocks::file_source::make(item_size_, filename_.c_str(), repeat_);
|
||||
|
||||
if( seconds_to_skip > 0 )
|
||||
{
|
||||
samples_to_skip = static_cast< long >(
|
||||
seconds_to_skip * sampling_frequency_ );
|
||||
|
||||
if( is_complex )
|
||||
{
|
||||
samples_to_skip *= 2;
|
||||
}
|
||||
}
|
||||
if( header_size > 0 )
|
||||
{
|
||||
samples_to_skip += header_size;
|
||||
}
|
||||
|
||||
if( samples_to_skip > 0 )
|
||||
{
|
||||
LOG(INFO) << "Skipping " << samples_to_skip << " samples of the input file";
|
||||
if( not file_source_->seek( samples_to_skip, SEEK_SET ) )
|
||||
{
|
||||
LOG(INFO) << "Error skipping bytes!";
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
catch (const std::exception &e)
|
||||
{
|
||||
@ -174,7 +207,9 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
|
||||
|
||||
if (size > 0)
|
||||
{
|
||||
samples_ = floor(static_cast<double>(size) / static_cast<double>(item_size()) - ceil(0.002 * static_cast<double>(sampling_frequency_))); //process all the samples available in the file excluding at least the last 1 ms
|
||||
long bytes_to_skip = samples_to_skip*item_size_;
|
||||
long bytes_to_process = static_cast<long>(size) - bytes_to_skip;
|
||||
samples_ = floor(static_cast<double>(bytes_to_process) / static_cast<double>(item_size()) - ceil(0.002 * static_cast<double>(sampling_frequency_))); //process all the samples available in the file excluding at least the last 1 ms
|
||||
}
|
||||
}
|
||||
|
||||
@ -182,10 +217,11 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
|
||||
double signal_duration_s;
|
||||
signal_duration_s = static_cast<double>(samples_) * ( 1 / static_cast<double>(sampling_frequency_));
|
||||
|
||||
if ((item_type_.compare("gr_complex") != 0) || (item_type_.compare("ishort") != 0) || (item_type_.compare("ibyte") != 0) ) // signal is complex (interleaved)
|
||||
{
|
||||
signal_duration_s /= 2;
|
||||
}
|
||||
if( is_complex )
|
||||
{
|
||||
signal_duration_s /= 2.0;
|
||||
}
|
||||
|
||||
DLOG(INFO) << "Total number samples to be processed= " << samples_ << " GNSS signal duration= " << signal_duration_s << " [s]";
|
||||
std::cout << "GNSS signal recorded time to be processed: " << signal_duration_s << " [s]" << std::endl;
|
||||
|
||||
|
@ -210,10 +210,10 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking()
|
||||
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_rem_code_phase_samples = 0.0;
|
||||
d_rem_carr_phase_rad = 0;
|
||||
d_acc_carrier_phase_rad = 0;
|
||||
d_rem_carr_phase_rad = 0.0;
|
||||
d_acc_carrier_phase_rad = 0.0;
|
||||
|
||||
d_acc_code_phase_secs = 0;
|
||||
d_acc_code_phase_secs = 0.0;
|
||||
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
|
||||
d_current_prn_length_samples = d_vector_length;
|
||||
|
||||
@ -249,17 +249,17 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
|
||||
code_phase_step_chips = d_code_freq_chips / (static_cast<double>(d_fs_in));
|
||||
code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast<double>(d_fs_in));
|
||||
|
||||
rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in);
|
||||
rem_code_phase_half_chips = d_rem_code_phase_samples * (2.0 * d_code_freq_chips / static_cast<double>(d_fs_in));
|
||||
tcode_half_chips = - rem_code_phase_half_chips;
|
||||
|
||||
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
|
||||
very_early_late_spc_samples = round(d_very_early_late_spc_chips / code_phase_step_chips);
|
||||
early_late_spc_samples = std::round(d_early_late_spc_chips / code_phase_step_chips);
|
||||
very_early_late_spc_samples = std::round(d_very_early_late_spc_chips / code_phase_step_chips);
|
||||
|
||||
epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples * 2;
|
||||
|
||||
for (int i = 0; i < epl_loop_length_samples; i++)
|
||||
{
|
||||
associated_chip_index = 2 + round(fmod(tcode_half_chips - 2 * d_very_early_late_spc_chips, code_length_half_chips));
|
||||
associated_chip_index = 2 + std::round(std::fmod(tcode_half_chips - 2.0 * d_very_early_late_spc_chips, static_cast<double>(code_length_half_chips)));
|
||||
d_very_early_code[i] = d_ca_code[associated_chip_index];
|
||||
tcode_half_chips = tcode_half_chips + code_phase_step_half_chips;
|
||||
}
|
||||
@ -273,7 +273,7 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
|
||||
void galileo_e1_dll_pll_veml_tracking_cc::update_local_carrier()
|
||||
{
|
||||
float sin_f, cos_f;
|
||||
float phase_step_rad = static_cast<float>(2 * GALILEO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
|
||||
float phase_step_rad = static_cast<float>(2.0 * GALILEO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
|
||||
int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
|
||||
int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad);
|
||||
|
||||
@ -310,11 +310,10 @@ galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc()
|
||||
int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vector_int &ninput_items,
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
|
||||
{
|
||||
double carr_error_hz = 0.0;
|
||||
double carr_error_filt_hz = 0.0;
|
||||
double code_error_chips = 0.0;
|
||||
double code_error_filt_chips = 0.0;
|
||||
|
||||
double carr_error_hz = 0.0;
|
||||
double carr_error_filt_hz = 0.0;
|
||||
double code_error_chips = 0.0;
|
||||
double code_error_filt_chips = 0.0;
|
||||
|
||||
if (d_enable_tracking == true)
|
||||
{
|
||||
@ -327,8 +326,8 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
|
||||
double acq_trk_shif_correction_samples;
|
||||
int acq_to_trk_delay_samples;
|
||||
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
|
||||
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
|
||||
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
|
||||
acq_trk_shif_correction_samples = static_cast<double>(d_current_prn_length_samples) - static_cast<double>(std::fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_current_prn_length_samples)));
|
||||
samples_offset = static_cast<int>(std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples));
|
||||
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
|
||||
d_pull_in = false;
|
||||
consume_each(samples_offset); //shift input to perform alignment with local replica
|
||||
@ -365,7 +364,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
|
||||
|
||||
// ################## PLL ##########################################################
|
||||
// PLL discriminator
|
||||
carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_TWO_PI);
|
||||
carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<double>(GPS_TWO_PI);
|
||||
// Carrier discriminator filter
|
||||
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
|
||||
// New carrier Doppler frequency estimation
|
||||
@ -376,7 +375,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
|
||||
d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
|
||||
//remnant carrier phase to prevent overflow in the code NCO
|
||||
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
|
||||
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
|
||||
d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
|
||||
|
||||
// ################## DLL ##########################################################
|
||||
// DLL discriminator
|
||||
@ -400,7 +399,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
|
||||
T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS;
|
||||
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
|
||||
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
|
||||
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples
|
||||
d_current_prn_length_samples = static_cast<int>(std::round(K_blk_samples)); //round to a discrete samples
|
||||
//d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
|
||||
|
||||
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
|
||||
@ -457,7 +456,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
|
||||
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??)
|
||||
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
|
||||
//compute remnant code phase samples AFTER the Tracking timestamp
|
||||
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
|
||||
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_current_prn_length_samples); //rounding error < 1 sample
|
||||
|
||||
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
|
||||
current_synchro_data.Code_phase_secs = 0;
|
||||
@ -472,10 +471,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
|
||||
* \todo The stop timer has to be moved to the signal source!
|
||||
*/
|
||||
// stream to collect cout calls to improve thread safety
|
||||
std::stringstream tmp_str_stream;
|
||||
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||
if (std::floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||
{
|
||||
d_last_seg = floor(d_sample_counter / d_fs_in);
|
||||
d_last_seg = std::floor(d_sample_counter / d_fs_in);
|
||||
|
||||
if (d_channel == 0)
|
||||
{
|
||||
@ -498,9 +496,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
|
||||
*/
|
||||
// stream to collect cout calls to improve thread safety
|
||||
std::stringstream tmp_str_stream;
|
||||
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||
if (std::floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||
{
|
||||
d_last_seg = floor(d_sample_counter / d_fs_in);
|
||||
d_last_seg = std::floor(d_sample_counter / d_fs_in);
|
||||
|
||||
if (d_channel == 0)
|
||||
{
|
||||
|
@ -126,8 +126,8 @@ private:
|
||||
long d_if_freq;
|
||||
long d_fs_in;
|
||||
|
||||
double d_early_late_spc_chips;
|
||||
double d_very_early_late_spc_chips;
|
||||
float d_early_late_spc_chips;
|
||||
float d_very_early_late_spc_chips;
|
||||
|
||||
gr_complex* d_ca_code;
|
||||
|
||||
@ -146,22 +146,22 @@ private:
|
||||
|
||||
// remaining code phase and carrier phase between tracking loops
|
||||
double d_rem_code_phase_samples;
|
||||
double d_rem_carr_phase_rad;
|
||||
float d_rem_carr_phase_rad;
|
||||
|
||||
// PLL and DLL filter library
|
||||
Tracking_2nd_DLL_filter d_code_loop_filter;
|
||||
Tracking_2nd_PLL_filter d_carrier_loop_filter;
|
||||
|
||||
// acquisition
|
||||
double d_acq_code_phase_samples;
|
||||
double d_acq_carrier_doppler_hz;
|
||||
float d_acq_code_phase_samples;
|
||||
float d_acq_carrier_doppler_hz;
|
||||
|
||||
// correlator
|
||||
Correlator d_correlator;
|
||||
|
||||
// tracking vars
|
||||
double d_code_freq_chips;
|
||||
double d_carrier_doppler_hz;
|
||||
float d_carrier_doppler_hz;
|
||||
double d_acc_carrier_phase_rad;
|
||||
double d_acc_code_phase_secs;
|
||||
|
||||
@ -175,9 +175,9 @@ private:
|
||||
// CN0 estimation and lock detector
|
||||
int d_cn0_estimation_counter;
|
||||
gr_complex* d_Prompt_buffer;
|
||||
double d_carrier_lock_test;
|
||||
double d_CN0_SNV_dB_Hz;
|
||||
double d_carrier_lock_threshold;
|
||||
float d_carrier_lock_test;
|
||||
float d_CN0_SNV_dB_Hz;
|
||||
float d_carrier_lock_threshold;
|
||||
int d_carrier_lock_fail_counter;
|
||||
|
||||
// control vars
|
||||
|
@ -297,7 +297,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code()
|
||||
void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_carrier()
|
||||
{
|
||||
float sin_f, cos_f;
|
||||
float phase_step_rad = static_cast<float>(GPS_TWO_PI) * static_cast<float>(d_carrier_doppler_hz) / static_cast<float>(d_fs_in);
|
||||
float phase_step_rad = static_cast<float>(GPS_TWO_PI) * ( d_if_freq + d_carrier_doppler_hz ) / static_cast<float>(d_fs_in);
|
||||
int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
|
||||
int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad);
|
||||
|
||||
@ -424,7 +424,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||
//carrier phase accumulator for (K) doppler estimation
|
||||
d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
|
||||
//remanent carrier phase to prevent overflow in the code NCO
|
||||
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
|
||||
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * ( d_if_freq + d_carrier_doppler_hz ) * GPS_L1_CA_CODE_PERIOD;
|
||||
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
|
||||
|
||||
// ################## DLL ##########################################################
|
||||
|
@ -40,6 +40,7 @@ set(TRACKING_LIB_SOURCES
|
||||
tracking_2nd_PLL_filter.cc
|
||||
tracking_discriminators.cc
|
||||
tracking_FLL_PLL_filter.cc
|
||||
tracking_loop_filter.cc
|
||||
)
|
||||
|
||||
include_directories(
|
||||
|
@ -151,3 +151,50 @@ void Correlator::Carrier_wipeoff_and_EPL_volk_custom(int signal_length_samples,
|
||||
volk_cw_epl_corr_u(input, carrier, E_code, P_code, L_code, E_out, P_out, L_out, signal_length_samples);
|
||||
}
|
||||
#endif
|
||||
void Correlator::Carrier_rotate_and_EPL_volk(int signal_length_samples,
|
||||
const gr_complex* input,
|
||||
gr_complex *phase_as_complex,
|
||||
gr_complex phase_inc_as_complex,
|
||||
const gr_complex* E_code,
|
||||
const gr_complex* P_code,
|
||||
const gr_complex* L_code,
|
||||
gr_complex* E_out,
|
||||
gr_complex* P_out,
|
||||
gr_complex* L_out )
|
||||
{
|
||||
gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
volk_32fc_s32fc_x2_rotator_32fc(bb_signal, input, phase_inc_as_complex, phase_as_complex, signal_length_samples);
|
||||
volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples);
|
||||
volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples);
|
||||
volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples);
|
||||
|
||||
volk_free(bb_signal);
|
||||
}
|
||||
|
||||
void Correlator::Carrier_rotate_and_VEPL_volk(int signal_length_samples,
|
||||
const gr_complex* input,
|
||||
gr_complex *phase_as_complex,
|
||||
gr_complex phase_inc_as_complex,
|
||||
const gr_complex* VE_code,
|
||||
const gr_complex* E_code,
|
||||
const gr_complex* P_code,
|
||||
const gr_complex* L_code,
|
||||
const gr_complex* VL_code,
|
||||
gr_complex* VE_out,
|
||||
gr_complex* E_out,
|
||||
gr_complex* P_out,
|
||||
gr_complex* L_out,
|
||||
gr_complex* VL_out )
|
||||
{
|
||||
gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
volk_32fc_s32fc_x2_rotator_32fc(bb_signal, input, phase_inc_as_complex, phase_as_complex, signal_length_samples);
|
||||
volk_32fc_x2_dot_prod_32fc(VE_out, bb_signal, VE_code, signal_length_samples);
|
||||
volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples);
|
||||
volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples);
|
||||
volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples);
|
||||
volk_32fc_x2_dot_prod_32fc(VL_out, bb_signal, VL_code, signal_length_samples);
|
||||
|
||||
volk_free(bb_signal);
|
||||
}
|
||||
|
@ -56,13 +56,40 @@
|
||||
class Correlator
|
||||
{
|
||||
public:
|
||||
Correlator();
|
||||
~Correlator();
|
||||
void Carrier_wipeoff_and_EPL_generic(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out);
|
||||
void Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out);
|
||||
void Carrier_wipeoff_and_VEPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* VE_code, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* VL_code, gr_complex* VE_out, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* VL_out);
|
||||
// void Carrier_wipeoff_and_EPL_volk_IQ(int prn_length_samples,int integration_time ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out);
|
||||
void Carrier_wipeoff_and_EPL_volk_IQ(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out);
|
||||
Correlator();
|
||||
~Correlator();
|
||||
|
||||
void Carrier_rotate_and_EPL_volk(int signal_length_samples,
|
||||
const gr_complex* input,
|
||||
gr_complex *phase_as_complex,
|
||||
gr_complex phase_inc_as_complex,
|
||||
const gr_complex* E_code,
|
||||
const gr_complex* P_code,
|
||||
const gr_complex* L_code,
|
||||
gr_complex* E_out,
|
||||
gr_complex* P_out,
|
||||
gr_complex* L_out );
|
||||
|
||||
void Carrier_rotate_and_VEPL_volk(int signal_length_samples,
|
||||
const gr_complex* input,
|
||||
gr_complex *phase_as_complex,
|
||||
gr_complex phase_inc_as_complex,
|
||||
const gr_complex* VE_code,
|
||||
const gr_complex* E_code,
|
||||
const gr_complex* P_code,
|
||||
const gr_complex* L_code,
|
||||
const gr_complex* VL_code,
|
||||
gr_complex* VE_out,
|
||||
gr_complex* E_out,
|
||||
gr_complex* P_out,
|
||||
gr_complex* L_out,
|
||||
gr_complex* VL_out );
|
||||
|
||||
#if USING_VOLK_CW_EPL_CORR_CUSTOM
|
||||
void Carrier_wipeoff_and_EPL_volk_custom(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out);
|
||||
#endif
|
||||
|
@ -91,7 +91,7 @@ double pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
|
||||
/*
|
||||
* DLL Noncoherent Early minus Late envelope normalized discriminator:
|
||||
* \f{equation}
|
||||
* error=\frac{E-L}{E+L},
|
||||
* error=\frac{1}{2}\frac{E-L}{E+L},
|
||||
* \f}
|
||||
* where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and
|
||||
* \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips].
|
||||
@ -101,7 +101,14 @@ double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1)
|
||||
double P_early, P_late;
|
||||
P_early = std::abs(early_s1);
|
||||
P_late = std::abs(late_s1);
|
||||
return 0.5*(P_early - P_late) / ((P_early + P_late));
|
||||
if( P_early + P_late == 0.0 )
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0.5 * (P_early - P_late) / ((P_early + P_late));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
@ -118,5 +125,12 @@ double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr
|
||||
double P_early, P_late;
|
||||
P_early = std::sqrt(std::norm(very_early_s1) + std::norm(early_s1));
|
||||
P_late = std::sqrt(std::norm(very_late_s1) + std::norm(late_s1));
|
||||
return (P_early - P_late) / ((P_early + P_late));
|
||||
if( P_early + P_late == 0.0 )
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return (P_early - P_late) / ((P_early + P_late));
|
||||
}
|
||||
}
|
||||
|
284
src/algorithms/tracking/libs/tracking_loop_filter.cc
Normal file
284
src/algorithms/tracking/libs/tracking_loop_filter.cc
Normal file
@ -0,0 +1,284 @@
|
||||
/*!
|
||||
* \file tracking_loop_filter.cc
|
||||
* \brief Generic 1st to 3rd order loop filter implementation
|
||||
* \author Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
|
||||
*
|
||||
* Class implementing a generic 1st, 2nd or 3rd order loop filter. Based
|
||||
* on the bilinear transform of the standard Weiner filter.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include "tracking_loop_filter.h"
|
||||
#include <cmath>
|
||||
#include <glog/logging.h>
|
||||
|
||||
|
||||
#define MAX_LOOP_ORDER 3
|
||||
#define MAX_HISTORY_LENGTH 4
|
||||
|
||||
Tracking_loop_filter::Tracking_loop_filter( float update_interval,
|
||||
float noise_bandwidth,
|
||||
int loop_order,
|
||||
bool include_last_integrator )
|
||||
: d_loop_order( loop_order ),
|
||||
d_current_index( 0 ),
|
||||
d_include_last_integrator( include_last_integrator ),
|
||||
d_noise_bandwidth( noise_bandwidth ),
|
||||
d_update_interval( update_interval )
|
||||
{
|
||||
d_inputs.resize( MAX_HISTORY_LENGTH, 0.0 );
|
||||
d_outputs.resize( MAX_HISTORY_LENGTH, 0.0 );
|
||||
update_coefficients();
|
||||
}
|
||||
|
||||
Tracking_loop_filter::Tracking_loop_filter()
|
||||
: d_loop_order( 2 ),
|
||||
d_current_index( 0 ),
|
||||
d_include_last_integrator( false ),
|
||||
d_noise_bandwidth( 15.0 ),
|
||||
d_update_interval( 0.001 )
|
||||
{
|
||||
d_inputs.resize( MAX_HISTORY_LENGTH, 0.0 );
|
||||
d_outputs.resize( MAX_HISTORY_LENGTH, 0.0 );
|
||||
update_coefficients();
|
||||
}
|
||||
|
||||
Tracking_loop_filter::~Tracking_loop_filter()
|
||||
{
|
||||
// Don't need to do anything here
|
||||
}
|
||||
|
||||
float Tracking_loop_filter::apply( float current_input )
|
||||
{
|
||||
|
||||
// Now apply the filter coefficients:
|
||||
float result = 0;
|
||||
|
||||
// Hanlde the old outputs first:
|
||||
for( unsigned int ii=0; ii < d_output_coefficients.size(); ++ii )
|
||||
{
|
||||
result += d_output_coefficients[ii] * d_outputs[ (d_current_index+ii)%MAX_HISTORY_LENGTH ];
|
||||
}
|
||||
|
||||
// Now update the index to handle the inputs.
|
||||
// DO NOT CHANGE THE ORDER OF THE ABOVE AND BELOW CODE
|
||||
// SNIPPETS!!!!!!!
|
||||
|
||||
// Implementing a sort of circular buffer for the inputs and outputs
|
||||
// the current input/output is at d_current_index, the nth previous
|
||||
// input/output is at (d_current_index+n)%d_loop_order
|
||||
d_current_index--;
|
||||
if( d_current_index < 0 )
|
||||
{
|
||||
d_current_index += MAX_HISTORY_LENGTH;
|
||||
}
|
||||
|
||||
d_inputs[d_current_index] = current_input;
|
||||
|
||||
|
||||
for( unsigned int ii=0; ii < d_input_coefficients.size(); ++ii )
|
||||
{
|
||||
result += d_input_coefficients[ii] * d_inputs[ (d_current_index+ii)%MAX_HISTORY_LENGTH ];
|
||||
}
|
||||
|
||||
|
||||
d_outputs[d_current_index] = result;
|
||||
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void Tracking_loop_filter::update_coefficients( void )
|
||||
{
|
||||
// Analog gains:
|
||||
float g1;
|
||||
float g2;
|
||||
float g3;
|
||||
|
||||
// Natural frequency
|
||||
float wn;
|
||||
float T = d_update_interval;
|
||||
|
||||
float zeta = 1/std::sqrt(2);
|
||||
|
||||
// The following is based on the bilinear transform approximation of
|
||||
// the analog integrator. The loop format is from Kaplan & Hegarty
|
||||
// Table 5.6. The basic concept is that the loop has a cascade of
|
||||
// integrators:
|
||||
// 1 for a 1st order loop
|
||||
// 2 for a 2nd order loop
|
||||
// 3 for a 3rd order loop
|
||||
// The bilinear transform approximates 1/s as
|
||||
// T/2(1 + z^-1)/(1-z^-1) in the z domain.
|
||||
|
||||
switch( d_loop_order )
|
||||
{
|
||||
case 1:
|
||||
wn = d_noise_bandwidth*4.0;
|
||||
g1 = wn;
|
||||
if( d_include_last_integrator )
|
||||
{
|
||||
d_input_coefficients.resize(2);
|
||||
d_input_coefficients[0] = g1*T/2.0;
|
||||
d_input_coefficients[1] = g1*T/2.0;
|
||||
|
||||
d_output_coefficients.resize(1);
|
||||
d_output_coefficients[0] = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_input_coefficients.resize(1);
|
||||
d_input_coefficients[0] = g1;
|
||||
|
||||
d_output_coefficients.resize(0);
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
wn = d_noise_bandwidth * (8*zeta)/ (4*zeta*zeta + 1 );
|
||||
g1 = wn*wn;
|
||||
g2 = wn*2*zeta;
|
||||
if( d_include_last_integrator )
|
||||
{
|
||||
d_input_coefficients.resize(3);
|
||||
d_input_coefficients[0] = T/2*( g1*T/2 + g2 );
|
||||
d_input_coefficients[1] = T*T/2*g1;
|
||||
d_input_coefficients[2] = T/2*( g1*T/2 - g2 );
|
||||
|
||||
d_output_coefficients.resize(2);
|
||||
d_output_coefficients[0] = 2;
|
||||
d_output_coefficients[1] = -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_input_coefficients.resize(2);
|
||||
d_input_coefficients[0] = ( g1*T/2.0+g2 );
|
||||
d_input_coefficients[1] = g1*T/2-g2;
|
||||
|
||||
d_output_coefficients.resize(1);
|
||||
d_output_coefficients[0] = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
case 3:
|
||||
wn = d_noise_bandwidth / 0.7845; // From Kaplan
|
||||
float a3 = 1.1;
|
||||
float b3 = 2.4;
|
||||
g1 = wn*wn*wn;
|
||||
g2 = a3*wn*wn;
|
||||
g3 = b3*wn;
|
||||
|
||||
if( d_include_last_integrator )
|
||||
{
|
||||
d_input_coefficients.resize(4);
|
||||
d_input_coefficients[0] = T/2*( g3 + T/2*( g2 + T/2*g1 ) );
|
||||
d_input_coefficients[1] = T/2*( -g3 + T/2*( g2 + 3*T/2*g1 ) );
|
||||
d_input_coefficients[2] = T/2*( -g3 - T/2*( g2 - 3*T/2*g1 ) );
|
||||
d_input_coefficients[3] = T/2*( g3 - T/2*( g2 - T/2*g1 ) );
|
||||
|
||||
d_output_coefficients.resize(3);
|
||||
d_output_coefficients[0] = 3;
|
||||
d_output_coefficients[1] = -3;
|
||||
d_output_coefficients[2] = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_input_coefficients.resize(3);
|
||||
d_input_coefficients[0] = g3 + T/2*( g2 + T/2*g1 );
|
||||
d_input_coefficients[1] = g1*T*T/2 -2*g3;
|
||||
d_input_coefficients[2] = g3 + T/2*( -g2 + T/2*g1 );
|
||||
|
||||
|
||||
d_output_coefficients.resize(2);
|
||||
d_output_coefficients[0] = 2;
|
||||
d_output_coefficients[1] = -1;
|
||||
}
|
||||
break;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
void Tracking_loop_filter::set_noise_bandwidth( float noise_bandwidth )
|
||||
{
|
||||
d_noise_bandwidth = noise_bandwidth;
|
||||
update_coefficients();
|
||||
}
|
||||
|
||||
float Tracking_loop_filter::get_noise_bandwidth( void ) const
|
||||
{
|
||||
return d_noise_bandwidth;
|
||||
}
|
||||
|
||||
void Tracking_loop_filter::set_update_interval( float update_interval )
|
||||
{
|
||||
d_update_interval = update_interval;
|
||||
update_coefficients();
|
||||
}
|
||||
|
||||
float Tracking_loop_filter::get_update_interval( void ) const
|
||||
{
|
||||
return d_update_interval;
|
||||
}
|
||||
|
||||
void Tracking_loop_filter::set_include_last_integrator( bool include_last_integrator )
|
||||
{
|
||||
d_include_last_integrator = include_last_integrator;
|
||||
update_coefficients();
|
||||
}
|
||||
|
||||
bool Tracking_loop_filter::get_include_last_integrator( void ) const
|
||||
{
|
||||
return d_include_last_integrator;
|
||||
}
|
||||
|
||||
void Tracking_loop_filter::set_order( int loop_order )
|
||||
{
|
||||
if( loop_order < 1 || loop_order > MAX_LOOP_ORDER )
|
||||
{
|
||||
LOG(ERROR) << "Ignoring attempt to set loop order to " << loop_order
|
||||
<< ". Maximum allowed order is: " << MAX_LOOP_ORDER
|
||||
<< ". Not changing current value of " << d_loop_order;
|
||||
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
d_loop_order = loop_order;
|
||||
update_coefficients();
|
||||
}
|
||||
|
||||
int Tracking_loop_filter::get_order( void ) const
|
||||
{
|
||||
return d_loop_order;
|
||||
}
|
||||
|
||||
void Tracking_loop_filter::initialize( float initial_output )
|
||||
{
|
||||
d_inputs.assign( MAX_HISTORY_LENGTH, 0.0 );
|
||||
d_outputs.assign( MAX_HISTORY_LENGTH, initial_output );
|
||||
d_current_index = MAX_HISTORY_LENGTH - 1;
|
||||
}
|
98
src/algorithms/tracking/libs/tracking_loop_filter.h
Normal file
98
src/algorithms/tracking/libs/tracking_loop_filter.h
Normal file
@ -0,0 +1,98 @@
|
||||
/*!
|
||||
* \file tracking_loop_filter.h
|
||||
* \brief Generic 1st to 3rd order loop filter implementation
|
||||
* \author Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
|
||||
*
|
||||
* Class implementing a generic 1st, 2nd or 3rd order loop filter. Based
|
||||
* on the bilinear transform of the standard Weiner filter.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_TRACKING_LOOP_FILTER_H_
|
||||
#define GNSS_SDR_TRACKING_LOOP_FILTER_H_
|
||||
|
||||
#include <vector>
|
||||
|
||||
|
||||
/*!
|
||||
* \brief This class implements a generic 1st, 2nd or 3rd order loop filter
|
||||
*
|
||||
*/
|
||||
class Tracking_loop_filter
|
||||
{
|
||||
private:
|
||||
// Store the last inputs and outputs:
|
||||
std::vector< float > d_inputs;
|
||||
std::vector< float > d_outputs;
|
||||
|
||||
// Store the filter coefficients:
|
||||
std::vector< float > d_input_coefficients;
|
||||
std::vector< float > d_output_coefficients;
|
||||
|
||||
// The loop order:
|
||||
int d_loop_order;
|
||||
|
||||
// The current index in the i/o arrays:
|
||||
int d_current_index;
|
||||
|
||||
// Should the last integrator be included?
|
||||
bool d_include_last_integrator;
|
||||
|
||||
// The noise bandwidth (in Hz)
|
||||
// Note this is an approximation only valid when the product of this
|
||||
// number and the update interval (T) is small.
|
||||
float d_noise_bandwidth;
|
||||
|
||||
// Loop update interval
|
||||
float d_update_interval;
|
||||
|
||||
// Compute the filter coefficients:
|
||||
void update_coefficients(void);
|
||||
|
||||
|
||||
public:
|
||||
float get_noise_bandwidth(void) const;
|
||||
float get_update_interval(void) const;
|
||||
bool get_include_last_integrator(void) const;
|
||||
int get_order(void) const;
|
||||
|
||||
void set_noise_bandwidth( float noise_bandwidth );
|
||||
void set_update_interval( float update_interval );
|
||||
void set_include_last_integrator( bool include_last_integrator );
|
||||
void set_order( int loop_order );
|
||||
|
||||
void initialize(float initial_output = 0.0);
|
||||
float apply(float current_input );
|
||||
|
||||
Tracking_loop_filter(float update_interval, float noise_bandwidth,
|
||||
int loop_order = 2,
|
||||
bool include_last_integrator = false );
|
||||
|
||||
Tracking_loop_filter();
|
||||
~Tracking_loop_filter();
|
||||
};
|
||||
|
||||
#endif
|
@ -34,10 +34,13 @@
|
||||
|
||||
#include "gnss_flowgraph.h"
|
||||
#include "unistd.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <set>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/tokenizer.hpp>
|
||||
#include <glog/logging.h>
|
||||
#include "configuration_interface.h"
|
||||
#include "gnss_block_interface.h"
|
||||
@ -313,12 +316,12 @@ void GNSSFlowgraph::connect()
|
||||
}
|
||||
channels_.at(i)->set_signal(available_GNSS_signals_.front());
|
||||
LOG(INFO) << "Channel " << i << " assigned to " << available_GNSS_signals_.front();
|
||||
available_GNSS_signals_.pop_front();
|
||||
channels_.at(i)->start();
|
||||
|
||||
if (channels_state_[i] == 1)
|
||||
{
|
||||
channels_.at(i)->start_acquisition();
|
||||
available_GNSS_signals_.pop_front();
|
||||
LOG(INFO) << "Channel " << i << " connected to observables and ready for acquisition";
|
||||
}
|
||||
else
|
||||
@ -413,13 +416,20 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
|
||||
LOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite();
|
||||
channels_state_[who] = 2;
|
||||
acq_channels_count_--;
|
||||
if (acq_channels_count_ < max_acq_channels_)
|
||||
if (!available_GNSS_signals_.empty() && acq_channels_count_ < max_acq_channels_)
|
||||
{
|
||||
for (unsigned int i = 0; i < channels_count_; i++)
|
||||
{
|
||||
if (channels_state_[i] == 0)
|
||||
{
|
||||
channels_state_[i] = 1;
|
||||
while (channels_.at(i)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 )
|
||||
{
|
||||
available_GNSS_signals_.push_back(available_GNSS_signals_.front());
|
||||
available_GNSS_signals_.pop_front();
|
||||
}
|
||||
channels_.at(i)->set_signal(available_GNSS_signals_.front());
|
||||
available_GNSS_signals_.pop_front();
|
||||
acq_channels_count_++;
|
||||
channels_.at(i)->start_acquisition();
|
||||
break;
|
||||
@ -442,6 +452,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
|
||||
{
|
||||
channels_state_[who] = 0;
|
||||
channels_.at(who)->standby();
|
||||
available_GNSS_signals_.push_back( channels_.at(who)->get_signal() );
|
||||
}
|
||||
|
||||
// for (unsigned int i = 0; i < channels_count_; i++)
|
||||
@ -593,8 +604,55 @@ void GNSSFlowgraph::set_signals_list()
|
||||
11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28,
|
||||
29, 30, 31, 32, 33, 34, 35, 36};
|
||||
|
||||
std::string sv_list = configuration_->property("Galileo.prns", std::string("") );
|
||||
|
||||
|
||||
if( sv_list.length() > 0 )
|
||||
{
|
||||
// Reset the available prns:
|
||||
std::set< unsigned int > tmp_set;
|
||||
boost::tokenizer<> tok( sv_list );
|
||||
std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ),
|
||||
boost::lexical_cast<unsigned int, std::string> );
|
||||
|
||||
if( tmp_set.size() > 0 )
|
||||
{
|
||||
available_galileo_prn = tmp_set;
|
||||
}
|
||||
}
|
||||
|
||||
sv_list = configuration_->property("GPS.prns", std::string("") );
|
||||
|
||||
if( sv_list.length() > 0 )
|
||||
{
|
||||
// Reset the available prns:
|
||||
std::set< unsigned int > tmp_set;
|
||||
boost::tokenizer<> tok( sv_list );
|
||||
std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ),
|
||||
boost::lexical_cast<unsigned int, std::string> );
|
||||
|
||||
if( tmp_set.size() > 0 )
|
||||
{
|
||||
available_gps_prn = tmp_set;
|
||||
}
|
||||
}
|
||||
|
||||
sv_list = configuration_->property("SBAS.prns", std::string("") );
|
||||
|
||||
if( sv_list.length() > 0 )
|
||||
{
|
||||
// Reset the available prns:
|
||||
std::set< unsigned int > tmp_set;
|
||||
boost::tokenizer<> tok( sv_list );
|
||||
std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ),
|
||||
boost::lexical_cast<unsigned int, std::string> );
|
||||
|
||||
if( tmp_set.size() > 0 )
|
||||
{
|
||||
available_sbas_prn = tmp_set;
|
||||
}
|
||||
}
|
||||
|
||||
if ((configuration_->property("Channels_1C.count", 0) > 0) or (default_system.find(std::string("GPS")) != std::string::npos) or (default_signal.compare("1C") == 0) or (configuration_->property("Channels_GPS.count", 0) > 0) )
|
||||
{
|
||||
/*
|
||||
@ -691,8 +749,16 @@ void GNSSFlowgraph::set_signals_list()
|
||||
}
|
||||
else
|
||||
{
|
||||
Gnss_Signal signal_value = Gnss_Signal(Gnss_Satellite(gnss_system, gnss_it->get_satellite().get_PRN()), gnss_signal);
|
||||
available_GNSS_signals_.remove(signal_value);
|
||||
Gnss_Signal signal_value = Gnss_Signal(Gnss_Satellite(gnss_system, ( sat != 0 ? sat : gnss_it->get_satellite().get_PRN())), gnss_signal);
|
||||
if( gnss_it == available_GNSS_signals_.begin() )
|
||||
{
|
||||
available_GNSS_signals_.remove(signal_value);
|
||||
gnss_it = available_GNSS_signals_.begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
available_GNSS_signals_.remove(signal_value);
|
||||
}
|
||||
available_GNSS_signals_.insert(gnss_it, signal_value);
|
||||
}
|
||||
}
|
||||
|
@ -73,7 +73,7 @@ bool operator==(const Gnss_Signal &sig1, const Gnss_Signal &sig2)
|
||||
|
||||
if (sig1.get_satellite() == sig2.get_satellite())
|
||||
{
|
||||
if (sig1.get_signal_str().compare(sig1.get_signal_str()))
|
||||
if (sig1.get_signal_str().compare(sig1.get_signal_str()) == 0)
|
||||
{
|
||||
equal = true;
|
||||
}
|
||||
|
@ -32,7 +32,7 @@
|
||||
#define GNSS_SDR_GNSS_SYNCHRO_H_
|
||||
|
||||
#include "gnss_signal.h"
|
||||
#include <deque>
|
||||
|
||||
|
||||
/*!
|
||||
* \brief This is the class that contains the information that is shared
|
||||
|
@ -335,8 +335,9 @@ endif(NOT ${GTEST_DIR_LOCAL})
|
||||
# add_test(acq_test acq_test)
|
||||
|
||||
add_executable(trk_test
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_dll_pll_veml_tracking_test.cc
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/arithmetic/tracking_loop_filter_test.cc
|
||||
)
|
||||
if(NOT ${ENABLE_PACKAGING})
|
||||
set_property(TARGET trk_test PROPERTY EXCLUDE_FROM_ALL TRUE)
|
||||
|
234
src/tests/arithmetic/tracking_loop_filter_test.cc
Normal file
234
src/tests/arithmetic/tracking_loop_filter_test.cc
Normal file
@ -0,0 +1,234 @@
|
||||
/*!
|
||||
* \file tracking_loop_filter_test.cc
|
||||
* \brief This file implements tests for the general loop filter
|
||||
* \author Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
|
||||
*
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "tracking_loop_filter.h"
|
||||
#include "tracking_2nd_PLL_filter.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
TEST(TrackingLoopFilterTest, FirstOrderLoop)
|
||||
{
|
||||
int loop_order = 1;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = false;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float g1 = noise_bandwidth*4.0;
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
|
||||
ASSERT_FLOAT_EQ( result, sample_data[i]*g1 );
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrackingLoopFilterTest, FirstOrderLoopWithLastIntegrator)
|
||||
{
|
||||
int loop_order = 1;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = true;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 0.01, 0.02, 0.02, 0.02 };
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float g1 = noise_bandwidth*4.0;
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
TEST(TrackingLoopFilterTest, SecondOrderLoop)
|
||||
{
|
||||
int loop_order = 2;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = false;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 13.37778, 0.0889, 0.0889, 0.0889 };
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrackingLoopFilterTest, SecondOrderLoopWithLastIntegrator)
|
||||
{
|
||||
int loop_order = 2;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = true;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 0.006689, 0.013422, 0.013511, 0.013600 };
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float g1 = noise_bandwidth*4.0;
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
|
||||
TEST(TrackingLoopFilterTest, ThirdOrderLoop)
|
||||
{
|
||||
int loop_order = 3;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = false;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 15.31877, 0.04494, 0.04520, 0.04546};
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrackingLoopFilterTest, ThirdOrderLoopWithLastIntegrator)
|
||||
{
|
||||
int loop_order = 3;
|
||||
float noise_bandwidth = 5.0;
|
||||
float update_interval = 0.001;
|
||||
bool include_last_integrator = true;
|
||||
|
||||
Tracking_loop_filter theFilter( update_interval,
|
||||
noise_bandwidth,
|
||||
loop_order,
|
||||
include_last_integrator );
|
||||
|
||||
EXPECT_EQ( theFilter.get_noise_bandwidth(), noise_bandwidth );
|
||||
EXPECT_EQ( theFilter.get_update_interval(), update_interval );
|
||||
EXPECT_EQ( theFilter.get_include_last_integrator(), include_last_integrator );
|
||||
EXPECT_EQ( theFilter.get_order(), loop_order );
|
||||
|
||||
std::vector< float > sample_data = { 0, 0, 1.0, 0.0, 0.0, 0.0 };
|
||||
std::vector< float > expected_out = { 0.0, 0.0, 0.007659, 0.015341, 0.015386, 0.015432};
|
||||
|
||||
theFilter.initialize( 0.0 );
|
||||
|
||||
float g1 = noise_bandwidth*4.0;
|
||||
|
||||
float result = 0.0;
|
||||
for( unsigned int i = 0; i < sample_data.size(); ++i )
|
||||
{
|
||||
result = theFilter.apply( sample_data[i] );
|
||||
ASSERT_NEAR( result, expected_out[i], 1e-4 );
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
|
@ -35,7 +35,7 @@ function [GNSS_tracking] = gps_l1_ca_dll_pll_read_tracking_dump (filename, count
|
||||
|
||||
m = nargchk (1,2,nargin);
|
||||
num_float_vars=16;
|
||||
num_double_vars=1;
|
||||
num_double_vars=2;
|
||||
double_size_bytes=8;
|
||||
float_size_bytes=4;
|
||||
skip_bytes_each_read=float_size_bytes*num_float_vars+double_size_bytes*num_double_vars;
|
||||
@ -100,6 +100,9 @@ function [GNSS_tracking] = gps_l1_ca_dll_pll_read_tracking_dump (filename, count
|
||||
bytes_shift=bytes_shift+float_size_bytes;
|
||||
fseek(f,bytes_shift,'bof'); % move to next interleaved float
|
||||
v17 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
|
||||
bytes_shift=bytes_shift+double_size_bytes;
|
||||
fseek(f,bytes_shift,'bof'); % move to next interleaved float
|
||||
v18 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
|
||||
fclose (f);
|
||||
|
||||
%%%%%%%% output vars %%%%%%%%
|
||||
@ -155,6 +158,7 @@ function [GNSS_tracking] = gps_l1_ca_dll_pll_read_tracking_dump (filename, count
|
||||
carrier_lock_test=v15;
|
||||
var1=v16;
|
||||
var2=v17;
|
||||
var3=v18;
|
||||
|
||||
GNSS_tracking.E=E;
|
||||
GNSS_tracking.P=P;
|
||||
@ -173,5 +177,6 @@ function [GNSS_tracking] = gps_l1_ca_dll_pll_read_tracking_dump (filename, count
|
||||
GNSS_tracking.carrier_lock_test=carrier_lock_test;
|
||||
GNSS_tracking.var1=var1;
|
||||
GNSS_tracking.var2=var2;
|
||||
GNSS_tracking.var3=var3;
|
||||
end
|
||||
|
||||
|
Loading…
x
Reference in New Issue
Block a user