mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-09-19 18:59:47 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
Carles Fernandez 2019-06-16 14:55:18 +02:00
commit eb18b86c29
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
4 changed files with 71 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -4,7 +4,7 @@
* *
* Cubature_Filter implements the functionality of the Cubature Kalman * Cubature_Filter implements the functionality of the Cubature Kalman
* Filter, which uses multidimensional cubature rules to estimate the * 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 * an Unscented Kalman Filter which uses Unscented Transform rules to
* perform a similar estimation. * perform a similar estimation.
* *
@ -44,7 +44,7 @@
/***************** CUBATURE KALMAN FILTER *****************/ /***************** CUBATURE KALMAN FILTER *****************/
Cubature_filter::Cubature_filter() CubatureFilter::CubatureFilter()
{ {
int nx = 1; int nx = 1;
x_pred_out = arma::zeros(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); x_pred_out = arma::zeros(nx, 1);
P_x_pred_out = arma::eye(nx, nx) * (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; x_pred_out = x_pred_0;
P_x_pred_out = P_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; x_pred_out = x_pred_0;
P_x_pred_out = P_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 * 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 // Compute number of cubature points
int nx = x_post.n_elem; 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 * 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 // Compute number of cubature points
int nx = x_pred.n_elem; 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; 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; return P_x_pred_out;
} }
arma::mat Cubature_filter::get_x_est() const arma::mat CubatureFilter::get_x_est() const
{ {
return x_est; return x_est;
} }
arma::mat Cubature_filter::get_P_x_est() const arma::mat CubatureFilter::get_P_x_est() const
{ {
return P_x_est; return P_x_est;
} }
@ -205,7 +205,7 @@ arma::mat Cubature_filter::get_P_x_est() const
/***************** UNSCENTED KALMAN FILTER *****************/ /***************** UNSCENTED KALMAN FILTER *****************/
Unscented_filter::Unscented_filter() UnscentedFilter::UnscentedFilter()
{ {
int nx = 1; int nx = 1;
x_pred_out = arma::zeros(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); x_pred_out = arma::zeros(nx, 1);
P_x_pred_out = arma::eye(nx, nx) * (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; x_pred_out = x_pred_0;
P_x_pred_out = P_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; x_pred_out = x_pred_0;
P_x_pred_out = P_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 * 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 // Compute number of sigma points
int nx = x_post.n_elem; 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 * 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 // Compute number of sigma points
int nx = x_pred.n_elem; 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; 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; return P_x_pred_out;
} }
arma::mat Unscented_filter::get_x_est() const arma::mat UnscentedFilter::get_x_est() const
{ {
return x_est; return x_est;
} }
arma::mat Unscented_filter::get_P_x_est() const arma::mat UnscentedFilter::get_P_x_est() const
{ {
return P_x_est; return P_x_est;
} }

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

@ -2,9 +2,9 @@
* \file nonlinear_tracking.h * \file nonlinear_tracking.h
* \brief Interface of a library for nonlinear tracking algorithms * \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 * 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 * an Unscented Kalman Filter which uses Unscented Transform rules to
* perform a similar estimation. * perform a similar estimation.
* *
@ -47,29 +47,29 @@
#include <gnuradio/gr_complex.h> #include <gnuradio/gr_complex.h>
// Abstract model function // Abstract model function
class Model_Function class ModelFunction
{ {
public: public:
Model_Function(){}; ModelFunction(){};
virtual arma::vec operator()(arma::vec input) = 0; virtual arma::vec operator()(const arma::vec& input) = 0;
virtual ~Model_Function() = default; virtual ~ModelFunction() = default;
}; };
class Cubature_filter class CubatureFilter
{ {
public: public:
// Constructors and destructors // Constructors and destructors
Cubature_filter(); CubatureFilter();
Cubature_filter(int nx); CubatureFilter(int nx);
Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0); CubatureFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~Cubature_filter(); ~CubatureFilter();
// Reinitialization function // Reinitialization function
void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0); void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
// Prediction and estimation // 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 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, Model_Function* measurement_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 // Getters
arma::mat get_x_pred() const; arma::mat get_x_pred() const;
@ -84,21 +84,21 @@ private:
arma::mat P_x_est; arma::mat P_x_est;
}; };
class Unscented_filter class UnscentedFilter
{ {
public: public:
// Constructors and destructors // Constructors and destructors
Unscented_filter(); UnscentedFilter();
Unscented_filter(int nx); UnscentedFilter(int nx);
Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0); UnscentedFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~Unscented_filter(); ~UnscentedFilter();
// Reinitialization function // Reinitialization function
void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0); void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
// Prediction and estimation // 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 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, Model_Function* measurement_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 // Getters
arma::mat get_x_pred() const; arma::mat get_x_pred() const;

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_N_TRIALS 1000
#define CUBATURE_TEST_TOLERANCE 0.01 #define CUBATURE_TEST_TOLERANCE 0.01
class Transition_Model : public Model_Function class TransitionModel : public ModelFunction
{ {
public: public:
Transition_Model(arma::mat kf_F) { coeff_mat = kf_F; }; TransitionModel(const arma::mat& kf_F) { coeff_mat = kf_F; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; }; virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; };
private: private:
arma::mat coeff_mat; arma::mat coeff_mat;
}; };
class Measurement_Model : public Model_Function class MeasurementModel : public ModelFunction
{ {
public: public:
Measurement_Model(arma::mat kf_H) { coeff_mat = kf_H; }; MeasurementModel(const arma::mat& kf_H) { coeff_mat = kf_H; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; }; virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; };
private: private:
arma::mat coeff_mat; arma::mat coeff_mat;
@ -58,7 +58,7 @@ private:
TEST(CubatureFilterComputationTest, CubatureFilterTest) TEST(CubatureFilterComputationTest, CubatureFilterTest)
{ {
Cubature_filter kf_cubature; CubatureFilter kf_cubature;
arma::vec kf_x; arma::vec kf_x;
arma::mat kf_P_x; arma::mat kf_P_x;
@ -88,8 +88,8 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
arma::mat kf_P_y; arma::mat kf_P_y;
arma::mat kf_K; arma::mat kf_K;
Model_Function* transition_function; ModelFunction* transition_function;
Model_Function* measurement_function; ModelFunction* measurement_function;
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
@ -97,14 +97,15 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
std::default_random_engine e1(r()); std::default_random_engine e1(r());
std::normal_distribution<float> normal_dist(0, 5); std::normal_distribution<float> normal_dist(0, 5);
std::uniform_real_distribution<float> uniform_dist(0.1, 5.0); 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 nx = 0;
uint8_t ny = 0; uint8_t ny = 0;
for (uint16_t k = 0; k < CUBATURE_TEST_N_TRIALS; k++) for (uint16_t k = 0; k < CUBATURE_TEST_N_TRIALS; k++)
{ {
nx = std::rand() % 5 + 1; nx = static_cast<uint8_t>(uniform_dist_int(e1));
ny = std::rand() % 5 + 1; ny = static_cast<uint8_t>(uniform_dist_int(e1));
kf_x = arma::randn<arma::vec>(nx, 1); kf_x = arma::randn<arma::vec>(nx, 1);
@ -117,7 +118,7 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
kf_F = arma::randu<arma::mat>(nx, nx); kf_F = arma::randu<arma::mat>(nx, nx);
kf_Q = arma::diagmat(arma::randu<arma::vec>(nx, 1)); 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); arma::mat ttx = (*transition_function)(kf_x_post);
kf_cubature.predict_sequential(kf_x_post, kf_P_x_post, transition_function, kf_Q); 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; 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); 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(); 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_N_TRIALS 10
#define UNSCENTED_TEST_TOLERANCE 10 #define UNSCENTED_TEST_TOLERANCE 10
class Transition_Model_UKF : public Model_Function class TransitionModelUKF : public ModelFunction
{ {
public: public:
Transition_Model_UKF(arma::mat kf_F) { coeff_mat = kf_F; }; TransitionModelUKF(const arma::mat& kf_F) { coeff_mat = kf_F; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; }; virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; };
private: private:
arma::mat coeff_mat; arma::mat coeff_mat;
}; };
class Measurement_Model_UKF : public Model_Function class MeasurementModelUKF : public ModelFunction
{ {
public: public:
Measurement_Model_UKF(arma::mat kf_H) { coeff_mat = kf_H; }; MeasurementModelUKF(const arma::mat& kf_H) { coeff_mat = kf_H; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; }; virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; };
private: private:
arma::mat coeff_mat; arma::mat coeff_mat;
@ -58,7 +58,7 @@ private:
TEST(UnscentedFilterComputationTest, UnscentedFilterTest) TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
{ {
Unscented_filter kf_unscented; UnscentedFilter kf_unscented;
arma::vec kf_x; arma::vec kf_x;
arma::mat kf_P_x; arma::mat kf_P_x;
@ -88,8 +88,8 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
arma::mat kf_P_y; arma::mat kf_P_y;
arma::mat kf_K; arma::mat kf_K;
Model_Function* transition_function; ModelFunction* transition_function;
Model_Function* measurement_function; ModelFunction* measurement_function;
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
@ -97,14 +97,15 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
std::default_random_engine e1(r()); std::default_random_engine e1(r());
std::normal_distribution<float> normal_dist(0, 5); std::normal_distribution<float> normal_dist(0, 5);
std::uniform_real_distribution<float> uniform_dist(0.1, 5.0); 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 nx = 0;
uint8_t ny = 0; uint8_t ny = 0;
for (uint16_t k = 0; k < UNSCENTED_TEST_N_TRIALS; k++) for (uint16_t k = 0; k < UNSCENTED_TEST_N_TRIALS; k++)
{ {
nx = std::rand() % 5 + 1; nx = static_cast<uint8_t>(uniform_dist_int(e1));
ny = std::rand() % 5 + 1; ny = static_cast<uint8_t>(uniform_dist_int(e1));
kf_x = arma::randn<arma::vec>(nx, 1); kf_x = arma::randn<arma::vec>(nx, 1);
@ -117,7 +118,7 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
kf_F = arma::randu<arma::mat>(nx, nx); kf_F = arma::randu<arma::mat>(nx, nx);
kf_Q = arma::diagmat(arma::randu<arma::vec>(nx, 1)); 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); arma::mat ttx = (*transition_function)(kf_x_post);
kf_unscented.predict_sequential(kf_x_post, kf_P_x_post, transition_function, kf_Q); 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; 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); 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(); ukf_x_post = kf_unscented.get_x_est();