mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-20 19:54:07 +00:00
Code Issues Releases Wiki Activity

Apply code formatting

Browse Source
This commit is contained in:
Carles Fernandez
2019-06-13 17:37:32 +02:00
parent ce3d018f67
commit dd2198fd00
3 changed files with 71 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -51,6 +51,7 @@ Cubature_filter::Cubature_filter()
P_x_est = P_x_pred_out; P_x_est = P_x_pred_out;
} }
Cubature_filter::Cubature_filter(int nx) Cubature_filter::Cubature_filter(int nx)
{ {
x_pred_out = arma::zeros(nx, 1); x_pred_out = arma::zeros(nx, 1);
@@ -60,6 +61,7 @@ Cubature_filter::Cubature_filter(int nx)
P_x_est = P_x_pred_out; P_x_est = P_x_pred_out;
} }
Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0) Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
{ {
x_pred_out = x_pred_0; x_pred_out = x_pred_0;
@@ -69,8 +71,10 @@ Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x
P_x_est = P_x_pred_out; P_x_est = P_x_pred_out;
} }
Cubature_filter::~Cubature_filter() = default; Cubature_filter::~Cubature_filter() = default;
void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0) void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
{ {
x_pred_out = x_pred_0; x_pred_out = x_pred_0;
@@ -106,7 +110,7 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma
for (uint8_t i = 0; i < np; i++) for (uint8_t i = 0; i < np; i++)
{ {
Xi_post = Sm_post * (std::sqrt(((float) np) / 2.0) * gen_one.col(i)) + x_post; Xi_post = Sm_post * (std::sqrt(static_cast<float>(np) / 2.0) * gen_one.col(i)) + x_post;
Xi_pred = (*transition_fcn)(Xi_post); Xi_pred = (*transition_fcn)(Xi_post);
x_pred = x_pred + Xi_pred; x_pred = x_pred + Xi_pred;
@@ -114,14 +118,15 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma
} }
// Estimate predicted state and error covariance // Estimate predicted state and error covariance
x_pred = x_pred / ((float) np); x_pred = x_pred / static_cast<float>(np);
P_x_pred = P_x_pred / ((float) np) - x_pred*x_pred.t() + noise_covariance; P_x_pred = P_x_pred / static_cast<float>(np) - x_pred * x_pred.t() + noise_covariance;
// Store predicted state and error covariance // Store predicted state and error covariance
x_pred_out = x_pred; x_pred_out = x_pred;
P_x_pred_out = P_x_pred; P_x_pred_out = P_x_pred;
} }
/* /*
* Perform the update step of the cubature Kalman filter * Perform the update step of the cubature Kalman filter
*/ */
@@ -148,7 +153,7 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
arma::vec Zi_pred; arma::vec Zi_pred;
for (uint8_t i = 0; i < np; i++) for (uint8_t i = 0; i < np; i++)
{ {
Xi_pred = Sm_pred * (std::sqrt(((float) np) / 2.0) * gen_one.col(i)) + x_pred; Xi_pred = Sm_pred * (std::sqrt(static_cast<float>(np) / 2.0) * gen_one.col(i)) + x_pred;
Zi_pred = (*measurement_fcn)(Xi_pred); Zi_pred = (*measurement_fcn)(Xi_pred);
z_pred = z_pred + Zi_pred; z_pred = z_pred + Zi_pred;
@@ -157,9 +162,9 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
} }
// Estimate measurement covariance and cross covariances // Estimate measurement covariance and cross covariances
z_pred = z_pred / ((float) np); z_pred = z_pred / static_cast<float>(np);
P_zz_pred = P_zz_pred / ((float) np) - z_pred*z_pred.t() + noise_covariance; P_zz_pred = P_zz_pred / static_cast<float>(np) - z_pred * z_pred.t() + noise_covariance;
P_xz_pred = P_xz_pred / ((float) np) - x_pred*z_pred.t(); P_xz_pred = P_xz_pred / static_cast<float>(np) - x_pred * z_pred.t();
// Estimate cubature Kalman gain // Estimate cubature Kalman gain
arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred); arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred);
@@ -169,21 +174,25 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
P_x_est = P_x_pred - W_k * P_zz_pred * W_k.t(); P_x_est = P_x_pred - W_k * P_zz_pred * W_k.t();
} }
arma::mat Cubature_filter::get_x_pred() const arma::mat Cubature_filter::get_x_pred() const
{ {
return x_pred_out; return x_pred_out;
} }
arma::mat Cubature_filter::get_P_x_pred() const arma::mat Cubature_filter::get_P_x_pred() const
{ {
return P_x_pred_out; return P_x_pred_out;
} }
arma::mat Cubature_filter::get_x_est() const arma::mat Cubature_filter::get_x_est() const
{ {
return x_est; return x_est;
} }
arma::mat Cubature_filter::get_P_x_est() const arma::mat Cubature_filter::get_P_x_est() const
{ {
return P_x_est; return P_x_est;

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

@@ -35,18 +35,22 @@
#define CUBATURE_TEST_N_TRIALS 1000 #define CUBATURE_TEST_N_TRIALS 1000
class Transition_Model : public Model_Function { class Transition_Model : public Model_Function
{
public: public:
Transition_Model(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 Measurement_Model : public Model_Function { class Measurement_Model : public Model_Function
{
public: public:
Measurement_Model(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;
}; };