diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt
index 3f772f149..fb816d0d8 100644
--- a/src/algorithms/tracking/libs/CMakeLists.txt
+++ b/src/algorithms/tracking/libs/CMakeLists.txt
@@ -33,6 +33,7 @@ set(TRACKING_LIB_SOURCES
cpu_multicorrelator.cc
cpu_multicorrelator_real_codes.cc
cpu_multicorrelator_16sc.cc
+ cubature_filter.cc
lock_detectors.cc
tcp_communication.cc
tcp_packet_data.cc
@@ -50,6 +51,7 @@ set(TRACKING_LIB_HEADERS
cpu_multicorrelator.h
cpu_multicorrelator_real_codes.h
cpu_multicorrelator_16sc.h
+ cubature_filter.h
lock_detectors.h
tcp_communication.h
tcp_packet_data.h
diff --git a/src/algorithms/tracking/libs/cubature_filter.cc b/src/algorithms/tracking/libs/cubature_filter.cc
new file mode 100644
index 000000000..cd7e3bb83
--- /dev/null
+++ b/src/algorithms/tracking/libs/cubature_filter.cc
@@ -0,0 +1,199 @@
+/*!
+ * \file cubature_filter.cc
+ * \brief Interface of a library with Bayesian noise statistic estimation
+ *
+ * Cubature_Filter implements the functionality of the Cubature Kalman
+ * 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
+ * Transactions on Automatic Control, 54(6):1254–1269,2009.
+ *
+ * \authors
+ * - Gerald LaMountain, 2019. gerald(at)ece.neu.edu
+ *
- Jordi Vila-Valls 2019. jvila(at)cttc.es
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ * Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see .
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "cubature_filter.h"
+
+
+Cubature_filter::Cubature_filter()
+{
+ int nx = 1;
+ x_pred_out = arma::zeros(nx, 1);
+ P_x_pred_out = arma::eye(nx, nx) * (nx + 1);
+
+ x_est = x_pred_out;
+ P_x_est = P_x_pred_out;
+}
+
+
+Cubature_filter::Cubature_filter(int nx)
+{
+ x_pred_out = arma::zeros(nx, 1);
+ P_x_pred_out = arma::eye(nx, nx) * (nx + 1);
+
+ x_est = x_pred_out;
+ 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;
+ P_x_pred_out = P_x_pred_0;
+
+ x_est = x_pred_out;
+ 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;
+ P_x_pred_out = P_x_pred_0;
+
+ x_est = x_pred_out;
+ P_x_est = P_x_pred_out;
+}
+
+
+/*
+ * 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)
+{
+ // Compute number of cubature points
+ int nx = x_post.n_elem;
+ int np = 2 * nx;
+
+ // Generator Matrix
+ 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);
+
+ // 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(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 / 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
+ */
+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;
+ int nz = z_upd.n_elem;
+ int np = 2 * nx;
+
+ // Generator Matrix
+ 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);
+
+ // Factorize predicted covariance
+ arma::mat Sm_pred = arma::chol(P_x_pred, "lower");
+
+ // Propagate and evaluate cubature points
+ arma::vec Xi_pred;
+ arma::vec Zi_pred;
+ for (uint8_t i = 0; i < np; i++)
+ {
+ 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 / 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);
+
+ // 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();
+}
+
+
+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
new file mode 100644
index 000000000..87f45bcdd
--- /dev/null
+++ b/src/algorithms/tracking/libs/cubature_filter.h
@@ -0,0 +1,84 @@
+/*!
+ * \file cubature_filter.h
+ * \brief Interface of a library with Bayesian noise statistic estimation
+ *
+ * Cubature_Filter implements the functionality of the Cubature Kalman
+ * 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
+ * Transactions on Automatic Control, 54(6):1254–1269,2009.
+ *
+ * \authors
+ * - Gerald LaMountain, 2019. gerald(at)ece.neu.edu
+ *
- Jordi Vila-Valls 2019. jvila(at)cttc.es
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ * Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see .
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_CUBATURE_FILTER_H_
+#define GNSS_SDR_CUBATURE_FILTER_H_
+
+#include
+#include
+
+// Abstract model function
+class Model_Function{
+ public:
+ Model_Function() {};
+ virtual arma::vec operator() (arma::vec input) = 0;
+ virtual ~Model_Function() = default;
+};
+
+class Cubature_filter
+{
+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();
+
+ // 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);
+
+ // Getters
+ arma::mat get_x_pred() const;
+ arma::mat get_P_x_pred() const;
+ arma::mat get_x_est() const;
+ arma::mat get_P_x_est() const;
+
+private:
+ arma::vec x_pred_out;
+ arma::mat P_x_pred_out;
+ arma::vec x_est;
+ arma::mat P_x_est;
+};
+
+#endif
diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt
index 266d33b9d..8eb57397e 100644
--- a/src/tests/CMakeLists.txt
+++ b/src/tests/CMakeLists.txt
@@ -799,6 +799,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA)
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc
+ ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc
)
if(${FILESYSTEM_FOUND})
target_compile_definitions(trk_test PRIVATE -DHAS_STD_FILESYSTEM=1)
diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc
index 05df98d35..d22149fee 100644
--- a/src/tests/test_main.cc
+++ b/src/tests/test_main.cc
@@ -99,6 +99,7 @@ DECLARE_string(log_dir);
#endif
#include "unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc"
+#include "unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc"
diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc
index dd7d0398b..6fa5f1da0 100644
--- a/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc
+++ b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc
@@ -1,7 +1,7 @@
/*!
- * \file bayesian_estimation_positivity_test.cc
- * \brief This file implements timing tests for the Bayesian covariance estimator
- * \author Gerald LaMountain, 20168. gerald(at)ece.neu.edu
+ * \file bayesian_estimation_test.cc
+ * \brief This file implements feasability test for the BCE library.
+ * \author Gerald LaMountain, 2018. gerald(at)ece.neu.edu
*
*
* -------------------------------------------------------------------------
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
new file mode 100644
index 000000000..948e5cc69
--- /dev/null
+++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc
@@ -0,0 +1,160 @@
+/*!
+ * \file cubature_filter_test.cc
+ * \brief This file implements numerical accuracy test for the CKF library.
+ * \author Gerald LaMountain, 2019. gerald(at)ece.neu.edu
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ * Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see .
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "cubature_filter.h"
+#include
+#include
+#include
+
+#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 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)
+{
+ Cubature_filter kf_cubature;
+
+ arma::vec kf_x;
+ arma::mat kf_P_x;
+
+ arma::vec kf_x_pre;
+ arma::mat kf_P_x_pre;
+
+ arma::vec ckf_x_pre;
+ arma::mat ckf_P_x_pre;
+
+ arma::vec kf_x_post;
+ arma::mat kf_P_x_post;
+
+ arma::vec ckf_x_post;
+ arma::mat ckf_P_x_post;
+
+ arma::mat kf_F;
+ arma::mat kf_H;
+
+ arma::mat kf_Q;
+ arma::mat kf_R;
+
+ arma::vec eta;
+ arma::vec nu;
+
+ arma::vec kf_y;
+ arma::mat kf_P_y;
+ arma::mat kf_K;
+
+ Model_Function* transition_function;
+ Model_Function* measurement_function;
+
+ //--- Perform initializations ------------------------------
+
+ std::random_device r;
+ std::default_random_engine e1(r());
+ std::normal_distribution normal_dist(0, 5);
+ std::uniform_real_distribution uniform_dist(0.1, 5.0);
+
+ uint8_t nx = 0;
+ uint8_t ny = 0;
+
+ for (uint16_t k = 0; k < CUBATURE_TEST_N_TRIALS; k++)
+ {
+ nx = std::rand() % 5 + 1;
+ ny = std::rand() % 5 + 1;
+
+ kf_x = arma::randn(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));
+
+ 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);
+
+ ckf_x_pre = kf_cubature.get_x_pred();
+ ckf_P_x_pre = kf_cubature.get_P_x_pred();
+
+ kf_x_pre = kf_F * kf_x_post;
+ kf_P_x_pre = kf_F * kf_P_x_post * kf_F.t() + kf_Q;
+
+ EXPECT_TRUE(arma::approx_equal(ckf_x_pre, kf_x_pre, "absdiff", 0.01));
+ 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));
+
+ 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;
+
+ 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();
+ ckf_P_x_post = kf_cubature.get_P_x_est();
+
+ kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R;
+ 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;
+
+ 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));
+
+ delete transition_function;
+ delete measurement_function;
+ }
+}