2019-05-17 16:56:43 +00:00
|
|
|
|
/*!
|
2019-06-13 19:42:52 +00:00
|
|
|
|
* \file nonlinear_tracking.h
|
|
|
|
|
* \brief Interface of a library for nonlinear tracking algorithms
|
2019-05-17 16:56:43 +00:00
|
|
|
|
*
|
2019-06-16 12:54:41 +00:00
|
|
|
|
* CubatureFilter implements the functionality of the Cubature Kalman
|
2019-05-17 16:56:43 +00:00
|
|
|
|
* Filter, which uses multidimensional cubature rules to estimate the
|
2019-06-16 12:54:41 +00:00
|
|
|
|
* time evolution of a nonlinear system. UnscentedFilter implements
|
2019-06-13 19:42:52 +00:00
|
|
|
|
* an Unscented Kalman Filter which uses Unscented Transform rules to
|
|
|
|
|
* perform a similar estimation.
|
2019-05-17 16:56:43 +00:00
|
|
|
|
*
|
2019-06-13 15:37:32 +00:00
|
|
|
|
* [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
|
2019-06-12 18:51:19 +00:00
|
|
|
|
* Transactions on Automatic Control, 54(6):1254–1269,2009.
|
2019-05-17 16:56:43 +00:00
|
|
|
|
*
|
|
|
|
|
* \authors <ul>
|
|
|
|
|
* <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
|
|
|
|
|
* <li> Jordi Vila-Valls 2019. jvila(at)cttc.es
|
|
|
|
|
* </ul>
|
2020-07-28 14:57:15 +00:00
|
|
|
|
* -----------------------------------------------------------------------------
|
2019-05-17 16:56:43 +00:00
|
|
|
|
*
|
2020-12-30 12:35:06 +00:00
|
|
|
|
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
2019-05-17 16:56:43 +00:00
|
|
|
|
* This file is part of GNSS-SDR.
|
|
|
|
|
*
|
2020-12-30 12:35:06 +00:00
|
|
|
|
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
2020-02-08 00:20:02 +00:00
|
|
|
|
* SPDX-License-Identifier: GPL-3.0-or-later
|
2019-05-17 16:56:43 +00:00
|
|
|
|
*
|
2020-07-28 14:57:15 +00:00
|
|
|
|
* -----------------------------------------------------------------------------
|
2019-05-17 16:56:43 +00:00
|
|
|
|
*/
|
|
|
|
|
|
2020-02-08 09:10:46 +00:00
|
|
|
|
#ifndef GNSS_SDR_NONLINEAR_TRACKING_H
|
|
|
|
|
#define GNSS_SDR_NONLINEAR_TRACKING_H
|
2019-05-17 16:56:43 +00:00
|
|
|
|
|
2019-07-29 19:22:44 +00:00
|
|
|
|
#if ARMA_NO_BOUND_CHECKING
|
|
|
|
|
#define ARMA_NO_DEBUG 1
|
|
|
|
|
#endif
|
|
|
|
|
|
2019-05-17 16:56:43 +00:00
|
|
|
|
#include <armadillo>
|
|
|
|
|
#include <gnuradio/gr_complex.h>
|
|
|
|
|
|
2020-11-01 12:37:19 +00:00
|
|
|
|
/** \addtogroup Tracking
|
|
|
|
|
* \{ */
|
|
|
|
|
/** \addtogroup Tracking_libs
|
|
|
|
|
* \{ */
|
|
|
|
|
|
|
|
|
|
|
2019-06-12 18:51:19 +00:00
|
|
|
|
// Abstract model function
|
2019-06-16 12:54:41 +00:00
|
|
|
|
class ModelFunction
|
2019-06-14 08:21:26 +00:00
|
|
|
|
{
|
|
|
|
|
public:
|
2019-06-16 12:54:41 +00:00
|
|
|
|
ModelFunction(){};
|
|
|
|
|
virtual arma::vec operator()(const arma::vec& input) = 0;
|
|
|
|
|
virtual ~ModelFunction() = default;
|
2019-06-12 18:51:19 +00:00
|
|
|
|
};
|
|
|
|
|
|
2019-06-16 12:54:41 +00:00
|
|
|
|
class CubatureFilter
|
2019-05-17 16:56:43 +00:00
|
|
|
|
{
|
|
|
|
|
public:
|
|
|
|
|
// Constructors and destructors
|
2019-06-16 12:54:41 +00:00
|
|
|
|
CubatureFilter();
|
2019-08-23 19:25:44 +00:00
|
|
|
|
explicit CubatureFilter(int nx);
|
2019-06-16 12:54:41 +00:00
|
|
|
|
CubatureFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
|
2019-07-21 17:32:52 +00:00
|
|
|
|
~CubatureFilter() = default;
|
2019-05-17 16:56:43 +00:00
|
|
|
|
|
|
|
|
|
// Reinitialization function
|
2019-06-12 18:51:19 +00:00
|
|
|
|
void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
|
2019-05-17 16:56:43 +00:00
|
|
|
|
|
|
|
|
|
// Prediction and estimation
|
2019-06-16 12:54:41 +00:00
|
|
|
|
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);
|
2019-05-17 16:56:43 +00:00
|
|
|
|
|
|
|
|
|
// 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;
|
2019-06-12 18:51:19 +00:00
|
|
|
|
|
2019-05-17 16:56:43 +00:00
|
|
|
|
private:
|
|
|
|
|
arma::vec x_pred_out;
|
|
|
|
|
arma::mat P_x_pred_out;
|
|
|
|
|
arma::vec x_est;
|
|
|
|
|
arma::mat P_x_est;
|
|
|
|
|
};
|
|
|
|
|
|
2019-06-16 12:54:41 +00:00
|
|
|
|
class UnscentedFilter
|
2019-06-13 19:42:52 +00:00
|
|
|
|
{
|
|
|
|
|
public:
|
|
|
|
|
// Constructors and destructors
|
2019-06-16 12:54:41 +00:00
|
|
|
|
UnscentedFilter();
|
2019-08-23 19:25:44 +00:00
|
|
|
|
explicit UnscentedFilter(int nx);
|
2019-06-16 12:54:41 +00:00
|
|
|
|
UnscentedFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
|
2019-07-21 17:32:52 +00:00
|
|
|
|
~UnscentedFilter() = default;
|
2019-06-13 19:42:52 +00:00
|
|
|
|
|
|
|
|
|
// Reinitialization function
|
|
|
|
|
void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
|
|
|
|
|
|
|
|
|
|
// Prediction and estimation
|
2019-06-16 12:54:41 +00:00
|
|
|
|
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);
|
2019-06-13 19:42:52 +00:00
|
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
|
};
|
|
|
|
|
|
2020-11-01 12:37:19 +00:00
|
|
|
|
|
|
|
|
|
/** \} */
|
|
|
|
|
/** \} */
|
|
|
|
|
#endif // GNSS_SDR_NONLINEAR_TRACKING_H
|