Update cubature filter library to align with coding conventions

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);
-}

src/algorithms/tracking/libs/cubature_filter.h

@@ -45,19 +45,11 @@
 #include <gnuradio/gr_complex.h>
 
 // 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;

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<arma::mat>(nx,nx);
             kf_Q = arma::diagmat(arma::randu<arma::vec>(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();