mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-26 20:53:20 +00:00
Code Issues Releases Wiki Activity

Apply clang-format

Browse Source
This commit is contained in:
Carles Fernandez 2019-06-14 10:21:26 +02:00
parent c1f4c2aef3
commit 7c23fb35b6
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
3 changed files with 128 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -54,6 +54,7 @@ Cubature_filter::Cubature_filter()
P_x_est = P_x_pred_out;
}
Cubature_filter::Cubature_filter(int nx)
{
x_pred_out = arma::zeros(nx, 1);
@ -63,6 +64,7 @@ Cubature_filter::Cubature_filter(int nx)
P_x_est = P_x_pred_out;
}
Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
@ -72,8 +74,10 @@ Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x
P_x_est = P_x_pred_out;
}
Cubature_filter::~Cubature_filter() = default;
void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
@ -109,7 +113,7 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma
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);
x_pred = x_pred + Xi_pred;
@ -117,14 +121,15 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma
}
// Compute predicted mean and error covariance
x_pred = x_pred / ((float) np);
P_x_pred = P_x_pred / ((float) np) - x_pred*x_pred.t() + noise_covariance;
x_pred = x_pred / static_cast<float>(np);
P_x_pred = P_x_pred / static_cast<float>(np) - x_pred * x_pred.t() + noise_covariance;
// Store predicted mean and error covariance
x_pred_out = x_pred;
P_x_pred_out = P_x_pred;
}
/*
* Perform the update step of the cubature Kalman filter
*/
@ -151,7 +156,7 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
arma::vec Zi_pred;
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);
z_pred = z_pred + Zi_pred;
@ -160,9 +165,9 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
}
// Compute measurement mean, covariance and cross covariance
z_pred = z_pred / ((float) np);
P_zz_pred = P_zz_pred / ((float) np) - z_pred*z_pred.t() + noise_covariance;
P_xz_pred = P_xz_pred / ((float) np) - x_pred*z_pred.t();
z_pred = z_pred / static_cast<float>(np);
P_zz_pred = P_zz_pred / static_cast<float>(np) - z_pred * z_pred.t() + noise_covariance;
P_xz_pred = P_xz_pred / static_cast<float>(np) - x_pred * z_pred.t();
// Compute cubature Kalman gain
arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred);
@ -172,27 +177,32 @@ 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();
}
arma::mat Cubature_filter::get_x_pred() const
{
return x_pred_out;
}
arma::mat Cubature_filter::get_P_x_pred() const
{
return P_x_pred_out;
}
arma::mat Cubature_filter::get_x_est() const
{
return x_est;
}
arma::mat Cubature_filter::get_P_x_est() const
{
return P_x_est;
}
/***************** END CUBATURE KALMAN FILTER *****************/
/***************** UNSCENTED KALMAN FILTER *****************/
Unscented_filter::Unscented_filter()
@ -205,6 +215,7 @@ Unscented_filter::Unscented_filter()
P_x_est = P_x_pred_out;
}
Unscented_filter::Unscented_filter(int nx)
{
x_pred_out = arma::zeros(nx, 1);
@ -214,6 +225,7 @@ Unscented_filter::Unscented_filter(int nx)
P_x_est = P_x_pred_out;
}
Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
@ -223,8 +235,10 @@ Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P
P_x_est = P_x_pred_out;
}
Unscented_filter::~Unscented_filter() = default;
void Unscented_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
@ -248,12 +262,12 @@ void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::m
float kappa = 0.0;
float beta = 2.0;
float lambda = std::pow(alpha,2.0)*(((float) nx) + kappa) - ((float) nx);
float lambda = std::pow(alpha, 2.0) * (static_cast<float>(nx) + kappa) - static_cast<float>(nx);
// Compute UT Weights
float W0_m = lambda / (((float) nx) + lambda);
float W0_c = lambda / (((float) nx) + lambda) + (1 - std::pow(alpha,2.0) + beta);
float Wi_m = 1.0 / (2.0 * (((float) nx) + lambda));
float W0_m = lambda / (static_cast<float>(nx) + lambda);
float W0_c = lambda / (static_cast<float>(nx) + lambda) + (1 - std::pow(alpha, 2.0) + beta);
float Wi_m = 1.0 / (2.0 * (static_cast<float>(nx) + lambda));
// Propagate and evaluate sigma points
arma::mat Xi_fact = arma::zeros(nx, nx);
@ -265,7 +279,7 @@ void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::m
Xi_pred.col(0) = (*transition_fcn)(Xi_post.col(0));
for (uint8_t i = 1; i <= nx; i++)
{
Xi_fact = std::sqrt(((float) nx) + lambda) * arma::sqrtmat_sympd(P_x_post);
Xi_fact = std::sqrt(static_cast<float>(nx) + lambda) * arma::sqrtmat_sympd(P_x_post);
Xi_post.col(i) = x_post + Xi_fact.col(i - 1);
Xi_post.col(i + nx) = x_post - Xi_fact.col(i - 1);
@ -289,6 +303,7 @@ void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::m
P_x_pred_out = P_x_pred;
}
/*
* Perform the update step of the Unscented Kalman filter
*/
@ -303,12 +318,12 @@ void Unscented_filter::update_sequential(const arma::vec& z_upd, const arma::vec
float kappa = 0.0;
float beta = 2.0;
float lambda = std::pow(alpha,2.0)*(((float) nx) + kappa) - ((float) nx);
float lambda = std::pow(alpha, 2.0) * (static_cast<float>(nx) + kappa) - static_cast<float>(nx);
// Compute UT Weights
float W0_m = lambda / (((float) nx) + lambda);
float W0_c = lambda / (((float) nx) + lambda) + (1 - std::pow(alpha,2.0) + beta);
float Wi_m = 1.0 / (2.0 * (((float) nx) + lambda));
float W0_m = lambda / (static_cast<float>(nx) + lambda);
float W0_c = lambda / (static_cast<float>(nx) + lambda) + (1.0 - std::pow(alpha, 2.0) + beta);
float Wi_m = 1.0 / (2.0 * (static_cast<float>(nx) + lambda));
// Propagate and evaluate sigma points
arma::mat Xi_fact = arma::zeros(nx, nx);
@ -319,7 +334,7 @@ void Unscented_filter::update_sequential(const arma::vec& z_upd, const arma::vec
Zi_pred.col(0) = (*measurement_fcn)(Xi_pred.col(0));
for (uint8_t i = 1; i <= nx; i++)
{
Xi_fact = std::sqrt(((float) nx) + lambda) * arma::sqrtmat_sympd(P_x_pred);
Xi_fact = std::sqrt(static_cast<float>(nx) + lambda) * arma::sqrtmat_sympd(P_x_pred);
Xi_pred.col(i) = x_pred + Xi_fact.col(i - 1);
Xi_pred.col(i + nx) = x_pred - Xi_fact.col(i - 1);
@ -348,23 +363,28 @@ void Unscented_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();
}
arma::mat Unscented_filter::get_x_pred() const
{
return x_pred_out;
}
arma::mat Unscented_filter::get_P_x_pred() const
{
return P_x_pred_out;
}
arma::mat Unscented_filter::get_x_est() const
{
return x_est;
}
arma::mat Unscented_filter::get_P_x_est() const
{
return P_x_est;
}
/***************** END UNSCENTED KALMAN FILTER *****************/

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

@ -47,7 +47,8 @@
#include <gnuradio/gr_complex.h>
// Abstract model function
class Model_Function{
class Model_Function
{
public:
Model_Function(){};
virtual arma::vec operator()(arma::vec input) = 0;

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

@ -36,18 +36,22 @@
#define UNSCENTED_TEST_N_TRIALS 10
#define UNSCENTED_TEST_TOLERANCE 10
class Transition_Model_UKF : public Model_Function {
class Transition_Model_UKF : public Model_Function
{
public:
Transition_Model_UKF(arma::mat kf_F) { coeff_mat = kf_F; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
private:
arma::mat coeff_mat;
};
class Measurement_Model_UKF : public Model_Function {
class Measurement_Model_UKF : public Model_Function
{
public:
Measurement_Model_UKF(arma::mat kf_H) { coeff_mat = kf_H; };
virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
private:
arma::mat coeff_mat;
};