mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-12-02 06:38:08 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into tracking_debug

Browse Source
This commit is contained in:
Javier Arribas
2019-06-17 09:53:14 +02:00
parent 88f78a107c d49959d264
commit 47e6e173cb
20 changed files with 310 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@@ -138,6 +138,11 @@ Rtklib_Pvt::Rtklib_Pvt(ConfigurationInterface* configuration,
pvt_output_parameters.rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms;
}
pvt_output_parameters.kml_rate_ms = bc::lcm(configuration->property(role + ".kml_rate_ms", pvt_output_parameters.kml_rate_ms), pvt_output_parameters.output_rate_ms);
pvt_output_parameters.gpx_rate_ms = bc::lcm(configuration->property(role + ".gpx_rate_ms", pvt_output_parameters.gpx_rate_ms), pvt_output_parameters.output_rate_ms);
pvt_output_parameters.geojson_rate_ms = bc::lcm(configuration->property(role + ".geojson_rate_ms", pvt_output_parameters.geojson_rate_ms), pvt_output_parameters.output_rate_ms);
pvt_output_parameters.nmea_rate_ms = bc::lcm(configuration->property(role + ".nmea_rate_ms", pvt_output_parameters.nmea_rate_ms), pvt_output_parameters.output_rate_ms);
// Infer the type of receiver
/*
* TYPE | RECEIVER

41
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@@ -178,6 +178,11 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
std::string kml_dump_filename;
kml_dump_filename = d_dump_filename;
d_kml_output_enabled = conf_.kml_output_enabled;
d_kml_rate_ms = conf_.kml_rate_ms;
if (d_kml_rate_ms == 0)
{
d_kml_output_enabled = false;
}
if (d_kml_output_enabled)
{
d_kml_dump = std::make_shared<Kml_Printer>(conf_.kml_output_path);
@@ -192,6 +197,11 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
std::string gpx_dump_filename;
gpx_dump_filename = d_dump_filename;
d_gpx_output_enabled = conf_.gpx_output_enabled;
d_gpx_rate_ms = conf_.gpx_rate_ms;
if (d_gpx_rate_ms == 0)
{
d_gpx_output_enabled = false;
}
if (d_gpx_output_enabled)
{
d_gpx_dump = std::make_shared<Gpx_Printer>(conf_.gpx_output_path);
@@ -206,6 +216,11 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
std::string geojson_dump_filename;
geojson_dump_filename = d_dump_filename;
d_geojson_output_enabled = conf_.geojson_output_enabled;
d_geojson_rate_ms = conf_.geojson_rate_ms;
if (d_geojson_rate_ms == 0)
{
d_geojson_output_enabled = false;
}
if (d_geojson_output_enabled)
{
d_geojson_printer = std::make_shared<GeoJSON_Printer>(conf_.geojson_output_path);
@@ -218,6 +233,12 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
// initialize nmea_printer
d_nmea_output_file_enabled = (conf_.nmea_output_file_enabled or conf_.flag_nmea_tty_port);
d_nmea_rate_ms = conf_.nmea_rate_ms;
if (d_nmea_rate_ms == 0)
{
d_nmea_output_file_enabled = false;
}
if (d_nmea_output_file_enabled)
{
d_nmea_printer = std::make_shared<Nmea_Printer>(conf_.nmea_dump_filename, conf_.nmea_output_file_enabled, conf_.flag_nmea_tty_port, conf_.nmea_dump_devname, conf_.nmea_output_file_path);
@@ -1799,19 +1820,31 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
}
if (d_kml_output_enabled)
{
d_kml_dump->print_position(d_pvt_solver, false);
if (current_RX_time_ms % d_kml_rate_ms == 0)
{
d_kml_dump->print_position(d_pvt_solver, false);
}
}
if (d_gpx_output_enabled)
{
d_gpx_dump->print_position(d_pvt_solver, false);
if (current_RX_time_ms % d_gpx_rate_ms == 0)
{
d_gpx_dump->print_position(d_pvt_solver, false);
}
}
if (d_geojson_output_enabled)
{
d_geojson_printer->print_position(d_pvt_solver, false);
if (current_RX_time_ms % d_geojson_rate_ms == 0)
{
d_geojson_printer->print_position(d_pvt_solver, false);
}
}
if (d_nmea_output_file_enabled)
{
d_nmea_printer->Print_Nmea_Line(d_pvt_solver, false);
if (current_RX_time_ms % d_nmea_rate_ms == 0)
{
d_nmea_printer->Print_Nmea_Line(d_pvt_solver, false);
}
}
/*

5
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h
View File

@@ -106,6 +106,11 @@ private:
int32_t d_rtcm_MT1097_rate_ms; // Galileo MSM7. The type 7 Multiple Signal Message format for Europe’s Galileo system
int32_t d_rtcm_MSM_rate_ms;
int32_t d_kml_rate_ms;
int32_t d_gpx_rate_ms;
int32_t d_geojson_rate_ms;
int32_t d_nmea_rate_ms;
int32_t d_last_status_print_seg; // for status printer
uint32_t d_nchannels;

4
src/algorithms/PVT/libs/pvt_conf.cc
View File

@@ -35,6 +35,10 @@ Pvt_Conf::Pvt_Conf()
type_of_receiver = 0U;
output_rate_ms = 0;
display_rate_ms = 0;
kml_rate_ms = 1000;
gpx_rate_ms = 1000;
geojson_rate_ms = 1000;
nmea_rate_ms = 1000;
rinex_version = 0;
rinexobs_rate_ms = 0;

4
src/algorithms/PVT/libs/pvt_conf.h
View File

@@ -41,6 +41,10 @@ public:
uint32_t type_of_receiver;
int32_t output_rate_ms;
int32_t display_rate_ms;
int32_t kml_rate_ms;
int32_t gpx_rate_ms;
int32_t geojson_rate_ms;
int32_t nmea_rate_ms;
int32_t rinex_version;
int32_t rinexobs_rate_ms;

12
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt
View File

@@ -95,7 +95,11 @@ if((CMAKE_CXX_COMPILER_ID STREQUAL "GNU") AND NOT WIN32)
set(CMAKE_CXX_STANDARD 14)
else()
if(${FILESYSTEM_FOUND})
set(CMAKE_CXX_STANDARD 17)
if(CMAKE_VERSION VERSION_LESS 3.12)
set(CMAKE_CXX_STANDARD 17)
else()
set(CMAKE_CXX_STANDARD 20)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)
else()
set(CMAKE_CXX_STANDARD 14)
@@ -134,7 +138,11 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
set(CMAKE_CXX_STANDARD 14)
else()
if(${FILESYSTEM_FOUND})
set(CMAKE_CXX_STANDARD 17)
if(CMAKE_VERSION VERSION_LESS 3.12)
set(CMAKE_CXX_STANDARD 17)
else()
set(CMAKE_CXX_STANDARD 20)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)
else()
set(CMAKE_CXX_STANDARD 14)

8
src/algorithms/tracking/libs/CMakeLists.txt
View File

@@ -33,7 +33,6 @@ set(TRACKING_LIB_SOURCES
cpu_multicorrelator.cc
cpu_multicorrelator_real_codes.cc
cpu_multicorrelator_16sc.cc
nonlinear_tracking.cc
lock_detectors.cc
tcp_communication.cc
tcp_packet_data.cc
@@ -51,7 +50,6 @@ set(TRACKING_LIB_HEADERS
cpu_multicorrelator.h
cpu_multicorrelator_real_codes.h
cpu_multicorrelator_16sc.h
nonlinear_tracking.h
lock_detectors.h
tcp_communication.h
tcp_packet_data.h
@@ -65,6 +63,12 @@ set(TRACKING_LIB_HEADERS
exponential_smoother.h
)
if(ARMADILLO_VERSION_STRING VERSION_GREATER 7.400)
# sqrtmat_sympd() requires 7.400
set(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} nonlinear_tracking.cc)
set(TRACKING_LIB_HEADERS ${TRACKING_LIB_HEADERS} nonlinear_tracking.h)
endif()
if(ENABLE_FPGA)
set(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} fpga_multicorrelator.cc dll_pll_conf_fpga.cc)
set(TRACKING_LIB_HEADERS ${TRACKING_LIB_HEADERS} fpga_multicorrelator.h dll_pll_conf_fpga.h)

46
src/algorithms/tracking/libs/nonlinear_tracking.cc
View File

@@ -4,7 +4,7 @@
*
* Cubature_Filter implements the functionality of the Cubature Kalman
* Filter, which uses multidimensional cubature rules to estimate the
* time evolution of a nonlinear system. Unscented_filter implements
* time evolution of a nonlinear system. UnscentedFilter implements
* an Unscented Kalman Filter which uses Unscented Transform rules to
* perform a similar estimation.
*
@@ -44,7 +44,7 @@
/***************** CUBATURE KALMAN FILTER *****************/
Cubature_filter::Cubature_filter()
CubatureFilter::CubatureFilter()
{
int nx = 1;
x_pred_out = arma::zeros(nx, 1);
@@ -55,7 +55,7 @@ Cubature_filter::Cubature_filter()
}
Cubature_filter::Cubature_filter(int nx)
CubatureFilter::CubatureFilter(int nx)
{
x_pred_out = arma::zeros(nx, 1);
P_x_pred_out = arma::eye(nx, nx) * (nx + 1);
@@ -65,7 +65,7 @@ Cubature_filter::Cubature_filter(int nx)
}
Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
CubatureFilter::CubatureFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
P_x_pred_out = P_x_pred_0;
@@ -75,10 +75,10 @@ Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x
}
Cubature_filter::~Cubature_filter() = default;
CubatureFilter::~CubatureFilter() = default;
void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
void CubatureFilter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
P_x_pred_out = P_x_pred_0;
@@ -91,7 +91,7 @@ void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x
/*
* Perform the prediction step of the cubature Kalman filter
*/
void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance)
void CubatureFilter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, const arma::mat& noise_covariance)
{
// Compute number of cubature points
int nx = x_post.n_elem;
@@ -133,7 +133,7 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma
/*
* Perform the update step of the cubature Kalman filter
*/
void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance)
void CubatureFilter::update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, ModelFunction* measurement_fcn, const arma::mat& noise_covariance)
{
// Compute number of cubature points
int nx = x_pred.n_elem;
@@ -178,25 +178,25 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
}
arma::mat Cubature_filter::get_x_pred() const
arma::mat CubatureFilter::get_x_pred() const
{
return x_pred_out;
}
arma::mat Cubature_filter::get_P_x_pred() const
arma::mat CubatureFilter::get_P_x_pred() const
{
return P_x_pred_out;
}
arma::mat Cubature_filter::get_x_est() const
arma::mat CubatureFilter::get_x_est() const
{
return x_est;
}
arma::mat Cubature_filter::get_P_x_est() const
arma::mat CubatureFilter::get_P_x_est() const
{
return P_x_est;
}
@@ -205,7 +205,7 @@ arma::mat Cubature_filter::get_P_x_est() const
/***************** UNSCENTED KALMAN FILTER *****************/
Unscented_filter::Unscented_filter()
UnscentedFilter::UnscentedFilter()
{
int nx = 1;
x_pred_out = arma::zeros(nx, 1);
@@ -216,7 +216,7 @@ Unscented_filter::Unscented_filter()
}
Unscented_filter::Unscented_filter(int nx)
UnscentedFilter::UnscentedFilter(int nx)
{
x_pred_out = arma::zeros(nx, 1);
P_x_pred_out = arma::eye(nx, nx) * (nx + 1);
@@ -226,7 +226,7 @@ Unscented_filter::Unscented_filter(int nx)
}
Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
UnscentedFilter::UnscentedFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
P_x_pred_out = P_x_pred_0;
@@ -236,10 +236,10 @@ Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P
}
Unscented_filter::~Unscented_filter() = default;
UnscentedFilter::~UnscentedFilter() = default;
void Unscented_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
void UnscentedFilter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
P_x_pred_out = P_x_pred_0;
@@ -252,7 +252,7 @@ void Unscented_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_
/*
* Perform the prediction step of the Unscented Kalman filter
*/
void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance)
void UnscentedFilter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, const arma::mat& noise_covariance)
{
// Compute number of sigma points
int nx = x_post.n_elem;
@@ -307,7 +307,7 @@ void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::m
/*
* Perform the update step of the Unscented Kalman filter
*/
void Unscented_filter::update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance)
void UnscentedFilter::update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, ModelFunction* measurement_fcn, const arma::mat& noise_covariance)
{
// Compute number of sigma points
int nx = x_pred.n_elem;
@@ -364,25 +364,25 @@ void Unscented_filter::update_sequential(const arma::vec& z_upd, const arma::vec
}
arma::mat Unscented_filter::get_x_pred() const
arma::mat UnscentedFilter::get_x_pred() const
{
return x_pred_out;
}
arma::mat Unscented_filter::get_P_x_pred() const
arma::mat UnscentedFilter::get_P_x_pred() const
{
return P_x_pred_out;
}
arma::mat Unscented_filter::get_x_est() const
arma::mat UnscentedFilter::get_x_est() const
{
return x_est;
}
arma::mat Unscented_filter::get_P_x_est() const
arma::mat UnscentedFilter::get_P_x_est() const
{
return P_x_est;
}

40
src/algorithms/tracking/libs/nonlinear_tracking.h
View File

@@ -2,9 +2,9 @@
* \file nonlinear_tracking.h
* \brief Interface of a library for nonlinear tracking algorithms
*
* Cubature_Filter implements the functionality of the Cubature Kalman
* CubatureFilter implements the functionality of the Cubature Kalman
* Filter, which uses multidimensional cubature rules to estimate the
* time evolution of a nonlinear system. Unscented_filter implements
* time evolution of a nonlinear system. UnscentedFilter implements
* an Unscented Kalman Filter which uses Unscented Transform rules to
* perform a similar estimation.
*
@@ -47,29 +47,29 @@
#include <gnuradio/gr_complex.h>
// Abstract model function
class Model_Function
class ModelFunction
{
public:
Model_Function(){};
virtual arma::vec operator()(arma::vec input) = 0;
virtual ~Model_Function() = default;
ModelFunction(){};
virtual arma::vec operator()(const arma::vec& input) = 0;
virtual ~ModelFunction() = default;
};
class Cubature_filter
class CubatureFilter
{
public:
// Constructors and destructors
Cubature_filter();
Cubature_filter(int nx);
Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~Cubature_filter();
CubatureFilter();
CubatureFilter(int nx);
CubatureFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~CubatureFilter();
// Reinitialization function
void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
// Prediction and estimation
void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance);
void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance);
void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, const arma::mat& noise_covariance);
void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, ModelFunction* measurement_fcn, const arma::mat& noise_covariance);
// Getters
arma::mat get_x_pred() const;
@@ -84,21 +84,21 @@ private:
arma::mat P_x_est;
};
class Unscented_filter
class UnscentedFilter
{
public:
// Constructors and destructors
Unscented_filter();
Unscented_filter(int nx);
Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~Unscented_filter();
UnscentedFilter();
UnscentedFilter(int nx);
UnscentedFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~UnscentedFilter();
// Reinitialization function
void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
// Prediction and estimation
void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance);
void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance);
void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, const arma::mat& noise_covariance);
void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, ModelFunction* measurement_fcn, const arma::mat& noise_covariance);
// Getters
arma::mat get_x_pred() const;

