diff --git a/src/algorithms/tracking/libs/cubature_filter.cc b/src/algorithms/tracking/libs/cubature_filter.cc index 19dcdbcab..d3d32350e 100644 --- a/src/algorithms/tracking/libs/cubature_filter.cc +++ b/src/algorithms/tracking/libs/cubature_filter.cc @@ -6,7 +6,8 @@ * Filter, which uses multidimensional cubature rules to estimate the * time evolution of a nonlinear system. * - * [1] TODO: Refs + * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE + * Transactions on Automatic Control, 54(6):1254–1269,2009. * * \authors * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -70,7 +71,7 @@ Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x Cubature_filter::~Cubature_filter() = default; -void Cubature_filter::init(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; P_x_pred_out = P_x_pred_0; @@ -83,29 +84,33 @@ void Cubature_filter::init(const arma::mat& x_pred_0, const arma::mat& P_x_pred_ /* * 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, arma::vec (*transition_fcn)(const arma::mat&), const arma::mat& noise_covariance) +void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, const arma::mat& noise_covariance) { // Compute number of cubature points int nx = x_post.n_elem; int np = 2 * nx; + // Generator Matrix + arma::mat gen_one = arma::join_horiz(arma::eye(nx,nx),-1.0*arma::eye(nx,nx)); + // Initialize predicted mean and covariance arma::vec x_pred = arma::zeros(nx,1); arma::mat P_x_pred = arma::zeros(nx,nx); // Factorize posterior covariance - arma::mat Sm_post = arma::chol(P_x_post); + arma::mat Sm_post = arma::chol(P_x_post, "lower"); // Propagate and evaluate cubature points arma::vec Xi_post; arma::vec Xi_pred; - for (int32_t i = 0; i < np; i++) + + for (uint8_t i = 0; i < np; i++) { - Xi_post = Sm_post*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + x_post; + Xi_post = Sm_post * (std::sqrt(((float) np) / 2.0) * gen_one.col(i)) + x_post; Xi_pred = (*transition_fcn)(Xi_post); - - x_pred = x_post + Xi_pred; - P_x_pred = P_x_post + Xi_pred*Xi_pred.t(); + + x_pred = x_pred + Xi_pred; + P_x_pred = P_x_pred + Xi_pred*Xi_pred.t(); } // Estimate predicted state and error covariance @@ -120,34 +125,37 @@ void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::ma /* * 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, arma::vec (*measurement_fcn)(const arma::mat&), 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, ModelFunction* measurement_fcn, const arma::mat& noise_covariance) { // Compute number of cubature points int nx = x_pred.n_elem; int nz = z_upd.n_elem; int np = 2 * nx; + // Generator Matrix + arma::mat gen_one = arma::join_horiz(arma::eye(nx,nx),-1.0*arma::eye(nx,nx)); + // Evaluate predicted measurement and covariances - arma::mat z_pred = arma::zeros(nx,1); + 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); // Factorize predicted covariance - arma::mat Sm_pred = arma::chol(P_x_pred); + arma::mat Sm_pred = arma::chol(P_x_pred, "lower"); // Propagate and evaluate cubature points arma::vec Xi_pred; arma::vec Zi_pred; - for (int32_t i = 0; i < np; i++) + for (uint8_t i = 0; i < np; i++) { - Xi_pred = Sm_pred*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + x_pred; + Xi_pred = Sm_pred * (std::sqrt(((float) np) / 2.0) * gen_one.col(i)) + x_pred; Zi_pred = (*measurement_fcn)(Xi_pred); z_pred = z_pred + Zi_pred; P_zz_pred = P_zz_pred + Zi_pred*Zi_pred.t(); P_xz_pred = P_xz_pred + Xi_pred*Zi_pred.t(); } - + // Estimate measurement covariance and cross covariances z_pred = z_pred / ((float) np); P_zz_pred = P_zz_pred / ((float) np) - z_pred*z_pred.t() + noise_covariance; @@ -180,3 +188,8 @@ arma::mat Cubature_filter::get_P_x_est() const { return P_x_est; } + +double Cubature_filter::func_number(double number, TestModel* func) +{ + return (*func)(number); +} diff --git a/src/algorithms/tracking/libs/cubature_filter.h b/src/algorithms/tracking/libs/cubature_filter.h index 6c3c78968..f4b31c43a 100644 --- a/src/algorithms/tracking/libs/cubature_filter.h +++ b/src/algorithms/tracking/libs/cubature_filter.h @@ -6,7 +6,8 @@ * Filter, which uses multidimensional cubature rules to estimate the * time evolution of a nonlinear system. * - * [1] TODO: Refs + * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE + * Transactions on Automatic Control, 54(6):1254–1269,2009. * * \authors * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -43,6 +44,22 @@ #include #include +// Abstract model function +class ModelFunction{ + public: + ModelFunction() {}; + virtual arma::vec operator() (arma::vec input) = 0; + virtual ~ModelFunction() = 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 { public: @@ -53,17 +70,21 @@ public: ~Cubature_filter(); // Reinitialization function - void init(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 - void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, arma::vec (*transition_fcn)(const arma::mat&), const arma::mat& noise_covariance); - void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, arma::vec (*measurement_fcn)(const arma::mat&), const arma::mat& noise_covariance); + void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* 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); // 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; + + //Test-dev + double func_number(double number, TestModel* func); + private: arma::vec x_pred_out; arma::mat P_x_pred_out; diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 3053df1d4..51e67077f 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -755,6 +755,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc ${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 ) target_link_libraries(trk_test diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 05df98d35..d22149fee 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -99,6 +99,7 @@ DECLARE_string(log_dir); #endif #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/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/bayesian_estimation_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc index dd7d0398b..6fa5f1da0 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc @@ -1,7 +1,7 @@ /*! - * \file bayesian_estimation_positivity_test.cc - * \brief This file implements timing tests for the Bayesian covariance estimator - * \author Gerald LaMountain, 20168. gerald(at)ece.neu.edu + * \file bayesian_estimation_test.cc + * \brief This file implements feasability test for the BCE library. + * \author Gerald LaMountain, 2018. gerald(at)ece.neu.edu * * * ------------------------------------------------------------------------- diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc new file mode 100644 index 000000000..e73bb1d45 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc @@ -0,0 +1,156 @@ +/*! + * \file cubature_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 "cubature_filter.h" +#include +#include +#include + +#define CUBATURE_TEST_N_TRIALS 1000 + +class TransitionModel : public ModelFunction { + public: + TransitionModel(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 MeasurementModel : public ModelFunction { + public: + MeasurementModel(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(CubatureFilterComputationTest, CubatureFilterTest) +{ + Cubature_filter kf_cubature; + + arma::vec kf_x; + arma::mat kf_P_x; + + arma::vec kf_x_pre; + arma::mat kf_P_x_pre; + + arma::vec ckf_x_pre; + arma::mat ckf_P_x_pre; + + arma::vec kf_x_post; + arma::mat kf_P_x_post; + + arma::vec ckf_x_post; + arma::mat ckf_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; + + ModelFunction* transition_function; + ModelFunction* 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 < CUBATURE_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_cubature.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 TransitionModel(kf_F); + arma::mat ttx = (*transition_function)(kf_x_post); + + kf_cubature.predict_sequential(kf_x_post,kf_P_x_post,transition_function,kf_Q); + + ckf_x_pre = kf_cubature.get_x_pred(); + ckf_P_x_pre = kf_cubature.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(ckf_x_pre, kf_x_pre, "absdiff", 0.01)); + EXPECT_TRUE(arma::approx_equal(ckf_P_x_pre, kf_P_x_pre, "absdiff", 0.01)); + + // 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 MeasurementModel(kf_H); + 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_P_x_post = kf_cubature.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(ckf_x_post, kf_x_post, "absdiff", 0.01)); + EXPECT_TRUE(arma::approx_equal(ckf_P_x_post, kf_P_x_post, "absdiff", 0.01)); + + delete transition_function; + delete measurement_function; + } +}