Add tracking lib for cubature kalman filter

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

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

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