mirror of https://github.com/gnss-sdr/gnss-sdr
116 lines
3.5 KiB
C++
116 lines
3.5 KiB
C++
/*!
|
||
* \file nonlinear_tracking.h
|
||
* \brief Interface of a library for nonlinear tracking algorithms
|
||
*
|
||
* CubatureFilter implements the functionality of the Cubature Kalman
|
||
* Filter, which uses multidimensional cubature rules to estimate the
|
||
* time evolution of a nonlinear system. UnscentedFilter 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.
|
||
*
|
||
* \authors <ul>
|
||
* <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
|
||
* <li> Jordi Vila-Valls 2019. jvila(at)cttc.es
|
||
* </ul>
|
||
* -----------------------------------------------------------------------------
|
||
*
|
||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||
* This file is part of GNSS-SDR.
|
||
*
|
||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||
*
|
||
* -----------------------------------------------------------------------------
|
||
*/
|
||
|
||
#ifndef GNSS_SDR_NONLINEAR_TRACKING_H
|
||
#define GNSS_SDR_NONLINEAR_TRACKING_H
|
||
|
||
#if ARMA_NO_BOUND_CHECKING
|
||
#define ARMA_NO_DEBUG 1
|
||
#endif
|
||
|
||
#include <armadillo>
|
||
#include <gnuradio/gr_complex.h>
|
||
|
||
/** \addtogroup Tracking
|
||
* \{ */
|
||
/** \addtogroup Tracking_libs
|
||
* \{ */
|
||
|
||
|
||
// Abstract model function
|
||
class ModelFunction
|
||
{
|
||
public:
|
||
ModelFunction(){};
|
||
virtual arma::vec operator()(const arma::vec& input) = 0;
|
||
virtual ~ModelFunction() = default;
|
||
};
|
||
|
||
class CubatureFilter
|
||
{
|
||
public:
|
||
// Constructors and destructors
|
||
CubatureFilter();
|
||
explicit CubatureFilter(int nx);
|
||
CubatureFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
|
||
~CubatureFilter() = default;
|
||
|
||
// 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, 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;
|
||
|
||
private:
|
||
arma::vec x_pred_out;
|
||
arma::mat P_x_pred_out;
|
||
arma::vec x_est;
|
||
arma::mat P_x_est;
|
||
};
|
||
|
||
class UnscentedFilter
|
||
{
|
||
public:
|
||
// Constructors and destructors
|
||
UnscentedFilter();
|
||
explicit UnscentedFilter(int nx);
|
||
UnscentedFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
|
||
~UnscentedFilter() = default;
|
||
|
||
// 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, 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;
|
||
|
||
private:
|
||
arma::vec x_pred_out;
|
||
arma::mat P_x_pred_out;
|
||
arma::vec x_est;
|
||
arma::mat P_x_est;
|
||
};
|
||
|
||
|
||
/** \} */
|
||
/** \} */
|
||
#endif // GNSS_SDR_NONLINEAR_TRACKING_H
|