Pass vectors and matrices by reference, rename classes to CamelCase style

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

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

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 ------------------------------
 
@@ -118,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);
@@ -141,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();

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 ------------------------------
 
@@ -118,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);
@@ -141,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();