mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 07:13:03 +00:00
Code Issues Releases Wiki Activity

Pass vectors and matrices by reference, rename classes to CamelCase style

Browse Source
This commit is contained in:
Carles Fernandez
2019-06-16 14:54:41 +02:00
parent d6887488f3
commit 16f7c4b889
4 changed files with 65 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
src/algorithms/tracking/libs/nonlinear_tracking.cc
View File

@@ -4,7 +4,7 @@
*
* Cubature_Filter implements the functionality of the Cubature Kalman
* Filter, which uses multidimensional cubature rules to estimate the
* time evolution of a nonlinear system. Unscented_filter implements
* time evolution of a nonlinear system. UnscentedFilter implements
* an Unscented Kalman Filter which uses Unscented Transform rules to
* perform a similar estimation.
*
@@ -44,7 +44,7 @@
/***************** CUBATURE KALMAN FILTER *****************/
Cubature_filter::Cubature_filter()
CubatureFilter::CubatureFilter()
{
int nx = 1;
x_pred_out = arma::zeros(nx, 1);
@@ -55,7 +55,7 @@ Cubature_filter::Cubature_filter()
}
Cubature_filter::Cubature_filter(int nx)
CubatureFilter::CubatureFilter(int nx)
{
x_pred_out = arma::zeros(nx, 1);
P_x_pred_out = arma::eye(nx, nx) * (nx + 1);
@@ -65,7 +65,7 @@ Cubature_filter::Cubature_filter(int nx)
}
Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
CubatureFilter::CubatureFilter(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;
@@ -75,10 +75,10 @@ Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x
}
Cubature_filter::~Cubature_filter() = default;
CubatureFilter::~CubatureFilter() = default;
void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
void CubatureFilter::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;
@@ -91,7 +91,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, Model_Function* transition_fcn, const arma::mat& noise_covariance)
void CubatureFilter::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;
@@ -133,7 +133,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, Model_Function* measurement_fcn, const arma::mat& noise_covariance)
void CubatureFilter::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;
@@ -178,25 +178,25 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
}
arma::mat Cubature_filter::get_x_pred() const
arma::mat CubatureFilter::get_x_pred() const
{
return x_pred_out;
}
arma::mat Cubature_filter::get_P_x_pred() const
arma::mat CubatureFilter::get_P_x_pred() const
{
return P_x_pred_out;
}
arma::mat Cubature_filter::get_x_est() const
arma::mat CubatureFilter::get_x_est() const
{
return x_est;
}
arma::mat Cubature_filter::get_P_x_est() const
arma::mat CubatureFilter::get_P_x_est() const
{
return P_x_est;
}
@@ -205,7 +205,7 @@ arma::mat Cubature_filter::get_P_x_est() const
/***************** UNSCENTED KALMAN FILTER *****************/
Unscented_filter::Unscented_filter()
UnscentedFilter::UnscentedFilter()
{
int nx = 1;
x_pred_out = arma::zeros(nx, 1);
@@ -216,7 +216,7 @@ Unscented_filter::Unscented_filter()
}
Unscented_filter::Unscented_filter(int nx)
UnscentedFilter::UnscentedFilter(int nx)
{
x_pred_out = arma::zeros(nx, 1);
P_x_pred_out = arma::eye(nx, nx) * (nx + 1);
@@ -226,7 +226,7 @@ Unscented_filter::Unscented_filter(int nx)
}
Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
UnscentedFilter::UnscentedFilter(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;
@@ -236,10 +236,10 @@ Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P
}
Unscented_filter::~Unscented_filter() = default;
UnscentedFilter::~UnscentedFilter() = default;
void Unscented_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
void UnscentedFilter::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;
@@ -252,7 +252,7 @@ void Unscented_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_
/*
* Perform the prediction step of the Unscented Kalman filter
*/
void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance)
void UnscentedFilter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, const arma::mat& noise_covariance)
{
// Compute number of sigma points
int nx = x_post.n_elem;
@@ -307,7 +307,7 @@ void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::m
/*
* Perform the update step of the Unscented Kalman filter
*/
void Unscented_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)
void UnscentedFilter::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 sigma points
int nx = x_pred.n_elem;
@@ -364,25 +364,25 @@ void Unscented_filter::update_sequential(const arma::vec& z_upd, const arma::vec
}
arma::mat Unscented_filter::get_x_pred() const
arma::mat UnscentedFilter::get_x_pred() const
{
return x_pred_out;
}
arma::mat Unscented_filter::get_P_x_pred() const
arma::mat UnscentedFilter::get_P_x_pred() const
{
return P_x_pred_out;
}
arma::mat Unscented_filter::get_x_est() const
arma::mat UnscentedFilter::get_x_est() const
{
return x_est;
}
arma::mat Unscented_filter::get_P_x_est() const
arma::mat UnscentedFilter::get_P_x_est() const
{
return P_x_est;
}

40
src/algorithms/tracking/libs/nonlinear_tracking.h
View File

@@ -2,9 +2,9 @@
* \file nonlinear_tracking.h
* \brief Interface of a library for nonlinear tracking algorithms
*
* Cubature_Filter implements the functionality of the Cubature Kalman
* CubatureFilter implements the functionality of the Cubature Kalman
* Filter, which uses multidimensional cubature rules to estimate the
* time evolution of a nonlinear system. Unscented_filter implements
* time evolution of a nonlinear system. UnscentedFilter implements
* an Unscented Kalman Filter which uses Unscented Transform rules to
* perform a similar estimation.
*
@@ -47,29 +47,29 @@
#include <gnuradio/gr_complex.h>
// Abstract model function
class Model_Function
class ModelFunction
{
public:
Model_Function(){};
virtual arma::vec operator()(arma::vec input) = 0;
virtual ~Model_Function() = default;
ModelFunction(){};
virtual arma::vec operator()(const arma::vec& input) = 0;
virtual ~ModelFunction() = default;
};
class Cubature_filter
class CubatureFilter
{
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();
CubatureFilter();
CubatureFilter(int nx);
CubatureFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~CubatureFilter();
// Reinitialization function
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, 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);
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;
@@ -84,21 +84,21 @@ private:
arma::mat P_x_est;
};
class Unscented_filter
class UnscentedFilter
{
public:
// Constructors and destructors
Unscented_filter();
Unscented_filter(int nx);
Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~Unscented_filter();
UnscentedFilter();
UnscentedFilter(int nx);
UnscentedFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~UnscentedFilter();
// Reinitialization function
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, 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);
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;