mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-25 22:43:14 +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

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

@ -8,7 +8,7 @@
* an Unscented Kalman Filter which uses Unscented Transform rules to * an Unscented Kalman Filter which uses Unscented Transform rules to
* perform a similar estimation. * perform a similar estimation.
* *
* [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
* Transactions on Automatic Control, 54(6):1254–1269,2009. * Transactions on Automatic Control, 54(6):1254–1269,2009.
* *
* \authors <ul> * \authors <ul>
@ -54,6 +54,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);
@ -63,6 +64,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;
@ -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; 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;
@ -94,37 +98,38 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma
int np = 2 * nx; int np = 2 * nx;
// Generator Matrix // Generator Matrix
arma::mat gen_one = arma::join_horiz(arma::eye(nx,nx),-1.0*arma::eye(nx,nx)); arma::mat gen_one = arma::join_horiz(arma::eye(nx, nx), -1.0 * arma::eye(nx, nx));
// Initialize predicted mean and covariance // Initialize predicted mean and covariance
arma::vec x_pred = arma::zeros(nx,1); arma::vec x_pred = arma::zeros(nx, 1);
arma::mat P_x_pred = arma::zeros(nx,nx); arma::mat P_x_pred = arma::zeros(nx, nx);
// Factorize posterior covariance // Factorize posterior covariance
arma::mat Sm_post = arma::chol(P_x_post, "lower"); arma::mat Sm_post = arma::chol(P_x_post, "lower");
// Propagate and evaluate cubature points // Propagate and evaluate cubature points
arma::vec Xi_post; arma::vec Xi_post;
arma::vec Xi_pred; arma::vec Xi_pred;
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;
P_x_pred = P_x_pred + Xi_pred*Xi_pred.t(); P_x_pred = P_x_pred + Xi_pred * Xi_pred.t();
} }
// Compute predicted mean and error covariance // Compute predicted mean 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 mean and error covariance // Store predicted mean 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
*/ */
@ -136,12 +141,12 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
int np = 2 * nx; int np = 2 * nx;
// Generator Matrix // Generator Matrix
arma::mat gen_one = arma::join_horiz(arma::eye(nx,nx),-1.0*arma::eye(nx,nx)); arma::mat gen_one = arma::join_horiz(arma::eye(nx, nx), -1.0 * arma::eye(nx, nx));
// Initialize estimated predicted measurement and covariances // Initialize estimated predicted measurement and covariances
arma::mat z_pred = arma::zeros(nz,1); arma::mat z_pred = arma::zeros(nz, 1);
arma::mat P_zz_pred = arma::zeros(nz,nz); arma::mat P_zz_pred = arma::zeros(nz, nz);
arma::mat P_xz_pred = arma::zeros(nx,nz); arma::mat P_xz_pred = arma::zeros(nx, nz);
// Factorize predicted covariance // Factorize predicted covariance
arma::mat Sm_pred = arma::chol(P_x_pred, "lower"); arma::mat Sm_pred = arma::chol(P_x_pred, "lower");
@ -150,49 +155,54 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec&
arma::vec Xi_pred; arma::vec Xi_pred;
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;
P_zz_pred = P_zz_pred + Zi_pred*Zi_pred.t(); P_zz_pred = P_zz_pred + Zi_pred * Zi_pred.t();
P_xz_pred = P_xz_pred + Xi_pred*Zi_pred.t(); P_xz_pred = P_xz_pred + Xi_pred * Zi_pred.t();
} }
// Compute measurement mean, covariance and cross covariance // Compute measurement mean, covariance and cross covariance
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();
// Compute cubature Kalman gain // Compute 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);
// Compute and store the updated mean and error covariance // Compute and store the updated mean and error covariance
x_est = x_pred + W_k*(z_upd - z_pred); x_est = x_pred + W_k * (z_upd - z_pred);
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;
} }
/***************** END CUBATURE KALMAN FILTER *****************/ /***************** END CUBATURE KALMAN FILTER *****************/
/***************** UNSCENTED KALMAN FILTER *****************/ /***************** UNSCENTED KALMAN FILTER *****************/
Unscented_filter::Unscented_filter() Unscented_filter::Unscented_filter()
@ -205,6 +215,7 @@ Unscented_filter::Unscented_filter()
P_x_est = P_x_pred_out; P_x_est = P_x_pred_out;
} }
Unscented_filter::Unscented_filter(int nx) Unscented_filter::Unscented_filter(int nx)
{ {
x_pred_out = arma::zeros(nx, 1); x_pred_out = arma::zeros(nx, 1);
@ -214,6 +225,7 @@ Unscented_filter::Unscented_filter(int nx)
P_x_est = P_x_pred_out; P_x_est = P_x_pred_out;
} }
Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0) Unscented_filter::Unscented_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;
@ -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; P_x_est = P_x_pred_out;
} }
Unscented_filter::~Unscented_filter() = default; Unscented_filter::~Unscented_filter() = default;
void Unscented_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0) void Unscented_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;
@ -248,40 +262,40 @@ void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::m
float kappa = 0.0; float kappa = 0.0;
float beta = 2.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 // Compute UT Weights
float W0_m = lambda / (((float) nx) + lambda); float W0_m = lambda / (static_cast<float>(nx) + lambda);
float W0_c = lambda / (((float) nx) + lambda) + (1 - std::pow(alpha,2.0) + beta); float W0_c = lambda / (static_cast<float>(nx) + lambda) + (1 - std::pow(alpha, 2.0) + beta);
float Wi_m = 1.0 / (2.0 * (((float) nx) + lambda)); float Wi_m = 1.0 / (2.0 * (static_cast<float>(nx) + lambda));
// Propagate and evaluate sigma points // Propagate and evaluate sigma points
arma::mat Xi_fact = arma::zeros(nx,nx); arma::mat Xi_fact = arma::zeros(nx, nx);
arma::mat Xi_post = arma::zeros(nx,np); arma::mat Xi_post = arma::zeros(nx, np);
arma::mat Xi_pred = arma::zeros(nx,np); arma::mat Xi_pred = arma::zeros(nx, np);
Xi_post.col(0) = x_post; Xi_post.col(0) = x_post;
Xi_pred.col(0) = (*transition_fcn)(Xi_post.col(0)); Xi_pred.col(0) = (*transition_fcn)(Xi_post.col(0));
for (uint8_t i = 1; i <= nx; i++) 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) = x_post + Xi_fact.col(i - 1);
Xi_post.col(i+nx) = x_post - Xi_fact.col(i-1); Xi_post.col(i + nx) = x_post - Xi_fact.col(i - 1);
Xi_pred.col(i) = (*transition_fcn)(Xi_post.col(i)); Xi_pred.col(i) = (*transition_fcn)(Xi_post.col(i));
Xi_pred.col(i+nx) = (*transition_fcn)(Xi_post.col(i+nx)); Xi_pred.col(i + nx) = (*transition_fcn)(Xi_post.col(i + nx));
} }
// Compute predicted mean // Compute predicted mean
arma::vec x_pred = W0_m*Xi_pred.col(0) + Wi_m*arma::sum(Xi_pred.cols(1,np-1),1); arma::vec x_pred = W0_m * Xi_pred.col(0) + Wi_m * arma::sum(Xi_pred.cols(1, np - 1), 1);
// Compute predicted error covariance // Compute predicted error covariance
arma::mat P_x_pred = W0_c*((Xi_pred.col(0)-x_pred) * (Xi_pred.col(0).t()-x_pred.t())); arma::mat P_x_pred = W0_c * ((Xi_pred.col(0) - x_pred) * (Xi_pred.col(0).t() - x_pred.t()));
for (uint8_t i = 1; i < np; i++) for (uint8_t i = 1; i < np; i++)
{ {
P_x_pred = P_x_pred + Wi_m*((Xi_pred.col(i)-x_pred) * (Xi_pred.col(i).t()-x_pred.t())); P_x_pred = P_x_pred + Wi_m * ((Xi_pred.col(i) - x_pred) * (Xi_pred.col(i).t() - x_pred.t()));
} }
P_x_pred = P_x_pred + noise_covariance; P_x_pred = P_x_pred + noise_covariance;
// Store predicted mean and error covariance // Store predicted mean and error covariance
@ -289,6 +303,7 @@ void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::m
P_x_pred_out = P_x_pred; P_x_pred_out = P_x_pred;
} }
/* /*
* Perform the update step of the Unscented Kalman filter * Perform the update step of the Unscented Kalman filter
*/ */
@ -303,68 +318,73 @@ void Unscented_filter::update_sequential(const arma::vec& z_upd, const arma::vec
float kappa = 0.0; float kappa = 0.0;
float beta = 2.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 // Compute UT Weights
float W0_m = lambda / (((float) nx) + lambda); float W0_m = lambda / (static_cast<float>(nx) + lambda);
float W0_c = lambda / (((float) nx) + lambda) + (1 - std::pow(alpha,2.0) + beta); float W0_c = lambda / (static_cast<float>(nx) + lambda) + (1.0 - std::pow(alpha, 2.0) + beta);
float Wi_m = 1.0 / (2.0 * (((float) nx) + lambda)); float Wi_m = 1.0 / (2.0 * (static_cast<float>(nx) + lambda));
// Propagate and evaluate sigma points // Propagate and evaluate sigma points
arma::mat Xi_fact = arma::zeros(nx,nx); arma::mat Xi_fact = arma::zeros(nx, nx);
arma::mat Xi_pred = arma::zeros(nx,np); arma::mat Xi_pred = arma::zeros(nx, np);
arma::mat Zi_pred = arma::zeros(nz,np); arma::mat Zi_pred = arma::zeros(nz, np);
Xi_pred.col(0) = x_pred; Xi_pred.col(0) = x_pred;
Zi_pred.col(0) = (*measurement_fcn)(Xi_pred.col(0)); Zi_pred.col(0) = (*measurement_fcn)(Xi_pred.col(0));
for (uint8_t i = 1; i <= nx; i++) 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) = x_pred + Xi_fact.col(i - 1);
Xi_pred.col(i+nx) = x_pred - Xi_fact.col(i-1); Xi_pred.col(i + nx) = x_pred - Xi_fact.col(i - 1);
Zi_pred.col(i) = (*measurement_fcn)(Xi_pred.col(i)); Zi_pred.col(i) = (*measurement_fcn)(Xi_pred.col(i));
Zi_pred.col(i+nx) = (*measurement_fcn)(Xi_pred.col(i+nx)); Zi_pred.col(i + nx) = (*measurement_fcn)(Xi_pred.col(i + nx));
} }
// Compute measurement mean // Compute measurement mean
arma::mat z_pred = W0_m*Zi_pred.col(0) + Wi_m*arma::sum(Zi_pred.cols(1,np-1),1); arma::mat z_pred = W0_m * Zi_pred.col(0) + Wi_m * arma::sum(Zi_pred.cols(1, np - 1), 1);
// Compute measurement covariance and cross covariance // Compute measurement covariance and cross covariance
arma::mat P_zz_pred = W0_c * ((Zi_pred.col(0) - z_pred) * (Zi_pred.col(0).t() - z_pred.t())); arma::mat P_zz_pred = W0_c * ((Zi_pred.col(0) - z_pred) * (Zi_pred.col(0).t() - z_pred.t()));
arma::mat P_xz_pred = W0_c * ((Xi_pred.col(0) - x_pred) * (Zi_pred.col(0).t() - z_pred.t())); arma::mat P_xz_pred = W0_c * ((Xi_pred.col(0) - x_pred) * (Zi_pred.col(0).t() - z_pred.t()));
for (uint8_t i = 0; i < np; i++) for (uint8_t i = 0; i < np; i++)
{ {
P_zz_pred = P_zz_pred + Wi_m * ((Zi_pred.col(i) - z_pred) * (Zi_pred.col(i).t() - z_pred.t())); P_zz_pred = P_zz_pred + Wi_m * ((Zi_pred.col(i) - z_pred) * (Zi_pred.col(i).t() - z_pred.t()));
P_xz_pred = P_xz_pred + Wi_m * ((Xi_pred.col(i) - x_pred) * (Zi_pred.col(i).t() - z_pred.t())); P_xz_pred = P_xz_pred + Wi_m * ((Xi_pred.col(i) - x_pred) * (Zi_pred.col(i).t() - z_pred.t()));
} }
P_zz_pred = P_zz_pred + noise_covariance; P_zz_pred = P_zz_pred + noise_covariance;
// 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);
// Estimate and store the updated mean and error covariance // Estimate and store the updated mean and error covariance
x_est = x_pred + W_k*(z_upd - z_pred); x_est = x_pred + W_k * (z_upd - z_pred);
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 Unscented_filter::get_x_pred() const arma::mat Unscented_filter::get_x_pred() const
{ {
return x_pred_out; return x_pred_out;
} }
arma::mat Unscented_filter::get_P_x_pred() const arma::mat Unscented_filter::get_P_x_pred() const
{ {
return P_x_pred_out; return P_x_pred_out;
} }
arma::mat Unscented_filter::get_x_est() const arma::mat Unscented_filter::get_x_est() const
{ {
return x_est; return x_est;
} }
arma::mat Unscented_filter::get_P_x_est() const arma::mat Unscented_filter::get_P_x_est() const
{ {
return P_x_est; return P_x_est;
} }
/***************** END UNSCENTED KALMAN FILTER *****************/ /***************** END UNSCENTED KALMAN FILTER *****************/

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

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

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