15
src/tests/CMakeLists.txt
View File

@@ -161,6 +161,11 @@ if(ENABLE_FPGA)
add_definitions(-DFPGA_BLOCKS_TEST=1)
endif()
if(ARMADILLO_VERSION_STRING VERSION_GREATER 8.400)
# mvnrnd() requires 8.400
add_definitions(-DARMADILLO_HAVE_MVNRND=1)
endif()
find_package(Gnuplot)
if(GNUPLOT_FOUND)
add_definitions(-DGNUPLOT_EXECUTABLE="${GNUPLOT_EXECUTABLE}")
@@ -793,14 +798,20 @@ endif()
#########################################################
if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA)
set(NONLINEAR_SOURCES "")
if(ARMADILLO_VERSION_STRING VERSION_GREATER 8.400)
set(NONLINEAR_SOURCES
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc
)
endif()
add_executable(trk_test
${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc
${NONLINEAR_SOURCES}
)
if(${FILESYSTEM_FOUND})
target_compile_definitions(trk_test PRIVATE -DHAS_STD_FILESYSTEM=1)

2
src/tests/test_main.cc
View File

@@ -99,8 +99,10 @@ DECLARE_string(log_dir);
#endif
#include "unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc"
#if ARMADILLO_HAVE_MVNRND
#include "unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc"
#endif
#include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc"

27
src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc
View File

@@ -36,21 +36,21 @@
#define CUBATURE_TEST_N_TRIALS 1000
#define CUBATURE_TEST_TOLERANCE 0.01
class Transition_Model : public Model_Function
class TransitionModel : public ModelFunction
{
public:
Transition_Model(arma::mat kf_F) { coeff_mat = kf_F; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
TransitionModel(const arma::mat& kf_F) { coeff_mat = kf_F; };
virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; };
private:
arma::mat coeff_mat;
};
class Measurement_Model : public Model_Function
class MeasurementModel : public ModelFunction
{
public:
Measurement_Model(arma::mat kf_H) { coeff_mat = kf_H; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
MeasurementModel(const arma::mat& kf_H) { coeff_mat = kf_H; };
virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; };
private:
arma::mat coeff_mat;
@@ -58,7 +58,7 @@ private:
TEST(CubatureFilterComputationTest, CubatureFilterTest)
{
Cubature_filter kf_cubature;
CubatureFilter kf_cubature;
arma::vec kf_x;
arma::mat kf_P_x;
@@ -88,8 +88,8 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
arma::mat kf_P_y;
arma::mat kf_K;
Model_Function* transition_function;
Model_Function* measurement_function;
ModelFunction* transition_function;
ModelFunction* measurement_function;
//--- Perform initializations ------------------------------
@@ -97,14 +97,15 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
std::default_random_engine e1(r());
std::normal_distribution<float> normal_dist(0, 5);
std::uniform_real_distribution<float> uniform_dist(0.1, 5.0);
std::uniform_int_distribution<> uniform_dist_int(1, 5);
uint8_t nx = 0;
uint8_t ny = 0;
for (uint16_t k = 0; k < CUBATURE_TEST_N_TRIALS; k++)
{
nx = std::rand() % 5 + 1;
ny = std::rand() % 5 + 1;
nx = static_cast<uint8_t>(uniform_dist_int(e1));
ny = static_cast<uint8_t>(uniform_dist_int(e1));
kf_x = arma::randn<arma::vec>(nx, 1);
@@ -117,7 +118,7 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
kf_F = arma::randu<arma::mat>(nx, nx);
kf_Q = arma::diagmat(arma::randu<arma::vec>(nx, 1));
transition_function = new Transition_Model(kf_F);
transition_function = new TransitionModel(kf_F);
arma::mat ttx = (*transition_function)(kf_x_post);
kf_cubature.predict_sequential(kf_x_post, kf_P_x_post, transition_function, kf_Q);
@@ -140,7 +141,7 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
kf_y = kf_H * (kf_F * kf_x + eta) + nu;
measurement_function = new Measurement_Model(kf_H);
measurement_function = new MeasurementModel(kf_H);
kf_cubature.update_sequential(kf_y, kf_x_pre, kf_P_x_pre, measurement_function, kf_R);
ckf_x_post = kf_cubature.get_x_est();

27
src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc
View File

@@ -36,21 +36,21 @@
#define UNSCENTED_TEST_N_TRIALS 10
#define UNSCENTED_TEST_TOLERANCE 10
class Transition_Model_UKF : public Model_Function
class TransitionModelUKF : public ModelFunction
{
public:
Transition_Model_UKF(arma::mat kf_F) { coeff_mat = kf_F; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
TransitionModelUKF(const arma::mat& kf_F) { coeff_mat = kf_F; };
virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; };
private:
arma::mat coeff_mat;
};
class Measurement_Model_UKF : public Model_Function
class MeasurementModelUKF : public ModelFunction
{
public:
Measurement_Model_UKF(arma::mat kf_H) { coeff_mat = kf_H; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
MeasurementModelUKF(const arma::mat& kf_H) { coeff_mat = kf_H; };
virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; };
private:
arma::mat coeff_mat;
@@ -58,7 +58,7 @@ private:
TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
{
Unscented_filter kf_unscented;
UnscentedFilter kf_unscented;
arma::vec kf_x;
arma::mat kf_P_x;
@@ -88,8 +88,8 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
arma::mat kf_P_y;
arma::mat kf_K;
Model_Function* transition_function;
Model_Function* measurement_function;
ModelFunction* transition_function;
ModelFunction* measurement_function;
//--- Perform initializations ------------------------------
@@ -97,14 +97,15 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
std::default_random_engine e1(r());
std::normal_distribution<float> normal_dist(0, 5);
std::uniform_real_distribution<float> uniform_dist(0.1, 5.0);
std::uniform_int_distribution<> uniform_dist_int(1, 5);
uint8_t nx = 0;
uint8_t ny = 0;
for (uint16_t k = 0; k < UNSCENTED_TEST_N_TRIALS; k++)
{
nx = std::rand() % 5 + 1;
ny = std::rand() % 5 + 1;
nx = static_cast<uint8_t>(uniform_dist_int(e1));
ny = static_cast<uint8_t>(uniform_dist_int(e1));
kf_x = arma::randn<arma::vec>(nx, 1);
@@ -117,7 +118,7 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
kf_F = arma::randu<arma::mat>(nx, nx);
kf_Q = arma::diagmat(arma::randu<arma::vec>(nx, 1));
transition_function = new Transition_Model_UKF(kf_F);
transition_function = new TransitionModelUKF(kf_F);
arma::mat ttx = (*transition_function)(kf_x_post);
kf_unscented.predict_sequential(kf_x_post, kf_P_x_post, transition_function, kf_Q);
@@ -140,7 +141,7 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
kf_y = kf_H * (kf_F * kf_x + eta) + nu;
measurement_function = new Measurement_Model_UKF(kf_H);
measurement_function = new MeasurementModelUKF(kf_H);
kf_unscented.update_sequential(kf_y, kf_x_pre, kf_P_x_pre, measurement_function, kf_R);
ukf_x_post = kf_unscented.get_x_est();

2
src/utils/rinex2assist/CMakeLists.txt
View File

@@ -99,3 +99,5 @@ else()
message(STATUS "Boost Iostreams library not found.")
message(STATUS " rinex2assist will not be built.")
endif()
set(Boost_FOUND TRUE) # trick for summary report