Add bayesian_estimation library to tracking/libs which will be used to perform noise adaptation to Kalman filter based tracking

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h

@@ -49,6 +49,7 @@
 #include "tracking_2nd_PLL_filter.h"
 #include <armadillo>
 #include "cpu_multicorrelator_real_codes.h"
+#include "bayesian_estimation.h"
 
 class Gps_L1_Ca_Kf_Tracking_cc;
 
@@ -133,6 +134,9 @@ private:
     arma::colvec kf_y_pre;  //measurement vector
     arma::mat kf_K;         //Kalman gain matrix
 
+    // Bayesian estimator
+    Bayesian_estimator cov_est;
+
 
     // PLL and DLL filter library
     Tracking_2nd_DLL_filter d_code_loop_filter;

src/algorithms/tracking/libs/CMakeLists.txt

@@ -44,6 +44,7 @@ set(TRACKING_LIB_SOURCES
      tracking_FLL_PLL_filter.cc
      tracking_loop_filter.cc
      dll_pll_conf.cc
+     bayesian_estimation.cc
 )
 
 if(ENABLE_FPGA)

src/algorithms/tracking/libs/bayesian_estimation.cc

@@ -0,0 +1,167 @@
+/*!
+ * \file bayesian_estimation.cc
+ * \brief Interface of a library with Bayesian noise statistic estimation
+ *
+ * Bayesian_estimator is a Bayesian estimator which attempts to estimate
+ * the properties of a stochastic process based on a sequence of
+ * discrete samples of the sequence.
+ *
+ * [1] TODO: Refs
+ *
+ * \authors <ul>
+ *          <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu
+ *          <li> Jordi Vila-Valls 2018. 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 "bayesian_estimation.h"
+#include <armadillo>
+
+Bayesian_estimator::Bayesian_estimator()
+{
+    kappa_prior = 0;
+    nu_prior    = 0;
+}
+
+Bayesian_estimator::Bayesian_estimator(int ny)
+{
+    mu_prior    = arma::zeros(ny,1);
+    kappa_prior = 0;
+    nu_prior    = 0;
+    Psi_prior   = arma::eye(ny,ny) * (nu_prior + ny + 1);
+}
+
+Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0)
+{
+    mu_prior    = mu_prior_0;
+    kappa_prior = kappa_prior_0;
+    nu_prior    = nu_prior_0;
+    Psi_prior   = Psi_prior_0;
+}
+
+Bayesian_estimator::~Bayesian_estimator()
+{
+}
+
+/*
+ * Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in
+ * the class structure, and update the priors according to the computed posteriors
+ */
+void Bayesian_estimator::update_sequential(arma::vec data)
+{
+    int K  = data.n_cols;
+    int ny = data.n_rows;
+
+    if (mu_prior.is_empty())
+        {
+            mu_prior = arma::zeros(ny,1);
+        }
+
+    if (Psi_prior.is_empty())
+        {
+            Psi_prior = arma::zeros(ny,ny);
+        }
+
+    arma::vec y_mean = arma::mean(data, 1);
+    arma::mat Psi_N  = arma::zeros(ny, ny);
+
+    for (int kk = 0; kk < K; kk++)
+        {
+            Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t());
+        }
+
+    arma::vec mu_posterior  = (kappa_prior*mu_prior + K*y_mean) / (kappa_prior + K);
+    int kappa_posterior     = kappa_prior + K;
+    int nu_posterior        = nu_prior + K;
+    arma::mat Psi_posterior = Psi_prior + Psi_N + (kappa_prior*K)/(kappa_prior + K)*(y_mean - mu_prior)*((y_mean - mu_prior).t());
+
+    mu_est = mu_posterior;
+    if ((nu_posterior - ny - 1) > 0)
+        {
+            Psi_est = Psi_posterior / (nu_posterior - ny - 1);
+        }
+    else
+        {
+            Psi_est = Psi_posterior / (nu_posterior + ny + 1);
+        }
+
+    mu_prior    = mu_posterior;
+    kappa_prior = kappa_posterior;
+    nu_prior    = nu_posterior;
+    Psi_prior   = Psi_posterior;
+}
+
+
+/*
+ * Perform Bayesian noise estimation using a new set of normal-inverse-Wishart priors
+ * and update the priors according to the computed posteriors
+ */
+void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0)
+{
+
+    int K  = data.n_cols;
+    int ny = data.n_rows;
+
+    arma::vec y_mean = arma::mean(data, 1);
+    arma::mat Psi_N = arma::zeros(ny, ny);
+
+    for (int kk = 0; kk < K; kk++)
+        {
+            Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t());
+        }
+
+    arma::vec mu_posterior  = (kappa_prior_0*mu_prior_0 + K*y_mean) / (kappa_prior_0 + K);
+    int kappa_posterior     = kappa_prior_0 + K;
+    int nu_posterior        = nu_prior_0 + K;
+    arma::mat Psi_posterior = Psi_prior_0 + Psi_N + (kappa_prior_0*K)/(kappa_prior_0 + K)*(y_mean - mu_prior_0)*((y_mean - mu_prior_0).t());
+
+    mu_est = mu_posterior;
+    if ((nu_posterior - ny - 1) > 0)
+        {
+            Psi_est = Psi_posterior / (nu_posterior - ny - 1);
+        }
+    else
+        {
+            Psi_est = Psi_posterior / (nu_posterior + ny + 1);
+        }
+
+    mu_prior    = mu_posterior;
+    kappa_prior = kappa_posterior;
+    nu_prior    = nu_posterior;
+    Psi_prior   = Psi_posterior;
+
+}
+
+arma::vec Bayesian_estimator::get_mu_est()
+{
+    return mu_est;
+}
+
+arma::mat Bayesian_estimator::get_Psi_est()
+{
+    return Psi_est;
+}
+

