GNSS-SDR  0.0.19
An Open Source GNSS Software Defined Receiver
nonlinear_tracking.h
Go to the documentation of this file.
1 /*!
2  * \file nonlinear_tracking.h
3  * \brief Interface of a library for nonlinear tracking algorithms
4  *
5  * CubatureFilter implements the functionality of the Cubature Kalman
6  * Filter, which uses multidimensional cubature rules to estimate the
7  * time evolution of a nonlinear system. UnscentedFilter implements
8  * an Unscented Kalman Filter which uses Unscented Transform rules to
9  * perform a similar estimation.
10  *
11  * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
12  * Transactions on Automatic Control, 54(6):1254–1269,2009.
13  *
14  * \authors <ul>
15  * <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
16  * <li> Jordi Vila-Valls 2019. jvila(at)cttc.es
17  * </ul>
18  * -----------------------------------------------------------------------------
19  *
20  * GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
21  * This file is part of GNSS-SDR.
22  *
23  * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
24  * SPDX-License-Identifier: GPL-3.0-or-later
25  *
26  * -----------------------------------------------------------------------------
27  */
28 
29 #ifndef GNSS_SDR_NONLINEAR_TRACKING_H
30 #define GNSS_SDR_NONLINEAR_TRACKING_H
31 
32 #if ARMA_NO_BOUND_CHECKING
33 #define ARMA_NO_DEBUG 1
34 #endif
35 
36 #include <armadillo>
37 #include <gnuradio/gr_complex.h>
38 
39 /** \addtogroup Tracking
40  * \{ */
41 /** \addtogroup Tracking_libs
42  * \{ */
43 
44 
45 // Abstract model function
46 class ModelFunction
47 {
48 public:
49  ModelFunction(){};
50  virtual arma::vec operator()(const arma::vec& input) = 0;
51  virtual ~ModelFunction() = default;
52 };
53 
54 class CubatureFilter
55 {
56 public:
57  // Constructors and destructors
58  CubatureFilter();
59  explicit CubatureFilter(int nx);
60  CubatureFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
61  ~CubatureFilter() = default;
62 
63  // Reinitialization function
64  void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
65 
66  // Prediction and estimation
67  void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, const arma::mat& noise_covariance);
68  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);
69 
70  // Getters
71  arma::mat get_x_pred() const;
72  arma::mat get_P_x_pred() const;
73  arma::mat get_x_est() const;
74  arma::mat get_P_x_est() const;
75 
76 private:
77  arma::vec x_pred_out;
78  arma::mat P_x_pred_out;
79  arma::vec x_est;
80  arma::mat P_x_est;
81 };
82 
83 class UnscentedFilter
84 {
85 public:
86  // Constructors and destructors
87  UnscentedFilter();
88  explicit UnscentedFilter(int nx);
89  UnscentedFilter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
90  ~UnscentedFilter() = default;
91 
92  // Reinitialization function
93  void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
94 
95  // Prediction and estimation
96  void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, const arma::mat& noise_covariance);
97  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);
98 
99  // Getters
100  arma::mat get_x_pred() const;
101  arma::mat get_P_x_pred() const;
102  arma::mat get_x_est() const;
103  arma::mat get_P_x_est() const;
104 
105 private:
106  arma::vec x_pred_out;
107  arma::mat P_x_pred_out;
108  arma::vec x_est;
109  arma::mat P_x_est;
110 };
111 
112 
113 /** \} */
114 /** \} */
115 #endif // GNSS_SDR_NONLINEAR_TRACKING_H
Generated by   doxygen 1.8.14