Apply clang-format

src/algorithms/tracking/libs/nonlinear_tracking.cc

@@ -54,6 +54,7 @@ Cubature_filter::Cubature_filter()
     P_x_est = P_x_pred_out;
 }
 
+
 Cubature_filter::Cubature_filter(int nx)
 {
     x_pred_out = arma::zeros(nx, 1);
@@ -63,6 +64,7 @@ Cubature_filter::Cubature_filter(int nx)
     P_x_est = P_x_pred_out;
 }
 
+
 Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
 {
     x_pred_out = x_pred_0;
@@ -72,8 +74,10 @@ Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x
     P_x_est = P_x_pred_out;
 }
 
+
 Cubature_filter::~Cubature_filter() = default;
 
+
 void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
 {
     x_pred_out = x_pred_0;
@@ -94,11 +98,11 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma
     int np = 2 * nx;
 
     // Generator Matrix
-    arma::mat gen_one = arma::join_horiz(arma::eye(nx,nx),-1.0*arma::eye(nx,nx));
+    arma::mat gen_one = arma::join_horiz(arma::eye(nx, nx), -1.0 * arma::eye(nx, nx));
 
     // Initialize predicted mean and covariance
-    arma::vec x_pred = arma::zeros(nx,1);
+    arma::vec x_pred = arma::zeros(nx, 1);
-    arma::mat P_x_pred = arma::zeros(nx,nx);
+    arma::mat P_x_pred = arma::zeros(nx, nx);
 
     // Factorize posterior covariance
     arma::mat Sm_post = arma::chol(P_x_post, "lower");
@@ -109,22 +113,23 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma
 
     for (uint8_t i = 0; i < np; i++)
         {
-        Xi_post = Sm_post * (std::sqrt(((float) np) / 2.0) * gen_one.col(i)) + x_post;
+            Xi_post = Sm_post * (std::sqrt(static_cast<float>(np) / 2.0) * gen_one.col(i)) + x_post;
             Xi_pred = (*transition_fcn)(Xi_post);
 
             x_pred = x_pred + Xi_pred;
-        P_x_pred = P_x_pred + Xi_pred*Xi_pred.t();
+            P_x_pred = P_x_pred + Xi_pred * Xi_pred.t();
         }
 
     // Compute predicted mean and error covariance
-    x_pred = x_pred / ((float) np);
+    x_pred = x_pred / static_cast<float>(np);
-    P_x_pred = P_x_pred / ((float) np) - x_pred*x_pred.t() + noise_covariance;
+    P_x_pred = P_x_pred / static_cast<float>(np) - x_pred * x_pred.t() + noise_covariance;
 
     // Store predicted mean and error covariance
     x_pred_out = x_pred;
     P_x_pred_out = P_x_pred;
 }
 
+
 /*
  * Perform the update step of the cubature Kalman filter
  */
@@ -136,12 +141,12 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
     int np = 2 * nx;
 
     // Generator Matrix
-    arma::mat gen_one = arma::join_horiz(arma::eye(nx,nx),-1.0*arma::eye(nx,nx));
+    arma::mat gen_one = arma::join_horiz(arma::eye(nx, nx), -1.0 * arma::eye(nx, nx));
 
     // Initialize estimated predicted measurement and covariances
-    arma::mat z_pred = arma::zeros(nz,1);
+    arma::mat z_pred = arma::zeros(nz, 1);
-    arma::mat P_zz_pred = arma::zeros(nz,nz);
+    arma::mat P_zz_pred = arma::zeros(nz, nz);
-    arma::mat P_xz_pred = arma::zeros(nx,nz);
+    arma::mat P_xz_pred = arma::zeros(nx, nz);
 
     // Factorize predicted covariance
     arma::mat Sm_pred = arma::chol(P_x_pred, "lower");
