From d4d849c83d2d4b8f30c0aa86e5739c59b58c2659 Mon Sep 17 00:00:00 2001
From: Gerald LaMountain <lamountain.g@husky.neu.edu>
Date: Mon, 20 May 2019 12:42:43 -0400
Subject: [PATCH 1/4] Add refs to tracking lib for bayesian estimation

---
 src/algorithms/tracking/libs/bayesian_estimation.cc | 7 ++++++-
 src/algorithms/tracking/libs/bayesian_estimation.h  | 7 ++++++-
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/algorithms/tracking/libs/bayesian_estimation.cc b/src/algorithms/tracking/libs/bayesian_estimation.cc
index 227cd6321..73ab4cd3e 100644
--- a/src/algorithms/tracking/libs/bayesian_estimation.cc
+++ b/src/algorithms/tracking/libs/bayesian_estimation.cc
@@ -6,7 +6,12 @@
  * the properties of a stochastic process based on a sequence of
  * discrete samples of the sequence.
  *
- * [1] TODO: Refs
+ * [1]: LaMountain, Gerald, Vilà-Valls, Jordi, Closas, Pau, "Bayesian 
+ * Covariance Estimation for Kalman Filter based Digital Carrier 
+ * Synchronization," Proceedings of the 31st International Technical Meeting
+ * of the Satellite Division of The Institute of Navigation 
+ * (ION GNSS+ 2018), Miami, Florida, September 2018, pp. 3575-3586.
+ * https://doi.org/10.33012/2018.15911
  *
  * \authors <ul>
  *          <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu
diff --git a/src/algorithms/tracking/libs/bayesian_estimation.h b/src/algorithms/tracking/libs/bayesian_estimation.h
index 02f54143e..001b19fcd 100644
--- a/src/algorithms/tracking/libs/bayesian_estimation.h
+++ b/src/algorithms/tracking/libs/bayesian_estimation.h
@@ -6,7 +6,12 @@
  * the properties of a stochastic process based on a sequence of
  * discrete samples of the sequence.
  *
- * [1] TODO: Refs
+ * [1]: LaMountain, Gerald, Vilà-Valls, Jordi, Closas, Pau, "Bayesian 
+ * Covariance Estimation for Kalman Filter based Digital Carrier 
+ * Synchronization," Proceedings of the 31st International Technical Meeting
+ * of the Satellite Division of The Institute of Navigation 
+ * (ION GNSS+ 2018), Miami, Florida, September 2018, pp. 3575-3586.
+ * https://doi.org/10.33012/2018.15911
  *
  * \authors <ul>
  *          <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu

From 8cc141341b289d20d2fbeb0444c59ff09b50d21f Mon Sep 17 00:00:00 2001
From: Gerald LaMountain <lamountain.g@husky.neu.edu>
Date: Fri, 17 May 2019 12:56:43 -0400
Subject: [PATCH 2/4] Add tracking lib for cubature kalman filter

---
 src/algorithms/tracking/libs/CMakeLists.txt   |   2 +
 .../tracking/libs/cubature_filter.cc          | 182 ++++++++++++++++++
 .../tracking/libs/cubature_filter.h           |  74 +++++++
 3 files changed, 258 insertions(+)
 create mode 100644 src/algorithms/tracking/libs/cubature_filter.cc
 create mode 100644 src/algorithms/tracking/libs/cubature_filter.h

diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt
index 63de5d9a7..5950f8ad1 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..19dcdbcab
--- /dev/null
+++ b/src/algorithms/tracking/libs/cubature_filter.cc
@@ -0,0 +1,182 @@
+/*!
+ * \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] TODO: Refs
+ *
+ * \authors <ul>
+ *          <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
+ *          <li> Jordi Vila-Valls 2019. jvila(at)cttc.es
+ *          </ul>
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2018  (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 <https://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#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::init(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, arma::vec (*transition_fcn)(const arma::mat&), const arma::mat& noise_covariance)
+{
+    // Compute number of cubature points
+    int nx = x_post.n_elem;
+    int np = 2 * 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);
+    
+    // Propagate and evaluate cubature points
+    arma::vec Xi_post;
+    arma::vec Xi_pred;
+    for (int32_t i = 0; i < np; i++)
+    {
+        Xi_post = Sm_post*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + x_post;
+        Xi_pred = (*transition_fcn)(Xi_post);
+        
+        x_pred = x_post + Xi_pred;
+        P_x_pred = P_x_post + 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;
+
+    // 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, arma::vec (*measurement_fcn)(const arma::mat&), 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;
+
+    // Evaluate predicted measurement and covariances
+    arma::mat z_pred = arma::zeros(nx,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);
+
+    // Propagate and evaluate cubature points
+    arma::vec Xi_pred;
+    arma::vec Zi_pred;
+    for (int32_t i = 0; i < np; i++)
+    {
+        Xi_pred = Sm_pred*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + 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();
+
+    // 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..6c3c78968
--- /dev/null
+++ b/src/algorithms/tracking/libs/cubature_filter.h
@@ -0,0 +1,74 @@
+/*!
+ * \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] TODO: Refs
+ *
+ * \authors <ul>
+ *          <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
+ *          <li> Jordi Vila-Valls 2019. jvila(at)cttc.es
+ *          </ul>
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2018  (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 <https://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_CUBATURE_FILTER_H_
+#define GNSS_SDR_CUBATURE_FILTER_H_
+
+#include <armadillo>
+#include <gnuradio/gr_complex.h>
+
+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 init(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, arma::vec (*transition_fcn)(const arma::mat&), const arma::mat& noise_covariance);
+    void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, arma::vec (*measurement_fcn)(const arma::mat&), 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

From 6f5bca8188195cd94f2e3a39cfb84606bea81b66 Mon Sep 17 00:00:00 2001
From: Gerald LaMountain <lamountain.g@husky.neu.edu>
Date: Wed, 12 Jun 2019 14:51:19 -0400
Subject: [PATCH 3/4] Add cubature filter library to tracking and associated
 unit test to tests

---
 .../tracking/libs/cubature_filter.cc          |  45 +++--
 .../tracking/libs/cubature_filter.h           |  31 +++-
 src/tests/CMakeLists.txt                      |   1 +
 src/tests/test_main.cc                        |   1 +
 .../tracking/bayesian_estimation_test.cc      |   6 +-
 .../tracking/cubature_filter_test.cc          | 156 ++++++++++++++++++
 6 files changed, 216 insertions(+), 24 deletions(-)
 create mode 100644 src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc

diff --git a/src/algorithms/tracking/libs/cubature_filter.cc b/src/algorithms/tracking/libs/cubature_filter.cc
index 19dcdbcab..d3d32350e 100644
--- a/src/algorithms/tracking/libs/cubature_filter.cc
+++ b/src/algorithms/tracking/libs/cubature_filter.cc
@@ -6,7 +6,8 @@
  * Filter, which uses multidimensional cubature rules to estimate the
  * time evolution of a nonlinear system.
  *
- * [1] TODO: Refs
+ * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE 
+ * Transactions on Automatic Control, 54(6):1254–1269,2009.
  *
  * \authors <ul>
  *          <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
@@ -14,7 +15,7 @@
  *          </ul>
  * -------------------------------------------------------------------------
  *
- * Copyright (C) 2010-2018  (see AUTHORS file for a list of contributors)
+ * Copyright (C) 2010-2019  (see AUTHORS file for a list of contributors)
  *
  * GNSS-SDR is a software defined Global Navigation
  *          Satellite Systems receiver
@@ -70,7 +71,7 @@ Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x
 
 Cubature_filter::~Cubature_filter() = default;
 
-void Cubature_filter::init(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
+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;
@@ -83,29 +84,33 @@ void Cubature_filter::init(const arma::mat& x_pred_0, const arma::mat& P_x_pred_
 /*
  * 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, arma::vec (*transition_fcn)(const arma::mat&), const arma::mat& noise_covariance)
+void Cubature_filter::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;
     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);
+    arma::mat Sm_post = arma::chol(P_x_post, "lower");
     
     // Propagate and evaluate cubature points
     arma::vec Xi_post;
     arma::vec Xi_pred;
-    for (int32_t i = 0; i < np; i++)
+
+    for (uint8_t i = 0; i < np; i++)
     {
-        Xi_post = Sm_post*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + x_post;
+        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_post + Xi_pred;
-        P_x_pred = P_x_post + Xi_pred*Xi_pred.t();
+       
+        x_pred = x_pred + Xi_pred;
+        P_x_pred = P_x_pred + Xi_pred*Xi_pred.t();
     }
     
     // Estimate predicted state and error covariance
@@ -120,34 +125,37 @@ 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, arma::vec (*measurement_fcn)(const arma::mat&), 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, ModelFunction* 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(nx,1);
+    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);
+    arma::mat Sm_pred = arma::chol(P_x_pred, "lower");
 
     // Propagate and evaluate cubature points
     arma::vec Xi_pred;
     arma::vec Zi_pred;
-    for (int32_t i = 0; i < np; i++)
+    for (uint8_t i = 0; i < np; i++)
     {
-        Xi_pred = Sm_pred*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + x_pred;
+        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();
     }
-    
+
     // 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;
@@ -180,3 +188,8 @@ 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);
+}
diff --git a/src/algorithms/tracking/libs/cubature_filter.h b/src/algorithms/tracking/libs/cubature_filter.h
index 6c3c78968..f4b31c43a 100644
--- a/src/algorithms/tracking/libs/cubature_filter.h
+++ b/src/algorithms/tracking/libs/cubature_filter.h
@@ -6,7 +6,8 @@
  * Filter, which uses multidimensional cubature rules to estimate the
  * time evolution of a nonlinear system.
  *
- * [1] TODO: Refs
+ * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE 
+ * Transactions on Automatic Control, 54(6):1254–1269,2009.
  *
  * \authors <ul>
  *          <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
@@ -14,7 +15,7 @@
  *          </ul>
  * -------------------------------------------------------------------------
  *
- * Copyright (C) 2010-2018  (see AUTHORS file for a list of contributors)
+ * Copyright (C) 2010-2019  (see AUTHORS file for a list of contributors)
  *
  * GNSS-SDR is a software defined Global Navigation
  *          Satellite Systems receiver
@@ -43,6 +44,22 @@
 #include <armadillo>
 #include <gnuradio/gr_complex.h>
 
+// Abstract model function
+class ModelFunction{
+    public:
+        ModelFunction() {};
+        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;
+};
+
 class Cubature_filter
 {
 public:
@@ -53,17 +70,21 @@ public:
     ~Cubature_filter();
 
     // Reinitialization function
-    void init(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
+    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, arma::vec (*transition_fcn)(const arma::mat&), const arma::mat& noise_covariance);
-    void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, arma::vec (*measurement_fcn)(const arma::mat&), 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;
     arma::mat get_P_x_pred() const;
     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;
diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt
index 3053df1d4..51e67077f 100644
--- a/src/tests/CMakeLists.txt
+++ b/src/tests/CMakeLists.txt
@@ -755,6 +755,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
     )
 
     target_link_libraries(trk_test
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..e73bb1d45
--- /dev/null
+++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc
@@ -0,0 +1,156 @@
+/*!
+ * \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 <https://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "cubature_filter.h"
+#include <armadillo>
+#include <gtest/gtest.h>
+#include <random>
+
+#define CUBATURE_TEST_N_TRIALS 1000
+
+class TransitionModel : public ModelFunction {
+    public:
+        TransitionModel(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 {
+    public:
+        MeasurementModel(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;
+
+    ModelFunction* transition_function;
+    ModelFunction* measurement_function;
+
+    //--- Perform initializations ------------------------------
+
+    std::random_device r;
+    std::default_random_engine e1(r());
+    std::normal_distribution<float> normal_dist(0, 5);
+    std::uniform_real_distribution<float> 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<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_cubature.initialize(kf_x_post, kf_P_x_post);
+
+            // Prediction Step
+            kf_F = arma::randu<arma::mat>(nx,nx);
+            kf_Q = arma::diagmat(arma::randu<arma::vec>(nx,1));
+
+            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);
+
+            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<arma::mat>(ny,nx);
+            kf_R = arma::diagmat(arma::randu<arma::vec>(ny,1));
+
+            eta = arma::mvnrnd(arma::zeros<arma::vec>(nx,1),kf_Q);
+            nu = arma::mvnrnd(arma::zeros<arma::vec>(ny,1),kf_R);
+
+            kf_y = kf_H*(kf_F*kf_x + eta) + nu;
+
+            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();
+            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;
+        }
+}

From 83d77fabb0add15de6da7cca2d512909ce9e6ac1 Mon Sep 17 00:00:00 2001
From: Gerald LaMountain <lamountain.g@husky.neu.edu>
Date: Thu, 13 Jun 2019 11:07:10 -0400
Subject: [PATCH 4/4] Update cubature filter library to align with coding
 conventions

---
 .../tracking/libs/cubature_filter.cc          |  9 ++------
 .../tracking/libs/cubature_filter.h           | 21 +++++--------------
 .../tracking/cubature_filter_test.cc          | 16 +++++++-------
 3 files changed, 15 insertions(+), 31 deletions(-)

diff --git a/src/algorithms/tracking/libs/cubature_filter.cc b/src/algorithms/tracking/libs/cubature_filter.cc
index d3d32350e..05b346a7c 100644
--- a/src/algorithms/tracking/libs/cubature_filter.cc
+++ b/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);
-}
diff --git a/src/algorithms/tracking/libs/cubature_filter.h b/src/algorithms/tracking/libs/cubature_filter.h
index f4b31c43a..6a0806e0e 100644
--- a/src/algorithms/tracking/libs/cubature_filter.h
+++ b/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;
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 e73bb1d45..a86d5dd7c 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,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();