mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-16 04:05:46 +00:00
Code Issues Releases Wiki Activity

Pass parameters by reference instead of by value (improves effieciency)

Browse Source
This commit is contained in:
Carles Fernandez 2018-08-21 14:57:22 +02:00
parent a833abbb8a
commit 388d1623ed
2 changed files with 52 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
src/algorithms/tracking/libs/bayesian_estimation.cc
View File

@ -42,84 +42,84 @@
Bayesian_estimator::Bayesian_estimator() Bayesian_estimator::Bayesian_estimator()
{ {
int ny = 1; int ny = 1;
mu_prior = arma::zeros(ny,1); mu_prior = arma::zeros(ny, 1);
kappa_prior = 0; kappa_prior = 0;
nu_prior = 0; nu_prior = 0;
Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); Psi_prior = arma::eye(ny, ny) * (nu_prior + ny + 1);
mu_est = mu_prior; mu_est = mu_prior;
Psi_est = Psi_prior; Psi_est = Psi_prior;
} }
Bayesian_estimator::Bayesian_estimator(int ny) Bayesian_estimator::Bayesian_estimator(int ny)
{ {
mu_prior = arma::zeros(ny,1); mu_prior = arma::zeros(ny, 1);
kappa_prior = 0; kappa_prior = 0;
nu_prior = 0; nu_prior = 0;
Psi_prior = arma::eye(ny,ny) * (nu_prior + ny + 1); Psi_prior = arma::eye(ny, ny) * (nu_prior + ny + 1);
mu_est = mu_prior; mu_est = mu_prior;
Psi_est = Psi_prior; Psi_est = Psi_prior;
} }
Bayesian_estimator::Bayesian_estimator(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) Bayesian_estimator::Bayesian_estimator(const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0)
{ {
mu_prior = mu_prior_0; mu_prior = mu_prior_0;
kappa_prior = kappa_prior_0; kappa_prior = kappa_prior_0;
nu_prior = nu_prior_0; nu_prior = nu_prior_0;
Psi_prior = Psi_prior_0; Psi_prior = Psi_prior_0;
mu_est = mu_prior; mu_est = mu_prior;
Psi_est = Psi_prior; Psi_est = Psi_prior;
} }
Bayesian_estimator::~Bayesian_estimator() Bayesian_estimator::~Bayesian_estimator()
{ {
} }
void Bayesian_estimator::init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0) void Bayesian_estimator::init(const arma::mat& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0)
{ {
mu_prior = mu_prior_0; mu_prior = mu_prior_0;
kappa_prior = kappa_prior_0; kappa_prior = kappa_prior_0;
nu_prior = nu_prior_0; nu_prior = nu_prior_0;
Psi_prior = Psi_prior_0; Psi_prior = Psi_prior_0;
mu_est = mu_prior; mu_est = mu_prior;
Psi_est = Psi_prior; Psi_est = Psi_prior;
} }
/* /*
* Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in * Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in
* the class structure, and update the priors according to the computed posteriors * the class structure, and update the priors according to the computed posteriors
*/ */
void Bayesian_estimator::update_sequential(arma::vec data) void Bayesian_estimator::update_sequential(const arma::vec& data)
{ {
int K = data.n_cols; int K = data.n_cols;
int ny = data.n_rows; int ny = data.n_rows;
if (mu_prior.is_empty()) if (mu_prior.is_empty())
{ {
mu_prior = arma::zeros(ny,1); mu_prior = arma::zeros(ny, 1);
} }
if (Psi_prior.is_empty()) if (Psi_prior.is_empty())
{ {
Psi_prior = arma::zeros(ny,ny); Psi_prior = arma::zeros(ny, ny);
} }
arma::vec y_mean = arma::mean(data, 1); arma::vec y_mean = arma::mean(data, 1);
arma::mat Psi_N = arma::zeros(ny, ny); arma::mat Psi_N = arma::zeros(ny, ny);
for (int kk = 0; kk < K; kk++) for (int kk = 0; kk < K; kk++)
{ {
Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t()); 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); arma::vec mu_posterior = (kappa_prior * mu_prior + K * y_mean) / (kappa_prior + K);
int kappa_posterior = kappa_prior + K; int kappa_posterior = kappa_prior + K;
int nu_posterior = nu_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()); 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; mu_est = mu_posterior;
if ((nu_posterior - ny - 1) > 0) if ((nu_posterior - ny - 1) > 0)
@ -131,10 +131,10 @@ void Bayesian_estimator::update_sequential(arma::vec data)
Psi_est = Psi_posterior / (nu_posterior + ny + 1); Psi_est = Psi_posterior / (nu_posterior + ny + 1);
} }
mu_prior = mu_posterior; mu_prior = mu_posterior;
kappa_prior = kappa_posterior; kappa_prior = kappa_posterior;
nu_prior = nu_posterior; nu_prior = nu_posterior;
Psi_prior = Psi_posterior; Psi_prior = Psi_posterior;
} }
@ -142,10 +142,9 @@ void Bayesian_estimator::update_sequential(arma::vec data)
* Perform Bayesian noise estimation using a new set of normal-inverse-Wishart priors * Perform Bayesian noise estimation using a new set of normal-inverse-Wishart priors
* and update the priors according to the computed posteriors * 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) void Bayesian_estimator::update_sequential(const arma::vec& data, const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0)
{ {
int K = data.n_cols;
int K = data.n_cols;
int ny = data.n_rows; int ny = data.n_rows;
arma::vec y_mean = arma::mean(data, 1); arma::vec y_mean = arma::mean(data, 1);
@ -153,13 +152,13 @@ void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0,
for (int kk = 0; kk < K; kk++) for (int kk = 0; kk < K; kk++)
{ {
Psi_N = Psi_N + (data.col(kk)-y_mean)*((data.col(kk)-y_mean).t()); 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); 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 kappa_posterior = kappa_prior_0 + K;
int nu_posterior = nu_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()); 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; mu_est = mu_posterior;
if ((nu_posterior - ny - 1) > 0) if ((nu_posterior - ny - 1) > 0)
@ -171,10 +170,10 @@ void Bayesian_estimator::update_sequential(arma::vec data, arma::vec mu_prior_0,
Psi_est = Psi_posterior / (nu_posterior + ny + 1); Psi_est = Psi_posterior / (nu_posterior + ny + 1);
} }
mu_prior = mu_posterior; mu_prior = mu_posterior;
kappa_prior = kappa_posterior; kappa_prior = kappa_posterior;
nu_prior = nu_posterior; nu_prior = nu_posterior;
Psi_prior = Psi_posterior; Psi_prior = Psi_posterior;
} }
arma::mat Bayesian_estimator::get_mu_est() arma::mat Bayesian_estimator::get_mu_est()
@ -186,4 +185,3 @@ arma::mat Bayesian_estimator::get_Psi_est()
{ {
return Psi_est; return Psi_est;
} }