@@ -151,48 +156,53 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
     arma::vec Zi_pred;
     for (uint8_t i = 0; i < np; i++)
         {
-        Xi_pred = Sm_pred * (std::sqrt(((float) np) / 2.0) * gen_one.col(i)) + x_pred;
+            Xi_pred = Sm_pred * (std::sqrt(static_cast<float>(np) / 2.0) * gen_one.col(i)) + x_pred;
             Zi_pred = (*measurement_fcn)(Xi_pred);
 
             z_pred = z_pred + Zi_pred;
-        P_zz_pred = P_zz_pred + Zi_pred*Zi_pred.t();
+            P_zz_pred = P_zz_pred + Zi_pred * Zi_pred.t();
-        P_xz_pred = P_xz_pred + Xi_pred*Zi_pred.t();
+            P_xz_pred = P_xz_pred + Xi_pred * Zi_pred.t();
         }
 
     // Compute measurement mean, covariance and cross covariance
-    z_pred = z_pred / ((float) np);
+    z_pred = z_pred / static_cast<float>(np);
-    P_zz_pred = P_zz_pred / ((float) np) - z_pred*z_pred.t() + noise_covariance;
+    P_zz_pred = P_zz_pred / static_cast<float>(np) - z_pred * z_pred.t() + noise_covariance;
-    P_xz_pred = P_xz_pred / ((float) np) - x_pred*z_pred.t();
+    P_xz_pred = P_xz_pred / static_cast<float>(np) - x_pred * z_pred.t();
 
     // Compute cubature Kalman gain
-    arma::mat W_k = P_xz_pred*arma::inv(P_zz_pred);
+    arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred);
 
     // Compute and store the updated mean and error covariance
-    x_est = x_pred + W_k*(z_upd - z_pred);
+    x_est = x_pred + W_k * (z_upd - z_pred);
-    P_x_est = P_x_pred - W_k*P_zz_pred*W_k.t();
+    P_x_est = P_x_pred - W_k * P_zz_pred * W_k.t();
 }
 
+
 arma::mat Cubature_filter::get_x_pred() const
 {
     return x_pred_out;
 }
 
+
 arma::mat Cubature_filter::get_P_x_pred() const
 {
     return P_x_pred_out;
 }
 
+
 arma::mat Cubature_filter::get_x_est() const
 {
     return x_est;
 }
 
+
 arma::mat Cubature_filter::get_P_x_est() const
 {
     return P_x_est;
 }
 /***************** END CUBATURE KALMAN FILTER *****************/
 
+
 /***************** UNSCENTED KALMAN FILTER *****************/
 
 Unscented_filter::Unscented_filter()
@@ -205,6 +215,7 @@ Unscented_filter::Unscented_filter()
     P_x_est = P_x_pred_out;
 }
 
+
 Unscented_filter::Unscented_filter(int nx)
 {
     x_pred_out = arma::zeros(nx, 1);
@@ -214,6 +225,7 @@ Unscented_filter::Unscented_filter(int nx)
     P_x_est = P_x_pred_out;
 }
 
+
 Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
 {
     x_pred_out = x_pred_0;
@@ -223,8 +235,10 @@ Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P
     P_x_est = P_x_pred_out;
 }
 
+
 Unscented_filter::~Unscented_filter() = default;
 