@ -36,20 +36,24 @@
#define UNSCENTED_TEST_N_TRIALS 10 #define UNSCENTED_TEST_N_TRIALS 10
#define UNSCENTED_TEST_TOLERANCE 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;}; public:
virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;}; Transition_Model_UKF(arma::mat kf_F) { coeff_mat = kf_F; };
private: virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
arma::mat coeff_mat;
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;}; public:
virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;}; Measurement_Model_UKF(arma::mat kf_H) { coeff_mat = kf_H; };
private: virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; };
arma::mat coeff_mat;
private:
arma::mat coeff_mat;
}; };
TEST(UnscentedFilterComputationTest, UnscentedFilterTest) TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
@ -102,21 +106,21 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
nx = std::rand() % 5 + 1; nx = std::rand() % 5 + 1;
ny = std::rand() % 5 + 1; ny = std::rand() % 5 + 1;
kf_x = arma::randn<arma::vec>(nx,1); kf_x = arma::randn<arma::vec>(nx, 1);
kf_P_x_post = 5.0 * arma::diagmat(arma::randu<arma::vec>(nx,1)); kf_P_x_post = 5.0 * arma::diagmat(arma::randu<arma::vec>(nx, 1));
kf_x_post = arma::mvnrnd(kf_x, kf_P_x_post); kf_x_post = arma::mvnrnd(kf_x, kf_P_x_post);
kf_unscented.initialize(kf_x_post, kf_P_x_post); kf_unscented.initialize(kf_x_post, kf_P_x_post);
// Prediction Step // Prediction Step
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 Transition_Model_UKF(kf_F); transition_function = new Transition_Model_UKF(kf_F);
arma::mat ttx = (*transition_function)(kf_x_post); arma::mat ttx = (*transition_function)(kf_x_post);
kf_unscented.predict_sequential(kf_x_post,kf_P_x_post,transition_function,kf_Q); kf_unscented.predict_sequential(kf_x_post, kf_P_x_post, transition_function, kf_Q);
ukf_x_pre = kf_unscented.get_x_pred(); ukf_x_pre = kf_unscented.get_x_pred();
ukf_P_x_pre = kf_unscented.get_P_x_pred(); ukf_P_x_pre = kf_unscented.get_P_x_pred();
@ -128,16 +132,16 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
EXPECT_TRUE(arma::approx_equal(ukf_P_x_pre, kf_P_x_pre, "absdiff", UNSCENTED_TEST_TOLERANCE)); EXPECT_TRUE(arma::approx_equal(ukf_P_x_pre, kf_P_x_pre, "absdiff", UNSCENTED_TEST_TOLERANCE));
// Update Step // Update Step
kf_H = arma::randu<arma::mat>(ny,nx); kf_H = arma::randu<arma::mat>(ny, nx);
kf_R = arma::diagmat(arma::randu<arma::vec>(ny,1)); kf_R = arma::diagmat(arma::randu<arma::vec>(ny, 1));
eta = arma::mvnrnd(arma::zeros<arma::vec>(nx,1),kf_Q); eta = arma::mvnrnd(arma::zeros<arma::vec>(nx, 1), kf_Q);
nu = arma::mvnrnd(arma::zeros<arma::vec>(ny,1),kf_R); nu = arma::mvnrnd(arma::zeros<arma::vec>(ny, 1), kf_R);
kf_y = kf_H*(kf_F*kf_x + eta) + nu; kf_y = kf_H * (kf_F * kf_x + eta) + nu;
measurement_function = new Measurement_Model_UKF(kf_H); measurement_function = new Measurement_Model_UKF(kf_H);
kf_unscented.update_sequential(kf_y,kf_x_pre,kf_P_x_pre,measurement_function,kf_R); kf_unscented.update_sequential(kf_y, kf_x_pre, kf_P_x_pre, measurement_function, kf_R);
ukf_x_post = kf_unscented.get_x_est(); ukf_x_post = kf_unscented.get_x_est();
ukf_P_x_post = kf_unscented.get_P_x_est(); ukf_P_x_post = kf_unscented.get_P_x_est();
@ -146,7 +150,7 @@ TEST(UnscentedFilterComputationTest, UnscentedFilterTest)
kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y);
kf_x_post = kf_x_pre + kf_K * (kf_y - kf_H * kf_x_pre); kf_x_post = kf_x_pre + kf_K * (kf_y - kf_H * kf_x_pre);
kf_P_x_post = (arma::eye(nx,nx) - kf_K * kf_H) * kf_P_x_pre; kf_P_x_post = (arma::eye(nx, nx) - kf_K * kf_H) * kf_P_x_pre;
EXPECT_TRUE(arma::approx_equal(ukf_x_post, kf_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE)); EXPECT_TRUE(arma::approx_equal(ukf_x_post, kf_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE));
EXPECT_TRUE(arma::approx_equal(ukf_P_x_post, kf_P_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE)); EXPECT_TRUE(arma::approx_equal(ukf_P_x_post, kf_P_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE));