From 8cc141341b289d20d2fbeb0444c59ff09b50d21f Mon Sep 17 00:00:00 2001 From: Gerald LaMountain Date: Fri, 17 May 2019 12:56:43 -0400 Subject: [PATCH] Add tracking lib for cubature kalman filter --- src/algorithms/tracking/libs/CMakeLists.txt | 2 + .../tracking/libs/cubature_filter.cc | 182 ++++++++++++++++++ .../tracking/libs/cubature_filter.h | 74 +++++++ 3 files changed, 258 insertions(+) create mode 100644 src/algorithms/tracking/libs/cubature_filter.cc create mode 100644 src/algorithms/tracking/libs/cubature_filter.h diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 63de5d9a7..5950f8ad1 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -33,6 +33,7 @@ set(TRACKING_LIB_SOURCES cpu_multicorrelator.cc cpu_multicorrelator_real_codes.cc cpu_multicorrelator_16sc.cc + cubature_filter.cc lock_detectors.cc tcp_communication.cc tcp_packet_data.cc @@ -50,6 +51,7 @@ set(TRACKING_LIB_HEADERS cpu_multicorrelator.h cpu_multicorrelator_real_codes.h cpu_multicorrelator_16sc.h + cubature_filter.h lock_detectors.h tcp_communication.h tcp_packet_data.h diff --git a/src/algorithms/tracking/libs/cubature_filter.cc b/src/algorithms/tracking/libs/cubature_filter.cc new file mode 100644 index 000000000..19dcdbcab --- /dev/null +++ b/src/algorithms/tracking/libs/cubature_filter.cc @@ -0,0 +1,182 @@ +/*! + * \file cubature_filter.cc + * \brief Interface of a library with Bayesian noise statistic estimation + * + * Cubature_Filter implements the functionality of the Cubature Kalman + * Filter, which uses multidimensional cubature rules to estimate the + * time evolution of a nonlinear system. + * + * [1] TODO: Refs + * + * \authors + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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" + + +Cubature_filter::Cubature_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; +} + +Cubature_filter::Cubature_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; +} + +Cubature_filter::Cubature_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; +} + +Cubature_filter::~Cubature_filter() = default; + +void Cubature_filter::init(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 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) +{ + // Compute number of cubature points + int nx = x_post.n_elem; + int np = 2 * 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); + + // Propagate and evaluate cubature points + arma::vec Xi_post; + arma::vec Xi_pred; + for (int32_t i = 0; i < np; i++) + { + Xi_post = Sm_post*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + 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(); + } + + // Estimate predicted state 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 + x_pred_out = x_pred; + P_x_pred_out = P_x_pred; +} + +/* + * 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) +{ + // Compute number of cubature points + int nx = x_pred.n_elem; + int nz = z_upd.n_elem; + int np = 2 * nx; + + // Evaluate predicted measurement and covariances + arma::mat z_pred = arma::zeros(nx,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); + + // Propagate and evaluate cubature points + arma::vec Xi_pred; + arma::vec Zi_pred; + for (int32_t i = 0; i < np; i++) + { + Xi_pred = Sm_pred*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + 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; + P_xz_pred = P_xz_pred / ((float) np) - x_pred*z_pred.t(); + + // Estimate cubature Kalman gain + arma::mat W_k = P_xz_pred*arma::inv(P_zz_pred); + + // Estimate and store the updated state 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 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; +} diff --git a/src/algorithms/tracking/libs/cubature_filter.h b/src/algorithms/tracking/libs/cubature_filter.h new file mode 100644 index 000000000..6c3c78968 --- /dev/null +++ b/src/algorithms/tracking/libs/cubature_filter.h @@ -0,0 +1,74 @@ +/*! + * \file cubature_filter.h + * \brief Interface of a library with Bayesian noise statistic estimation + * + * Cubature_Filter implements the functionality of the Cubature Kalman + * Filter, which uses multidimensional cubature rules to estimate the + * time evolution of a nonlinear system. + * + * [1] TODO: Refs + * + * \authors + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_CUBATURE_FILTER_H_ +#define GNSS_SDR_CUBATURE_FILTER_H_ + +#include +#include + +class Cubature_filter +{ +public: + // Constructors and destructors + Cubature_filter(); + Cubature_filter(int nx); + Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0); + ~Cubature_filter(); + + // Reinitialization function + void init(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); + + // 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