+
 void Unscented_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
 {
     x_pred_out = x_pred_0;
@@ -248,39 +262,39 @@ void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::m
     float kappa = 0.0;
     float beta = 2.0;
 
-    float lambda = std::pow(alpha,2.0)*(((float) nx) + kappa) - ((float) nx);
+    float lambda = std::pow(alpha, 2.0) * (static_cast<float>(nx) + kappa) - static_cast<float>(nx);
 
     // Compute UT Weights
-    float W0_m = lambda / (((float) nx) + lambda);
+    float W0_m = lambda / (static_cast<float>(nx) + lambda);
-    float W0_c = lambda / (((float) nx) + lambda) + (1 - std::pow(alpha,2.0) + beta);
+    float W0_c = lambda / (static_cast<float>(nx) + lambda) + (1 - std::pow(alpha, 2.0) + beta);
-    float Wi_m = 1.0 / (2.0 * (((float) nx) + lambda));
+    float Wi_m = 1.0 / (2.0 * (static_cast<float>(nx) + lambda));
 
     // Propagate and evaluate sigma points
-    arma::mat Xi_fact = arma::zeros(nx,nx);
+    arma::mat Xi_fact = arma::zeros(nx, nx);
-    arma::mat Xi_post = arma::zeros(nx,np);
+    arma::mat Xi_post = arma::zeros(nx, np);
-    arma::mat Xi_pred = arma::zeros(nx,np);
+    arma::mat Xi_pred = arma::zeros(nx, np);
 
 
     Xi_post.col(0) = x_post;
     Xi_pred.col(0) = (*transition_fcn)(Xi_post.col(0));
     for (uint8_t i = 1; i <= nx; i++)
         {
-        Xi_fact = std::sqrt(((float) nx) + lambda) * arma::sqrtmat_sympd(P_x_post);
+            Xi_fact = std::sqrt(static_cast<float>(nx) + lambda) * arma::sqrtmat_sympd(P_x_post);
-        Xi_post.col(i) = x_post + Xi_fact.col(i-1);
+            Xi_post.col(i) = x_post + Xi_fact.col(i - 1);
-        Xi_post.col(i+nx) = x_post - Xi_fact.col(i-1);
+            Xi_post.col(i + nx) = x_post - Xi_fact.col(i - 1);
 
             Xi_pred.col(i) = (*transition_fcn)(Xi_post.col(i));
-        Xi_pred.col(i+nx) = (*transition_fcn)(Xi_post.col(i+nx)); 
+            Xi_pred.col(i + nx) = (*transition_fcn)(Xi_post.col(i + nx));
         }
 
     // Compute predicted mean
-    arma::vec x_pred = W0_m*Xi_pred.col(0) + Wi_m*arma::sum(Xi_pred.cols(1,np-1),1);
+    arma::vec x_pred = W0_m * Xi_pred.col(0) + Wi_m * arma::sum(Xi_pred.cols(1, np - 1), 1);
 
     // Compute predicted error covariance
-    arma::mat P_x_pred = W0_c*((Xi_pred.col(0)-x_pred) * (Xi_pred.col(0).t()-x_pred.t()));
+    arma::mat P_x_pred = W0_c * ((Xi_pred.col(0) - x_pred) * (Xi_pred.col(0).t() - x_pred.t()));
     for (uint8_t i = 1; i < np; i++)
         {
-        P_x_pred = P_x_pred + Wi_m*((Xi_pred.col(i)-x_pred) * (Xi_pred.col(i).t()-x_pred.t()));
+            P_x_pred = P_x_pred + Wi_m * ((Xi_pred.col(i) - x_pred) * (Xi_pred.col(i).t() - x_pred.t()));
         }
     P_x_pred = P_x_pred + noise_covariance;
 
@@ -289,6 +303,7 @@ void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::m
     P_x_pred_out = P_x_pred;
 }
 
+
 /*
  * Perform the update step of the Unscented Kalman filter
  */
@@ -303,32 +318,32 @@ void Unscented_filter::update_sequential(const arma::vec& z_upd, const arma::vec
     float kappa = 0.0;
     float beta = 2.0;
 
-    float lambda = std::pow(alpha,2.0)*(((float) nx) + kappa) - ((float) nx);
+    float lambda = std::pow(alpha, 2.0) * (static_cast<float>(nx) + kappa) - static_cast<float>(nx);
 
     // Compute UT Weights
-    float W0_m = lambda / (((float) nx) + lambda);
+    float W0_m = lambda / (static_cast<float>(nx) + lambda);
-    float W0_c = lambda / (((float) nx) + lambda) + (1 - std::pow(alpha,2.0) + beta);
+    float W0_c = lambda / (static_cast<float>(nx) + lambda) + (1.0 - std::pow(alpha, 2.0) + beta);
-    float Wi_m = 1.0 / (2.0 * (((float) nx) + lambda));
+    float Wi_m = 1.0 / (2.0 * (static_cast<float>(nx) + lambda));
 
     // Propagate and evaluate sigma points
-    arma::mat Xi_fact = arma::zeros(nx,nx);
+    arma::mat Xi_fact = arma::zeros(nx, nx);
-    arma::mat Xi_pred = arma::zeros(nx,np);
+    arma::mat Xi_pred = arma::zeros(nx, np);
-    arma::mat Zi_pred = arma::zeros(nz,np);
+    arma::mat Zi_pred = arma::zeros(nz, np);
 
     Xi_pred.col(0) = x_pred;
     Zi_pred.col(0) = (*measurement_fcn)(Xi_pred.col(0));
     for (uint8_t i = 1; i <= nx; i++)
         {
-        Xi_fact = std::sqrt(((float) nx) + lambda) * arma::sqrtmat_sympd(P_x_pred);
+            Xi_fact = std::sqrt(static_cast<float>(nx) + lambda) * arma::sqrtmat_sympd(P_x_pred);
-        Xi_pred.col(i) = x_pred + Xi_fact.col(i-1);
+            Xi_pred.col(i) = x_pred + Xi_fact.col(i - 1);
-        Xi_pred.col(i+nx) = x_pred - Xi_fact.col(i-1);
+            Xi_pred.col(i + nx) = x_pred - Xi_fact.col(i - 1);
 
             Zi_pred.col(i) = (*measurement_fcn)(Xi_pred.col(i));
-        Zi_pred.col(i+nx) = (*measurement_fcn)(Xi_pred.col(i+nx)); 
+            Zi_pred.col(i + nx) = (*measurement_fcn)(Xi_pred.col(i + nx));
         }
 
     // Compute measurement mean
-    arma::mat z_pred = W0_m*Zi_pred.col(0) + Wi_m*arma::sum(Zi_pred.cols(1,np-1),1);
+    arma::mat z_pred = W0_m * Zi_pred.col(0) + Wi_m * arma::sum(Zi_pred.cols(1, np - 1), 1);
 
     // Compute measurement covariance and cross covariance
     arma::mat P_zz_pred = W0_c * ((Zi_pred.col(0) - z_pred) * (Zi_pred.col(0).t() - z_pred.t()));
@@ -341,30 +356,35 @@ void Unscented_filter::update_sequential(const arma::vec& z_upd, const arma::vec
     P_zz_pred = P_zz_pred + noise_covariance;
 
     // Estimate cubature Kalman gain
-    arma::mat W_k = P_xz_pred*arma::inv(P_zz_pred);
+    arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred);
 
     // Estimate and store the updated mean and error covariance
-    x_est = x_pred + W_k*(z_upd - z_pred);
+    x_est = x_pred + W_k * (z_upd - z_pred);
-    P_x_est = P_x_pred - W_k*P_zz_pred*W_k.t();
+    P_x_est = P_x_pred - W_k * P_zz_pred * W_k.t();
 }
 
+
 arma::mat Unscented_filter::get_x_pred() const
 {
     return x_pred_out;
 }
 
+
 arma::mat Unscented_filter::get_P_x_pred() const
 {
     return P_x_pred_out;
 }
 
+
 arma::mat Unscented_filter::get_x_est() const
 {
     return x_est;
 }
 
+
 arma::mat Unscented_filter::get_P_x_est() const
 {
     return P_x_est;
 }
+
 /***************** END UNSCENTED KALMAN FILTER *****************/

src/algorithms/tracking/libs/nonlinear_tracking.h

@@ -47,10 +47,11 @@
 #include <gnuradio/gr_complex.h>
 
 // Abstract model function
-class Model_Function{
+class Model_Function
-    public:
+{
-        Model_Function() {};
+public:
-        virtual arma::vec operator() (arma::vec input) = 0;
+    Model_Function(){};
+    virtual arma::vec operator()(arma::vec input) = 0;
     virtual ~Model_Function() = default;
 };
 

src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc

@@ -36,19 +36,23 @@
 #define UNSCENTED_TEST_N_TRIALS 10
 #define UNSCENTED_TEST_TOLERANCE 10
 
-class Transition_Model_UKF : public Model_Function {
+class Transition_Model_UKF : public Model_Function
-    public:
+{
-        Transition_Model_UKF(arma::mat kf_F) {coeff_mat = kf_F;};
+public:
-        virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;};
+    Transition_Model_UKF(arma::mat kf_F) { coeff_mat = kf_F; };
-    private:
+    virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
+
+private:
     arma::mat coeff_mat;
 };
 
-class Measurement_Model_UKF : public Model_Function {
+class Measurement_Model_UKF : public Model_Function
-    public:
+{
-        Measurement_Model_UKF(arma::mat kf_H) {coeff_mat = kf_H;};
+public:
-        virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;};
+    Measurement_Model_UKF(arma::mat kf_H) { coeff_mat = kf_H; };
-    private:
+    virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
+
+private:
     arma::mat coeff_mat;
 };
 
@@ -102,21 +106,21 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
             nx = std::rand() % 5 + 1;
             ny = std::rand() % 5 + 1;
 
-            kf_x = arma::randn<arma::vec>(nx,1);
+            kf_x = arma::randn<arma::vec>(nx, 1);
 
-            kf_P_x_post = 5.0 * arma::diagmat(arma::randu<arma::vec>(nx,1));
+            kf_P_x_post = 5.0 * arma::diagmat(arma::randu<arma::vec>(nx, 1));
             kf_x_post = arma::mvnrnd(kf_x, kf_P_x_post);
 
             kf_unscented.initialize(kf_x_post, kf_P_x_post);
 
             // Prediction Step
-            kf_F = arma::randu<arma::mat>(nx,nx);
+            kf_F = arma::randu<arma::mat>(nx, nx);
-            kf_Q = arma::diagmat(arma::randu<arma::vec>(nx,1));
+            kf_Q = arma::diagmat(arma::randu<arma::vec>(nx, 1));
 
             transition_function = new Transition_Model_UKF(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);
+            kf_unscented.predict_sequential(kf_x_post, kf_P_x_post, transition_function, kf_Q);
 
             ukf_x_pre = kf_unscented.get_x_pred();
             ukf_P_x_pre = kf_unscented.get_P_x_pred();
@@ -128,16 +132,16 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
             EXPECT_TRUE(arma::approx_equal(ukf_P_x_pre, kf_P_x_pre, "absdiff", UNSCENTED_TEST_TOLERANCE));
 
             // Update Step
-            kf_H = arma::randu<arma::mat>(ny,nx);
+            kf_H = arma::randu<arma::mat>(ny, nx);
-            kf_R = arma::diagmat(arma::randu<arma::vec>(ny,1));
+            kf_R = arma::diagmat(arma::randu<arma::vec>(ny, 1));
 
-            eta = arma::mvnrnd(arma::zeros<arma::vec>(nx,1),kf_Q);
+            eta = arma::mvnrnd(arma::zeros<arma::vec>(nx, 1), kf_Q);
-            nu = arma::mvnrnd(arma::zeros<arma::vec>(ny,1),kf_R);
+            nu = arma::mvnrnd(arma::zeros<arma::vec>(ny, 1), kf_R);
 
-            kf_y = kf_H*(kf_F*kf_x + eta) + nu;
+            kf_y = kf_H * (kf_F * kf_x + eta) + nu;
 
             measurement_function = new Measurement_Model_UKF(kf_H);
-            kf_unscented.update_sequential(kf_y,kf_x_pre,kf_P_x_pre,measurement_function,kf_R);
+            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();
             ukf_P_x_post = kf_unscented.get_P_x_est();
@@ -146,7 +150,7 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
             kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y);
 
             kf_x_post = kf_x_pre + kf_K * (kf_y - kf_H * kf_x_pre);
-            kf_P_x_post = (arma::eye(nx,nx) - kf_K * kf_H) * kf_P_x_pre;
+            kf_P_x_post = (arma::eye(nx, nx) - kf_K * kf_H) * kf_P_x_pre;
 
             EXPECT_TRUE(arma::approx_equal(ukf_x_post, kf_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE));
             EXPECT_TRUE(arma::approx_equal(ukf_P_x_post, kf_P_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE));