yat/statistics/KalmanFilter.h

Code
Comments
Other
Rev Date Author Line
4287 30 Jan 23 peter 1 #ifndef theplu_kalman_filter
4287 30 Jan 23 peter 2 #define theplu_kalman_filter
4287 30 Jan 23 peter 3
4287 30 Jan 23 peter 4 // $Id$
4287 30 Jan 23 peter 5
4287 30 Jan 23 peter 6 /*
4287 30 Jan 23 peter 7   Copyright (C) 2023 Peter Johansson
4287 30 Jan 23 peter 8
4287 30 Jan 23 peter 9   This file is part of the yat library, https://dev.thep.lu.se/yat
4287 30 Jan 23 peter 10
4287 30 Jan 23 peter 11   The yat library is free software; you can redistribute it and/or
4287 30 Jan 23 peter 12   modify it under the terms of the GNU General Public License as
4287 30 Jan 23 peter 13   published by the Free Software Foundation; either version 3 of the
4287 30 Jan 23 peter 14   License, or (at your option) any later version.
4287 30 Jan 23 peter 15
4287 30 Jan 23 peter 16   The yat library is distributed in the hope that it will be useful,
4287 30 Jan 23 peter 17   but WITHOUT ANY WARRANTY; without even the implied warranty of
4287 30 Jan 23 peter 18   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
4287 30 Jan 23 peter 19   General Public License for more details.
4287 30 Jan 23 peter 20
4287 30 Jan 23 peter 21   You should have received a copy of the GNU General Public License
4287 30 Jan 23 peter 22   along with yat. If not, see <https://www.gnu.org/licenses/>.
4287 30 Jan 23 peter 23 */
4287 30 Jan 23 peter 24
4287 30 Jan 23 peter 25 #include <yat/utility/DiagonalMatrix.h>
4287 30 Jan 23 peter 26 #include <yat/utility/Matrix.h>
4287 30 Jan 23 peter 27 #include <yat/utility/Vector.h>
4287 30 Jan 23 peter 28
4287 30 Jan 23 peter 29 namespace theplu {
4287 30 Jan 23 peter 30 namespace yat {
4287 30 Jan 23 peter 31 namespace statistics {
4287 30 Jan 23 peter 32
4287 30 Jan 23 peter 33   /**
4287 30 Jan 23 peter 34      Data are modeled in discrete time as \f$ x_t = F_t x_{t-1} + W_t
4287 30 Jan 23 peter 35      \f$, where \a x is the state variable, \a F describe the state
4287 30 Jan 23 peter 36      transition, and \a W is the process noise.
4287 30 Jan 23 peter 37
4287 30 Jan 23 peter 38      The state variable is not observed per se, but the observables,
4287 30 Jan 23 peter 39      \a z, are observed via \f$ z_t = H_tx_t + V_t \f$, where \a H is
4287 30 Jan 23 peter 40      a matrix describing the relation between state variable and
4287 30 Jan 23 peter 41      observables and \a V is the observation noise.
4287 30 Jan 23 peter 42
4287 30 Jan 23 peter 43      \since New in yat 0.21
4287 30 Jan 23 peter 44    */
4287 30 Jan 23 peter 45   class KalmanFilter
4287 30 Jan 23 peter 46   {
4287 30 Jan 23 peter 47   public:
4287 30 Jan 23 peter 48     /**
4287 30 Jan 23 peter 49        \param x initial value of state variable
4287 30 Jan 23 peter 50        \param P covariance of state variable
4287 30 Jan 23 peter 51      */
4287 30 Jan 23 peter 52     KalmanFilter(const utility::Vector& x, const utility::Matrix& P);
4287 30 Jan 23 peter 53
4287 30 Jan 23 peter 54     /**
4287 30 Jan 23 peter 55        \brief Predict new state
4287 30 Jan 23 peter 56
4287 30 Jan 23 peter 57        The predicted state is calculated as
4287 30 Jan 23 peter 58
4287 30 Jan 23 peter 59        \f$ \hat{x} = F_t x_{t-1} \f$
4287 30 Jan 23 peter 60
4287 30 Jan 23 peter 61        and the covariance of the state variable
4287 30 Jan 23 peter 62
4287 30 Jan 23 peter 63        \f$ \hat{P} = F_t P_{t-1} F_t^T + Q_t \f$
4287 30 Jan 23 peter 64
4287 30 Jan 23 peter 65        \param F state transition model
4287 30 Jan 23 peter 66        \param Q process noise
4287 30 Jan 23 peter 67
4287 30 Jan 23 peter 68        \return predicted state, xhat.
4287 30 Jan 23 peter 69
4287 30 Jan 23 peter 70        \see xhat() and Phat()
4287 30 Jan 23 peter 71      */
4287 30 Jan 23 peter 72     const utility::Vector&
4287 30 Jan 23 peter 73     predict(const utility::MatrixBase& F, const utility::MatrixBase& Q);
4287 30 Jan 23 peter 74
4287 30 Jan 23 peter 75     /**
4287 30 Jan 23 peter 76        \brief update the state from an observation, z.
4287 30 Jan 23 peter 77
4287 30 Jan 23 peter 78        It is assumed that predict() has been called so predicted values,
4287 30 Jan 23 peter 79        \f$ \hat{x} \f$ and \f$ \hat{P} \f$, have been updated.
4287 30 Jan 23 peter 80
4287 30 Jan 23 peter 81        The state variable is updated as
4287 30 Jan 23 peter 82
4287 30 Jan 23 peter 83        \f$ x_t = \hat{x} + K_t \hat{y}_t \f$
4287 30 Jan 23 peter 84
4287 30 Jan 23 peter 85        and covariance as
4287 30 Jan 23 peter 86
4287 30 Jan 23 peter 87        \f$ P_t = \left( I - K_t H_t\right) \hat{P}_t \f$
4287 30 Jan 23 peter 88
4287 30 Jan 23 peter 89        where
4287 30 Jan 23 peter 90
4287 30 Jan 23 peter 91        \f$ K_t = \hat{P}_t H_t S_t^{-1} \f$
4287 30 Jan 23 peter 92
4287 30 Jan 23 peter 93        \f$ S_t = H_t \hat{P}_t H_t^T + R_t \f$
4287 30 Jan 23 peter 94
4287 30 Jan 23 peter 95        \f$ \hat{y}_t = z_t - H_t \hat{x}_t \f$
4287 30 Jan 23 peter 96
4287 30 Jan 23 peter 97        \param z observed values
4287 30 Jan 23 peter 98        \param H observation model
4287 30 Jan 23 peter 99        \param R observation noise
4287 30 Jan 23 peter 100
4287 30 Jan 23 peter 101        \return estimated state variable, x.
4287 30 Jan 23 peter 102      */
4287 30 Jan 23 peter 103     const utility::Vector&
4287 30 Jan 23 peter 104     update(const utility::VectorBase& z, const utility::MatrixBase& H,
4287 30 Jan 23 peter 105            const utility::MatrixBase& R);
4287 30 Jan 23 peter 106
4287 30 Jan 23 peter 107     /**
4287 30 Jan 23 peter 108        Equivalent with calling predict() followed by update().
4287 30 Jan 23 peter 109      */
4287 30 Jan 23 peter 110     void operator()(const utility::VectorBase& z,
4287 30 Jan 23 peter 111                     const utility::MatrixBase& F,
4287 30 Jan 23 peter 112                     const utility::MatrixBase& Q,
4287 30 Jan 23 peter 113                     const utility::MatrixBase& H,
4287 30 Jan 23 peter 114                     const utility::MatrixBase& R);
4287 30 Jan 23 peter 115     /**
4287 30 Jan 23 peter 116        \brief logarithm of marginal likelihood
4287 30 Jan 23 peter 117
4287 30 Jan 23 peter 118        The marginal log likelihood is calculated as
4287 30 Jan 23 peter 119
4287 30 Jan 23 peter 120        \f$ -0.5 \left(\hat{y}^TS^{-1}\hat{y} + \ln |S| + d \ln 2\pi \right) \f$
4287 30 Jan 23 peter 121        where \a d is the dimension of the model.
4287 30 Jan 23 peter 122      */
4287 30 Jan 23 peter 123     double logL(void) const;
4287 30 Jan 23 peter 124
4287 30 Jan 23 peter 125     /**
4287 30 Jan 23 peter 126        \brief Kalman gain
4287 30 Jan 23 peter 127
4287 30 Jan 23 peter 128        K is calculated in update() as
4287 30 Jan 23 peter 129
4287 30 Jan 23 peter 130        \f$ K = \hat{P} H^T S^{-1} \f$
4287 30 Jan 23 peter 131
4287 30 Jan 23 peter 132        \return K, Kalman gain
4287 30 Jan 23 peter 133      */
4287 30 Jan 23 peter 134     const utility::Matrix& K(void) const;
4287 30 Jan 23 peter 135
4287 30 Jan 23 peter 136     /**
4287 30 Jan 23 peter 137        \brief Predicted estimate
4287 30 Jan 23 peter 138
4287 30 Jan 23 peter 139        Estimate is calculated in predict() as
4287 30 Jan 23 peter 140        \f$ x_t = \hat{x} + K_t \hat{y}_t \f$
4287 30 Jan 23 peter 141     */
4287 30 Jan 23 peter 142     const utility::Vector& xhat(void) const;
4287 30 Jan 23 peter 143
4287 30 Jan 23 peter 144     /**
4287 30 Jan 23 peter 145        \brief Predicted estimate covariance
4287 30 Jan 23 peter 146
4287 30 Jan 23 peter 147        Covariance is calculated in predict() as
4287 30 Jan 23 peter 148        \f$ \hat{P} = F P F^T + Q \f$
4287 30 Jan 23 peter 149     */
4287 30 Jan 23 peter 150     const utility::Matrix& Phat(void) const;
4287 30 Jan 23 peter 151
4287 30 Jan 23 peter 152     /**
4287 30 Jan 23 peter 153        \brief Covariance of pre-fit residual
4287 30 Jan 23 peter 154
4287 30 Jan 23 peter 155        Covariance is calculated in evaluate() as
4287 30 Jan 23 peter 156
4287 30 Jan 23 peter 157        \f$ S = H \hat{P} H^T + R \f$
4287 30 Jan 23 peter 158      */
4287 30 Jan 23 peter 159     const utility::Matrix& S(void) const;
4287 30 Jan 23 peter 160
4287 30 Jan 23 peter 161     /**
4287 30 Jan 23 peter 162        \brief post-fit residual
4287 30 Jan 23 peter 163
4287 30 Jan 23 peter 164        It is calculated in evaluate() as
4287 30 Jan 23 peter 165
4287 30 Jan 23 peter 166        \f$ y = z - Hx \f$
4287 30 Jan 23 peter 167     */
4287 30 Jan 23 peter 168     const utility::Vector& y(void) const;
4287 30 Jan 23 peter 169
4287 30 Jan 23 peter 170     /**
4287 30 Jan 23 peter 171        \brief prefit residual
4287 30 Jan 23 peter 172
4287 30 Jan 23 peter 173        It is calculated in evaluate() as
4287 30 Jan 23 peter 174
4287 30 Jan 23 peter 175        \f$ \hat{y} = z - H\hat{x} \f$
4287 30 Jan 23 peter 176      */
4287 30 Jan 23 peter 177     const utility::Vector& yhat(void) const;
4287 30 Jan 23 peter 178   private:
4287 30 Jan 23 peter 179     utility::Vector x_;
4287 30 Jan 23 peter 180     utility::Matrix P_;
4287 30 Jan 23 peter 181     utility::DiagonalMatrix I_;
4287 30 Jan 23 peter 182     utility::Vector xhat_;
4287 30 Jan 23 peter 183     utility::Matrix Phat_;
4287 30 Jan 23 peter 184     utility::Vector yhat_;
4287 30 Jan 23 peter 185     utility::Matrix S_;
4287 30 Jan 23 peter 186     double ln_det_S_;
4287 30 Jan 23 peter 187     utility::Matrix S_inverse_;
4287 30 Jan 23 peter 188     utility::Matrix K_;
4287 30 Jan 23 peter 189     utility::Vector y_;
4287 30 Jan 23 peter 190   };
4287 30 Jan 23 peter 191
4287 30 Jan 23 peter 192 }}} // end of namespaces statistics, yat, and theplu
4287 30 Jan 23 peter 193
4287 30 Jan 23 peter 194 #endif

Generated on Sat Oct 14 02:23:45 2023 (UTC) by svndigest 0.11.2pre (dev build 1672)