14
src/algorithms/tracking/libs/bayesian_estimation.h
View File

@ -53,22 +53,21 @@
* \mathbf{\mu}_{0}, \kappa_{0}, \nu_{0}, and \mathbf{\Psi}. * \mathbf{\mu}_{0}, \kappa_{0}, \nu_{0}, and \mathbf{\Psi}.
* *
* [1] TODO: Ref1 * [1] TODO: Ref1
* *
*/ */
class Bayesian_estimator class Bayesian_estimator
{ {
public: public:
Bayesian_estimator(); Bayesian_estimator();
Bayesian_estimator(int ny); 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(const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0);
~Bayesian_estimator(); ~Bayesian_estimator();
void init(arma::vec mu_prior_0, int kappa_prior_0, int nu_prior_0, arma::mat Psi_prior_0); void init(const arma::mat& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0);
void update_sequential(arma::vec data); void update_sequential(const 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); void update_sequential(const arma::vec& data, const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0);
arma::mat get_mu_est(); arma::mat get_mu_est();
arma::mat get_Psi_est(); arma::mat get_Psi_est();
@ -76,12 +75,11 @@ public:
private: private:
arma::vec mu_est; arma::vec mu_est;
arma::mat Psi_est; arma::mat Psi_est;
arma::vec mu_prior; arma::vec mu_prior;
int kappa_prior; int kappa_prior;
int nu_prior; int nu_prior;
arma::mat Psi_prior; arma::mat Psi_prior;
}; };
#endif #endif