mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-09 03:20:01 +00:00
Merge branch 'next' of https://github.com/carlesfernandez/gnss-sdr into next
This commit is contained in:
Carles Fernandez
commit
eb18b86c29
@ -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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user