diff --git a/src/algorithms/tracking/libs/nonlinear_tracking.cc b/src/algorithms/tracking/libs/nonlinear_tracking.cc index 99d637f06..645185730 100644 --- a/src/algorithms/tracking/libs/nonlinear_tracking.cc +++ b/src/algorithms/tracking/libs/nonlinear_tracking.cc @@ -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; } diff --git a/src/algorithms/tracking/libs/nonlinear_tracking.h b/src/algorithms/tracking/libs/nonlinear_tracking.h index fdba4fec3..642645ea1 100644 --- a/src/algorithms/tracking/libs/nonlinear_tracking.h +++ b/src/algorithms/tracking/libs/nonlinear_tracking.h @@ -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 // 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; diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc index ec61ea47d..56e18df38 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc @@ -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 normal_dist(0, 5); std::uniform_real_distribution 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(uniform_dist_int(e1)); + ny = static_cast(uniform_dist_int(e1)); kf_x = arma::randn(nx, 1); @@ -117,7 +118,7 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest) kf_F = arma::randu(nx, nx); kf_Q = arma::diagmat(arma::randu(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(); diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc index d68a76ea6..eaef07845 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc @@ -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 normal_dist(0, 5); std::uniform_real_distribution 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(uniform_dist_int(e1)); + ny = static_cast(uniform_dist_int(e1)); kf_x = arma::randn(nx, 1); @@ -117,7 +118,7 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest) kf_F = arma::randu(nx, nx); kf_Q = arma::diagmat(arma::randu(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();