From 83d77fabb0add15de6da7cca2d512909ce9e6ac1 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Thu, 13 Jun 2019 11:07:10 -0400 Subject: [PATCH] Update cubature filter library to align with coding conventions --- .../tracking/libs/cubature_filter.cc | 9 ++------ .../tracking/libs/cubature_filter.h | 21 +++++-------------- .../tracking/cubature_filter_test.cc | 16 +++++++------- 3 files changed, 15 insertions(+), 31 deletions(-) diff --git a/src/algorithms/tracking/libs/cubature_filter.cc b/src/algorithms/tracking/libs/cubature_filter.cc index d3d32350e..05b346a7c 100644 --- a/src/algorithms/tracking/libs/cubature_filter.cc +++ b/src/algorithms/tracking/libs/cubature_filter.cc @@ -84,7 +84,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, ModelFunction* transition_fcn, const arma::mat& noise_covariance) +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) { // Compute number of cubature points int nx = x_post.n_elem; @@ -125,7 +125,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, ModelFunction* measurement_fcn, const arma::mat& noise_covariance) +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) { // Compute number of cubature points int nx = x_pred.n_elem; @@ -188,8 +188,3 @@ arma::mat Cubature_filter::get_P_x_est() const { return P_x_est; } - -double Cubature_filter::func_number(double number, TestModel* func) -{ - return (*func)(number); -} diff --git a/src/algorithms/tracking/libs/cubature_filter.h b/src/algorithms/tracking/libs/cubature_filter.h index f4b31c43a..6a0806e0e 100644 --- a/src/algorithms/tracking/libs/cubature_filter.h +++ b/src/algorithms/tracking/libs/cubature_filter.h @@ -45,19 +45,11 @@ #include // Abstract model function -class ModelFunction{ +class Model_Function{ public: - ModelFunction() {}; + Model_Function() {}; virtual arma::vec operator() (arma::vec input) = 0; - virtual ~ModelFunction() = default; -}; - -class TestModel{ - public: - TestModel() {}; - //virtual arma::vec operator() (arma::vec input) = 0; - virtual double operator() (double input) = 0; - virtual ~TestModel() = default; + virtual ~Model_Function() = default; }; class Cubature_filter @@ -73,8 +65,8 @@ public: 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, 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); + 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); // Getters arma::mat get_x_pred() const; @@ -82,9 +74,6 @@ public: arma::mat get_x_est() const; arma::mat get_P_x_est() const; - //Test-dev - double func_number(double number, TestModel* func); - private: arma::vec x_pred_out; arma::mat P_x_pred_out; 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 e73bb1d45..a86d5dd7c 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 @@ -35,17 +35,17 @@ #define CUBATURE_TEST_N_TRIALS 1000 -class TransitionModel : public ModelFunction { +class Transition_Model : public Model_Function { public: - TransitionModel(arma::mat kf_F) {coeff_mat = kf_F;}; + Transition_Model(arma::mat kf_F) {coeff_mat = kf_F;}; virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;}; private: arma::mat coeff_mat; }; -class MeasurementModel : public ModelFunction { +class Measurement_Model : public Model_Function { public: - MeasurementModel(arma::mat kf_H) {coeff_mat = kf_H;}; + Measurement_Model(arma::mat kf_H) {coeff_mat = kf_H;}; virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;}; private: arma::mat coeff_mat; @@ -83,8 +83,8 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest) arma::mat kf_P_y; arma::mat kf_K; - ModelFunction* transition_function; - ModelFunction* measurement_function; + Model_Function* transition_function; + Model_Function* measurement_function; //--- Perform initializations ------------------------------ @@ -112,7 +112,7 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest) kf_F = arma::randu(nx,nx); kf_Q = arma::diagmat(arma::randu(nx,1)); - transition_function = new TransitionModel(kf_F); + transition_function = new Transition_Model(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); @@ -135,7 +135,7 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest) kf_y = kf_H*(kf_F*kf_x + eta) + nu; - measurement_function = new MeasurementModel(kf_H); + measurement_function = new Measurement_Model(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();