yat/statistics/KalmanFilter.cc

Code
Comments
Other
Rev Date Author Line
4287 30 Jan 23 peter 1 // $Id$
4287 30 Jan 23 peter 2
4287 30 Jan 23 peter 3 /*
4287 30 Jan 23 peter 4   Copyright (C) 2023 Peter Johansson
4287 30 Jan 23 peter 5
4287 30 Jan 23 peter 6   This file is part of the yat library, https://dev.thep.lu.se/yat
4287 30 Jan 23 peter 7
4287 30 Jan 23 peter 8   The yat library is free software; you can redistribute it and/or
4287 30 Jan 23 peter 9   modify it under the terms of the GNU General Public License as
4287 30 Jan 23 peter 10   published by the Free Software Foundation; either version 3 of the
4287 30 Jan 23 peter 11   License, or (at your option) any later version.
4287 30 Jan 23 peter 12
4287 30 Jan 23 peter 13   The yat library is distributed in the hope that it will be useful,
4287 30 Jan 23 peter 14   but WITHOUT ANY WARRANTY; without even the implied warranty of
4287 30 Jan 23 peter 15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
4287 30 Jan 23 peter 16   General Public License for more details.
4287 30 Jan 23 peter 17
4287 30 Jan 23 peter 18   You should have received a copy of the GNU General Public License
4287 30 Jan 23 peter 19   along with yat. If not, see <https://www.gnu.org/licenses/>.
4287 30 Jan 23 peter 20 */
4287 30 Jan 23 peter 21
4299 05 Feb 23 peter 22 #include <config.h>
4299 05 Feb 23 peter 23
4287 30 Jan 23 peter 24 #include "KalmanFilter.h"
4287 30 Jan 23 peter 25
4288 31 Jan 23 peter 26 #include "yat/utility/SVD.h"
4287 30 Jan 23 peter 27
4287 30 Jan 23 peter 28 #include <gsl/gsl_math.h>
4287 30 Jan 23 peter 29
4287 30 Jan 23 peter 30 namespace theplu {
4287 30 Jan 23 peter 31 namespace yat {
4287 30 Jan 23 peter 32 namespace statistics {
4287 30 Jan 23 peter 33
4287 30 Jan 23 peter 34   using utility::Matrix;
4287 30 Jan 23 peter 35   using utility::MatrixBase;
4287 30 Jan 23 peter 36   using utility::Vector;
4287 30 Jan 23 peter 37   using utility::VectorBase;
4287 30 Jan 23 peter 38
4287 30 Jan 23 peter 39   KalmanFilter::KalmanFilter(const Vector& x, const Matrix& P)
4287 30 Jan 23 peter 40     : x_(x), P_(P), I_(x.size(), x.size(), 1.0)
4287 30 Jan 23 peter 41   {
4287 30 Jan 23 peter 42   }
4287 30 Jan 23 peter 43
4287 30 Jan 23 peter 44
4287 30 Jan 23 peter 45   void KalmanFilter::operator()(const VectorBase& z, const MatrixBase& F,
4287 30 Jan 23 peter 46                                 const MatrixBase& Q, const MatrixBase& H,
4287 30 Jan 23 peter 47                                 const MatrixBase& R)
4287 30 Jan 23 peter 48   {
4287 30 Jan 23 peter 49     predict(F, Q);
4287 30 Jan 23 peter 50     update(z, H, R);
4287 30 Jan 23 peter 51   }
4287 30 Jan 23 peter 52
4287 30 Jan 23 peter 53
4287 30 Jan 23 peter 54   double KalmanFilter::logL(void) const
4287 30 Jan 23 peter 55   {
4287 30 Jan 23 peter 56     size_t d = yhat_.size();
4287 30 Jan 23 peter 57     return -0.5 * (yhat_*S_inverse_*yhat_ + ln_det_S_ + d*(M_LN2 + M_LNPI));
4287 30 Jan 23 peter 58   }
4287 30 Jan 23 peter 59
4287 30 Jan 23 peter 60
4287 30 Jan 23 peter 61   const Vector& KalmanFilter::predict(const MatrixBase& F, const MatrixBase& Q)
4287 30 Jan 23 peter 62   {
4287 30 Jan 23 peter 63     assert(F.columns() == x_.size());
4287 30 Jan 23 peter 64     assert(F.columns() == P_.rows());
4287 30 Jan 23 peter 65     assert(P_.columns() == F.columns());
4287 30 Jan 23 peter 66     assert(F.rows() == Q.rows());
4287 30 Jan 23 peter 67     assert(Q.rows() == Q.columns());
4287 30 Jan 23 peter 68     xhat_ = F * x_;
4287 30 Jan 23 peter 69     Phat_ = F * P_ * transpose(F) + Q;
4287 30 Jan 23 peter 70     return xhat_;
4287 30 Jan 23 peter 71   }
4287 30 Jan 23 peter 72
4287 30 Jan 23 peter 73
4287 30 Jan 23 peter 74   const Matrix& KalmanFilter::K(void) const
4287 30 Jan 23 peter 75   {
4287 30 Jan 23 peter 76     return K_;
4287 30 Jan 23 peter 77   }
4287 30 Jan 23 peter 78
4287 30 Jan 23 peter 79
4287 30 Jan 23 peter 80   const Matrix& KalmanFilter::Phat(void) const
4287 30 Jan 23 peter 81   {
4287 30 Jan 23 peter 82     return Phat_;
4287 30 Jan 23 peter 83   }
4287 30 Jan 23 peter 84
4287 30 Jan 23 peter 85
4287 30 Jan 23 peter 86   const Vector& KalmanFilter::xhat(void) const
4287 30 Jan 23 peter 87   {
4287 30 Jan 23 peter 88     return xhat_;
4287 30 Jan 23 peter 89   }
4287 30 Jan 23 peter 90
4287 30 Jan 23 peter 91
4287 30 Jan 23 peter 92   const Matrix& KalmanFilter::S(void) const
4287 30 Jan 23 peter 93   {
4287 30 Jan 23 peter 94     return S_;
4287 30 Jan 23 peter 95   }
4287 30 Jan 23 peter 96
4287 30 Jan 23 peter 97
4287 30 Jan 23 peter 98   const Vector& KalmanFilter::y(void) const
4287 30 Jan 23 peter 99   {
4287 30 Jan 23 peter 100     return y_;
4287 30 Jan 23 peter 101   }
4287 30 Jan 23 peter 102
4287 30 Jan 23 peter 103
4287 30 Jan 23 peter 104   const Vector& KalmanFilter::yhat(void) const
4287 30 Jan 23 peter 105   {
4287 30 Jan 23 peter 106     return yhat_;
4287 30 Jan 23 peter 107   }
4287 30 Jan 23 peter 108
4287 30 Jan 23 peter 109
4287 30 Jan 23 peter 110   const Vector& KalmanFilter::update(const VectorBase& z, const MatrixBase& H,
4287 30 Jan 23 peter 111                                      const MatrixBase& R)
4287 30 Jan 23 peter 112   {
4287 30 Jan 23 peter 113     assert(H.columns() == xhat_.size());
4287 30 Jan 23 peter 114     assert(z.size() == H.rows());
4287 30 Jan 23 peter 115     yhat_ = z - H * xhat_;
4287 30 Jan 23 peter 116
4287 30 Jan 23 peter 117     assert(H.columns() == Phat_.rows());
4287 30 Jan 23 peter 118     assert(Phat_.columns() == H.columns());
4287 30 Jan 23 peter 119     assert(H.rows() == R.rows());
4287 30 Jan 23 peter 120     assert(R.rows() == R.columns());
4287 30 Jan 23 peter 121     S_ = H * Phat_ * transpose(H) + R;
4287 30 Jan 23 peter 122     utility::SVD svd(S_);
4287 30 Jan 23 peter 123     svd.decompose();
4287 30 Jan 23 peter 124     svd.inverse(S_inverse_);
4287 30 Jan 23 peter 125     size_t n = svd.s().size();
4287 30 Jan 23 peter 126     ln_det_S_ = 0;
4287 30 Jan 23 peter 127     for (size_t i=0; i<n; ++i)
4287 30 Jan 23 peter 128       ln_det_S_ += std::log(svd.s()(i));
4287 30 Jan 23 peter 129
4287 30 Jan 23 peter 130     K_ = Phat_ * transpose(H) * S_inverse_;
4287 30 Jan 23 peter 131     x_ = xhat_ + K_ * yhat_;
4287 30 Jan 23 peter 132     P_ = (I_ - K_ * H) * Phat_;
4287 30 Jan 23 peter 133     y_ = z - H * x_;
4287 30 Jan 23 peter 134     return x_;
4287 30 Jan 23 peter 135   }
4287 30 Jan 23 peter 136
4287 30 Jan 23 peter 137 }}}

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