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

Update cubature filter library to align with coding conventions

Browse Source
This commit is contained in:
Gerald LaMountain 2019-06-13 11:07:10 -04:00
parent c567be407c
commit 83d77fabb0
3 changed files with 15 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/algorithms/tracking/libs/cubature_filter.cc
View File

@ -84,7 +84,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 * 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, ModelFunction* transition_fcn, const arma::mat& noise_covariance) 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)
{ {
// Compute number of cubature points // Compute number of cubature points
int nx = x_post.n_elem; int nx = x_post.n_elem;
@ -125,7 +125,7 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma
/* /*
* Perform the update step of the cubature Kalman filter * 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, ModelFunction* measurement_fcn, const arma::mat& noise_covariance) 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)
{ {
// Compute number of cubature points // Compute number of cubature points
int nx = x_pred.n_elem; int nx = x_pred.n_elem;
@ -188,8 +188,3 @@ arma::mat Cubature_filter::get_P_x_est() const
{ {
return P_x_est; return P_x_est;
} }
double Cubature_filter::func_number(double number, TestModel* func)
{
return (*func)(number);
}

21
src/algorithms/tracking/libs/cubature_filter.h
View File

@ -45,19 +45,11 @@
#include <gnuradio/gr_complex.h> #include <gnuradio/gr_complex.h>
// Abstract model function // Abstract model function
class ModelFunction{ class Model_Function{
public: public:
ModelFunction() {}; Model_Function() {};
virtual arma::vec operator() (arma::vec input) = 0; virtual arma::vec operator() (arma::vec input) = 0;
virtual ~ModelFunction() = default; virtual ~Model_Function() = default;
};
class TestModel{
public:
TestModel() {};
//virtual arma::vec operator() (arma::vec input) = 0;
virtual double operator() (double input) = 0;
virtual ~TestModel() = default;
}; };
class Cubature_filter class Cubature_filter
@ -73,8 +65,8 @@ public:
void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0); void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
// Prediction and estimation // Prediction and estimation
void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, const arma::mat& noise_covariance); 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, ModelFunction* measurement_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);
// Getters // Getters
arma::mat get_x_pred() const; arma::mat get_x_pred() const;
@ -82,9 +74,6 @@ public:
arma::mat get_x_est() const; arma::mat get_x_est() const;
arma::mat get_P_x_est() const; arma::mat get_P_x_est() const;
//Test-dev
double func_number(double number, TestModel* func);
private: private:
arma::vec x_pred_out; arma::vec x_pred_out;
arma::mat P_x_pred_out; arma::mat P_x_pred_out;

16
src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc
View File

@ -35,17 +35,17 @@
#define CUBATURE_TEST_N_TRIALS 1000 #define CUBATURE_TEST_N_TRIALS 1000
class TransitionModel : public ModelFunction { class Transition_Model : public Model_Function {
public: public:
TransitionModel(arma::mat kf_F) {coeff_mat = kf_F;}; Transition_Model(arma::mat kf_F) {coeff_mat = kf_F;};
virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;}; virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;};
private: private:
arma::mat coeff_mat; arma::mat coeff_mat;
}; };
class MeasurementModel : public ModelFunction { class Measurement_Model : public Model_Function {
public: public:
MeasurementModel(arma::mat kf_H) {coeff_mat = kf_H;}; Measurement_Model(arma::mat kf_H) {coeff_mat = kf_H;};
virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;}; virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;};
private: private:
arma::mat coeff_mat; arma::mat coeff_mat;
@ -83,8 +83,8 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
arma::mat kf_P_y; arma::mat kf_P_y;
arma::mat kf_K; arma::mat kf_K;
ModelFunction* transition_function; Model_Function* transition_function;
ModelFunction* measurement_function; Model_Function* measurement_function;
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
@ -112,7 +112,7 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
kf_F = arma::randu<arma::mat>(nx,nx); kf_F = arma::randu<arma::mat>(nx,nx);
kf_Q = arma::diagmat(arma::randu<arma::vec>(nx,1)); kf_Q = arma::diagmat(arma::randu<arma::vec>(nx,1));
transition_function = new TransitionModel(kf_F); transition_function = new Transition_Model(kf_F);
arma::mat ttx = (*transition_function)(kf_x_post); arma::mat ttx = (*transition_function)(kf_x_post);
kf_cubature.predict_sequential(kf_x_post,kf_P_x_post,transition_function,kf_Q); kf_cubature.predict_sequential(kf_x_post,kf_P_x_post,transition_function,kf_Q);
@ -135,7 +135,7 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
kf_y = kf_H*(kf_F*kf_x + eta) + nu; kf_y = kf_H*(kf_F*kf_x + eta) + nu;
measurement_function = new MeasurementModel(kf_H); measurement_function = new Measurement_Model(kf_H);
kf_cubature.update_sequential(kf_y,kf_x_pre,kf_P_x_pre,measurement_function,kf_R); kf_cubature.update_sequential(kf_y,kf_x_pre,kf_P_x_pre,measurement_function,kf_R);
ckf_x_post = kf_cubature.get_x_est(); ckf_x_post = kf_cubature.get_x_est();