Skip to content
Permalink
Browse files

Add bayesian_estimation library to tracking/libs which will be used t…

…o perform noise adaptation to Kalman filter based tracking
  • Loading branch information
glamountain committed Jul 9, 2018
1 parent e454dc7 commit 486ac195db380a6c23b6d7f3770674d53bbbb9d3
@@ -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 @@ class Gps_L1_Ca_Kf_Tracking_cc : public gr::block
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;
@@ -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)
@@ -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;
}

@@ -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

0 comments on commit 486ac19

Please sign in to comment.
You can’t perform that action at this time.