src/algorithms/tracking/libs/bayesian_estimation.h

@@ -0,0 +1,86 @@
+/*!
+ * \file bayesian_estimation.h
+ * \brief Interface of a library with Bayesian noise statistic estimation
+ *
+ * Bayesian_estimator is a Bayesian estimator which attempts to estimate
+ * the properties of a stochastic process based on a sequence of
+ * discrete samples of the sequence.
+ *
+ * [1] TODO: Refs
+ *
+ * \authors <ul>
+ *          <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu
+ *          <li> Jordi Vila-Valls 2018. 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_BAYESIAN_ESTIMATION_H_
+#define GNSS_SDR_BAYESIAN_ESTIMATION_H_
+
+#include <gnuradio/gr_complex.h>
+#include <armadillo>
+
+/*! \brief Bayesian_estimator is an estimator of noise characteristics (i.e. mean, covariance)
+ *
+ * Bayesian_estimator is an estimator which performs estimation of noise characteristics from
+ * a sequence of identically and independently distributed (IID) samples of a stationary
+ * stochastic process by way of Bayesian inference using conjugate priors. The posterior
+ * distribution is assumed to be Gaussian with mean \mathbf{\mu} and covariance \hat{\mathbf{C}},
+ * which has a conjugate prior given by a normal-inverse-Wishart distribution with paramemters
+ * \mathbf{\mu}_{0}, \kappa_{0}, \nu_{0}, and \mathbf{\Psi}.
+ *
+ * [1] TODO: Ref1
+ * 
+ */
+
+class Bayesian_estimator
+{
+
+public:
+    Bayesian_estimator();
+    Bayesian_estimator(int ny);
+    Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0);
+    ~Bayesian_estimator();
+
+    void update_sequential(arma::vec data);
+    void update_sequential(arma::vec data, arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0);
+
+    arma::vec get_mu_est();
+    arma::mat get_Psi_est();
+
+private:
+
+    arma::vec mu_est;
+    arma::mat Psi_est;
+    
+    arma::vec mu_prior;
+    int kappa_prior;
+    int nu_prior;
+    arma::mat Psi_prior;
+    
+};
+
+#endif