From 0e68befe7c486e35b331e266eff069c3ca5d32a4 Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Thu, 13 Jun 2019 15:42:52 -0400 Subject: [PATCH] Add unscented filter to nonlinear_filtering library and add associated unit test --- .../tracking/libs/nonlinear_tracking.cc | 198 +++++++++++++++++- .../tracking/libs/nonlinear_tracking.h | 41 +++- src/tests/CMakeLists.txt | 1 + src/tests/test_main.cc | 1 + .../tracking/unscented_filter_test.cc | 157 ++++++++++++++ 5 files changed, 384 insertions(+), 14 deletions(-) create mode 100644 src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc diff --git a/src/algorithms/tracking/libs/nonlinear_tracking.cc b/src/algorithms/tracking/libs/nonlinear_tracking.cc index 05b346a7c..5b5ec072c 100644 --- a/src/algorithms/tracking/libs/nonlinear_tracking.cc +++ b/src/algorithms/tracking/libs/nonlinear_tracking.cc @@ -1,10 +1,12 @@ /*! * \file cubature_filter.cc - * \brief Interface of a library with Bayesian noise statistic estimation + * \brief Interface of a library for nonlinear tracking algorithms * * Cubature_Filter implements the functionality of the Cubature Kalman * Filter, which uses multidimensional cubature rules to estimate the - * time evolution of a nonlinear system. + * time evolution of a nonlinear system. Unscented_filter implements + * an Unscented Kalman Filter which uses Unscented Transform rules to + * perform a similar estimation. * * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE * Transactions on Automatic Control, 54(6):1254–1269,2009. @@ -38,8 +40,9 @@ * ------------------------------------------------------------------------- */ -#include "cubature_filter.h" +#include "nonlinear_tracking.h" +/***************** CUBATURE KALMAN FILTER *****************/ Cubature_filter::Cubature_filter() { @@ -113,11 +116,11 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma P_x_pred = P_x_pred + Xi_pred*Xi_pred.t(); } - // Estimate predicted state and error covariance + // 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; - // Store predicted state and error covariance + // Store predicted mean and error covariance x_pred_out = x_pred; P_x_pred_out = P_x_pred; } @@ -135,7 +138,7 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec& // Generator Matrix arma::mat gen_one = arma::join_horiz(arma::eye(nx,nx),-1.0*arma::eye(nx,nx)); - // Evaluate predicted measurement and covariances + // Initialize estimated predicted measurement and covariances arma::mat z_pred = arma::zeros(nz,1); arma::mat P_zz_pred = arma::zeros(nz,nz); arma::mat P_xz_pred = arma::zeros(nx,nz); @@ -156,15 +159,15 @@ void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec& P_xz_pred = P_xz_pred + Xi_pred*Zi_pred.t(); } - // Estimate measurement covariance and cross covariances + // 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(); - // Estimate cubature Kalman gain + // Compute cubature Kalman gain arma::mat W_k = P_xz_pred*arma::inv(P_zz_pred); - // Estimate and store the updated state and error covariance + // Compute and store the updated mean and error covariance x_est = x_pred + W_k*(z_upd - z_pred); P_x_est = P_x_pred - W_k*P_zz_pred*W_k.t(); } @@ -188,3 +191,180 @@ arma::mat Cubature_filter::get_P_x_est() const { return P_x_est; } +/***************** END CUBATURE KALMAN FILTER *****************/ + +/***************** UNSCENTED KALMAN FILTER *****************/ + +Unscented_filter::Unscented_filter() +{ + int nx = 1; + x_pred_out = arma::zeros(nx, 1); + P_x_pred_out = arma::eye(nx, nx) * (nx + 1); + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + +Unscented_filter::Unscented_filter(int nx) +{ + x_pred_out = arma::zeros(nx, 1); + P_x_pred_out = arma::eye(nx, nx) * (nx + 1); + + x_est = 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) +{ + x_pred_out = x_pred_0; + P_x_pred_out = P_x_pred_0; + + x_est = x_pred_out; + 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; + P_x_pred_out = P_x_pred_0; + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + + +/* + * 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) +{ + // Compute number of sigma points + int nx = x_post.n_elem; + int np = 2 * nx + 1; + + float alpha = 0.001; + float kappa = 0.0; + float beta = 2.0; + + float lambda = std::pow(alpha,2.0)*(((float) nx) + kappa) - ((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)); + + // Propagate and evaluate sigma points + arma::mat Xi_fact = arma::zeros(nx,nx); + arma::mat Xi_post = arma::zeros(nx,np); + arma::mat Xi_pred = arma::zeros(nx,np); + + + Xi_post.col(0) = x_post; + 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_post.col(i) = 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+nx) = (*transition_fcn)(Xi_post.col(i+nx)); + } + + // Compute predicted mean + 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 + 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++) + { + 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; + + // Store predicted mean and error covariance + x_pred_out = x_pred; + P_x_pred_out = P_x_pred; +} + +/* + * 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) +{ + // Compute number of sigma points + int nx = x_pred.n_elem; + int nz = z_upd.n_elem; + int np = 2 * nx + 1; + + float alpha = 0.001; + float kappa = 0.0; + float beta = 2.0; + + float lambda = std::pow(alpha,2.0)*(((float) nx) + kappa) - ((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)); + + // Propagate and evaluate sigma points + arma::mat Xi_fact = arma::zeros(nx,nx); + arma::mat Xi_pred = arma::zeros(nx,np); + arma::mat Zi_pred = arma::zeros(nz,np); + + Xi_pred.col(0) = x_pred; + 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_pred.col(i) = 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+nx) = (*measurement_fcn)(Xi_pred.col(i+nx)); + } + + // Compute measurement mean + 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 + 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())); + 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_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; + + // Estimate cubature Kalman gain + arma::mat W_k = P_xz_pred*arma::inv(P_zz_pred); + + // Estimate and store the updated mean and error covariance + x_est = x_pred + W_k*(z_upd - z_pred); + 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 *****************/ diff --git a/src/algorithms/tracking/libs/nonlinear_tracking.h b/src/algorithms/tracking/libs/nonlinear_tracking.h index 6a0806e0e..f6a66a337 100644 --- a/src/algorithms/tracking/libs/nonlinear_tracking.h +++ b/src/algorithms/tracking/libs/nonlinear_tracking.h @@ -1,10 +1,12 @@ /*! - * \file cubature_filter.h - * \brief Interface of a library with Bayesian noise statistic estimation + * \file nonlinear_tracking.h + * \brief Interface of a library for nonlinear tracking algorithms * * Cubature_Filter implements the functionality of the Cubature Kalman * Filter, which uses multidimensional cubature rules to estimate the - * time evolution of a nonlinear system. + * time evolution of a nonlinear system. Unscented_filter implements + * an Unscented Kalman Filter which uses Unscented Transform rules to + * perform a similar estimation. * * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE * Transactions on Automatic Control, 54(6):1254–1269,2009. @@ -38,8 +40,8 @@ * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_CUBATURE_FILTER_H_ -#define GNSS_SDR_CUBATURE_FILTER_H_ +#ifndef GNSS_SDR_NONLINEAR_TRACKING_H_ +#define GNSS_SDR_NONLINEAR_TRACKING_H_ #include #include @@ -81,4 +83,33 @@ private: arma::mat P_x_est; }; +class Unscented_filter +{ +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(); + + // 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); + + // Getters + arma::mat get_x_pred() const; + arma::mat get_P_x_pred() const; + arma::mat get_x_est() const; + arma::mat get_P_x_est() const; + +private: + arma::vec x_pred_out; + arma::mat P_x_pred_out; + arma::vec x_est; + arma::mat P_x_est; +}; + #endif diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 2ace1a4b5..81b040f94 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -782,6 +782,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc ) if(${FILESYSTEM_FOUND}) target_compile_definitions(trk_test PRIVATE -DHAS_STD_FILESYSTEM=1) diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index d22149fee..f171f199a 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -100,6 +100,7 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc" #include "unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc" +#include "unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc" #include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc" #include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc" #include "unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc" diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc new file mode 100644 index 000000000..770086e3a --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc @@ -0,0 +1,157 @@ +/*! + * \file unscented_filter_test.cc + * \brief This file implements numerical accuracy test for the CKF library. + * \author Gerald LaMountain, 2019. gerald(at)ece.neu.edu + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "nonlinear_tracking.h" +#include +#include +#include + +#define UNSCENTED_TEST_N_TRIALS 10 +#define UNSCENTED_TEST_TOLERANCE 10 + +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 { + 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; +}; + +TEST(UnscentedFilterComputationTest, UnscentedFilterTest) +{ + Unscented_filter kf_unscented; + + arma::vec kf_x; + arma::mat kf_P_x; + + arma::vec kf_x_pre; + arma::mat kf_P_x_pre; + + arma::vec ukf_x_pre; + arma::mat ukf_P_x_pre; + + arma::vec kf_x_post; + arma::mat kf_P_x_post; + + arma::vec ukf_x_post; + arma::mat ukf_P_x_post; + + arma::mat kf_F; + arma::mat kf_H; + + arma::mat kf_Q; + arma::mat kf_R; + + arma::vec eta; + arma::vec nu; + + arma::vec kf_y; + arma::mat kf_P_y; + arma::mat kf_K; + + Model_Function* transition_function; + Model_Function* measurement_function; + + //--- Perform initializations ------------------------------ + + std::random_device r; + std::default_random_engine e1(r()); + std::normal_distribution normal_dist(0, 5); + std::uniform_real_distribution uniform_dist(0.1, 5.0); + + uint8_t nx = 0; + uint8_t ny = 0; + + for (uint16_t k = 0; k < UNSCENTED_TEST_N_TRIALS; k++) + { + nx = std::rand() % 5 + 1; + ny = std::rand() % 5 + 1; + + kf_x = arma::randn(nx,1); + + kf_P_x_post = 5.0 * arma::diagmat(arma::randu(nx,1)); + kf_x_post = arma::mvnrnd(kf_x, kf_P_x_post); + + kf_unscented.initialize(kf_x_post, kf_P_x_post); + + // Prediction Step + kf_F = arma::randu(nx,nx); + kf_Q = arma::diagmat(arma::randu(nx,1)); + + transition_function = new Transition_Model_UKF(kf_F); + arma::mat ttx = (*transition_function)(kf_x_post); + + kf_unscented.predict_sequential(kf_x_post,kf_P_x_post,transition_function,kf_Q); + + ukf_x_pre = kf_unscented.get_x_pred(); + ukf_P_x_pre = kf_unscented.get_P_x_pred(); + + kf_x_pre = kf_F * kf_x_post; + kf_P_x_pre = kf_F * kf_P_x_post * kf_F.t() + kf_Q; + + EXPECT_TRUE(arma::approx_equal(ukf_x_pre, kf_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 + kf_H = arma::randu(ny,nx); + kf_R = arma::diagmat(arma::randu(ny,1)); + + eta = arma::mvnrnd(arma::zeros(nx,1),kf_Q); + nu = arma::mvnrnd(arma::zeros(ny,1),kf_R); + + kf_y = kf_H*(kf_F*kf_x + eta) + nu; + + 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); + + ukf_x_post = kf_unscented.get_x_est(); + ukf_P_x_post = kf_unscented.get_P_x_est(); + + kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; + 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_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_P_x_post, kf_P_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE)); + + delete transition_function; + delete measurement_function; + } +}