diff --git a/src/algorithms/tracking/libs/cubature_filter.cc b/src/algorithms/tracking/libs/cubature_filter.cc
index 05b346a7c..cd7e3bb83 100644
--- a/src/algorithms/tracking/libs/cubature_filter.cc
+++ b/src/algorithms/tracking/libs/cubature_filter.cc
@@ -6,7 +6,7 @@
* Filter, which uses multidimensional cubature rules to estimate the
* time evolution of a nonlinear system.
*
- * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
+ * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
* Transactions on Automatic Control, 54(6):1254–1269,2009.
*
* \authors
@@ -51,6 +51,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);
@@ -60,6 +61,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;
@@ -69,8 +71,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;
@@ -91,37 +95,38 @@ 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::mat P_x_pred = arma::zeros(nx,nx);
+ arma::vec x_pred = arma::zeros(nx, 1);
+ arma::mat P_x_pred = arma::zeros(nx, nx);
// Factorize posterior covariance
arma::mat Sm_post = arma::chol(P_x_post, "lower");
-
+
// Propagate and evaluate cubature points
arma::vec Xi_post;
arma::vec Xi_pred;
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_pred = (*transition_fcn)(Xi_post);
-
- x_pred = x_pred + Xi_pred;
- P_x_pred = P_x_pred + Xi_pred*Xi_pred.t();
- }
-
+ {
+ Xi_post = Sm_post * (std::sqrt(static_cast(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();
+ }
+
// Estimate predicted state and error covariance
- x_pred = x_pred / ((float) np);
- P_x_pred = P_x_pred / ((float) np) - x_pred*x_pred.t() + noise_covariance;
+ x_pred = x_pred / static_cast(np);
+ P_x_pred = P_x_pred / static_cast(np) - x_pred * x_pred.t() + noise_covariance;
// Store predicted state and error covariance
x_pred_out = x_pred;
P_x_pred_out = P_x_pred;
}
+
/*
* Perform the update step of the cubature Kalman filter
*/
@@ -133,12 +138,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));
// Evaluate predicted measurement and covariances
- arma::mat z_pred = arma::zeros(nz,1);
- arma::mat P_zz_pred = arma::zeros(nz,nz);
- arma::mat P_xz_pred = arma::zeros(nx,nz);
+ arma::mat z_pred = arma::zeros(nz, 1);
+ arma::mat P_zz_pred = arma::zeros(nz, nz);
+ arma::mat P_xz_pred = arma::zeros(nx, nz);
// Factorize predicted covariance
arma::mat Sm_pred = arma::chol(P_x_pred, "lower");
@@ -147,43 +152,47 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
arma::vec Xi_pred;
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;
- Zi_pred = (*measurement_fcn)(Xi_pred);
-
- z_pred = z_pred + Zi_pred;
- P_zz_pred = P_zz_pred + Zi_pred*Zi_pred.t();
- P_xz_pred = P_xz_pred + Xi_pred*Zi_pred.t();
- }
+ {
+ Xi_pred = Sm_pred * (std::sqrt(static_cast(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_xz_pred = P_xz_pred + Xi_pred * Zi_pred.t();
+ }
// Estimate measurement covariance and cross covariances
- z_pred = z_pred / ((float) np);
- P_zz_pred = P_zz_pred / ((float) np) - z_pred*z_pred.t() + noise_covariance;
- P_xz_pred = P_xz_pred / ((float) np) - x_pred*z_pred.t();
+ z_pred = z_pred / static_cast(np);
+ P_zz_pred = P_zz_pred / static_cast(np) - z_pred * z_pred.t() + noise_covariance;
+ P_xz_pred = P_xz_pred / static_cast(np) - x_pred * z_pred.t();
// 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 state and error covariance
- x_est = x_pred + W_k*(z_upd - z_pred);
- P_x_est = P_x_pred - W_k*P_zz_pred*W_k.t();
+ x_est = x_pred + W_k * (z_upd - z_pred);
+ 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;
diff --git a/src/algorithms/tracking/libs/cubature_filter.h b/src/algorithms/tracking/libs/cubature_filter.h
index 6a0806e0e..87f45bcdd 100644
--- a/src/algorithms/tracking/libs/cubature_filter.h
+++ b/src/algorithms/tracking/libs/cubature_filter.h
@@ -6,7 +6,7 @@
* Filter, which uses multidimensional cubature rules to estimate the
* time evolution of a nonlinear system.
*
- * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
+ * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
* Transactions on Automatic Control, 54(6):1254–1269,2009.
*
* \authors
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 a86d5dd7c..948e5cc69 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,20 +35,24 @@
#define CUBATURE_TEST_N_TRIALS 1000
-class Transition_Model : public Model_Function {
- public:
- 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 Transition_Model : public Model_Function
+{
+public:
+ 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 Measurement_Model : public Model_Function {
- public:
- 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;
+class Measurement_Model : public Model_Function
+{
+public:
+ 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;
};
TEST(CubatureFilterComputationTest, CubatureFilterTest)
@@ -101,21 +105,21 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
nx = std::rand() % 5 + 1;
ny = std::rand() % 5 + 1;
- kf_x = arma::randn(nx,1);
+ kf_x = arma::randn(nx, 1);
- kf_P_x_post = 5.0 * arma::diagmat(arma::randu(nx,1));
+ kf_P_x_post = 5.0 * arma::diagmat(arma::randu(nx, 1));
kf_x_post = arma::mvnrnd(kf_x, kf_P_x_post);
kf_cubature.initialize(kf_x_post, kf_P_x_post);
// Prediction Step
- kf_F = arma::randu(nx,nx);
- kf_Q = arma::diagmat(arma::randu(nx,1));
+ kf_F = arma::randu(nx, nx);
+ kf_Q = arma::diagmat(arma::randu(nx, 1));
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);
+ kf_cubature.predict_sequential(kf_x_post, kf_P_x_post, transition_function, kf_Q);
ckf_x_pre = kf_cubature.get_x_pred();
ckf_P_x_pre = kf_cubature.get_P_x_pred();
@@ -127,16 +131,16 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
EXPECT_TRUE(arma::approx_equal(ckf_P_x_pre, kf_P_x_pre, "absdiff", 0.01));
// Update Step
- kf_H = arma::randu(ny,nx);
- kf_R = arma::diagmat(arma::randu(ny,1));
+ kf_H = arma::randu(ny, nx);
+ kf_R = arma::diagmat(arma::randu(ny, 1));
- eta = arma::mvnrnd(arma::zeros(nx,1),kf_Q);
- nu = arma::mvnrnd(arma::zeros(ny,1),kf_R);
+ eta = arma::mvnrnd(arma::zeros(nx, 1), kf_Q);
+ nu = arma::mvnrnd(arma::zeros(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(kf_H);
- kf_cubature.update_sequential(kf_y,kf_x_pre,kf_P_x_pre,measurement_function,kf_R);
+ 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();
ckf_P_x_post = kf_cubature.get_P_x_est();
@@ -145,7 +149,7 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
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(ckf_x_post, kf_x_post, "absdiff", 0.01));
EXPECT_TRUE(arma::approx_equal(ckf_P_x_post, kf_P_x_post, "absdiff", 0.01));