Line data    Source code

       1              : #ifndef D0D9A45D_92DE_4848_87C8_9B309333C102
       2              : #define D0D9A45D_92DE_4848_87C8_9B309333C102
       3              : 
       4              : #include "filter/information_filter.h"
       5              : 
       6              : #include "math/linalg/covariance_matrix_factored.hpp" // IWYU pragma: keep
       7              : #include "math/linalg/covariance_matrix_full.hpp"     // IWYU pragma: keep
       8              : #include "math/linalg/diagonal_matrix.hpp"            // IWYU pragma: keep
       9              : #include "math/linalg/matrix_column_view.hpp"         // IWYU pragma: keep
      10              : #include "math/linalg/square_matrix.hpp"              // IWYU pragma: keep
      11              : #include "math/linalg/vector.hpp"                     // IWYU pragma: keep
      12              : 
      13              : 
      14              : namespace tracking
      15              : {
      16              : namespace filter
      17              : {
      18              : 
      19              : template <typename CovarianceMatrixPolicy_>
      20              : template <sint32 DimX_, sint32 DimQ_>
      21           40 : void InformationFilter<CovarianceMatrixPolicy_>::predictCovariance(CovarianceMatrixType<DimX_>&                   Y,
      22              :                                                                    const math::SquareMatrix<value_type, DimX_>&   A,
      23              :                                                                    const math::Matrix<value_type, DimX_, DimQ_>&  G,
      24              :                                                                    const math::DiagonalMatrix<value_type, DimQ_>& Q)
      25              : {
      26              :   if constexpr (CovarianceMatrixPolicy_::is_factored)
      27              :   {
      28              :     // Information Formulation of the UDU Kalman Filter
      29              :     // Christopher D’Souza and Renato Zanetti
      30              :     // https://sites.utexas.edu/renato/files/2018/05/UDU_Information.pdf
      31           20 :     const math::SquareMatrix<value_type, DimX_, false>  invA     = A.inverse();
      32           20 :     const math::Matrix<value_type, DimX_, DimQ_, false> invAMulG = invA * G;
      33              : 
      34              :     // apply DimQ times the Rank-1 update P = P - c*x*x'
      35              :     // with Gi=inv(A)*G(:,i) and ci=inv(Gi'*Y*Gi+inv(Q(i,i))) is (1x1) and x=Y*Gi is (nx1)
      36              :     value_type                      ci;
      37           20 :     math::Vector<value_type, DimX_> xi;
      38              :     using ColView = math::MatrixColumnView<value_type, DimX_, DimQ_, false>;
      39           90 :     for (sint32 i = 0; i < DimQ_; ++i)
      40              :     {
      41           70 :       const ColView Gi{invAMulG, i};
      42           70 :       const auto    fullY{Y()};
      43           70 :       xi = fullY * Gi;
      44           70 :       ci = -1 / (1 / Q.at_unsafe(i) + Gi * xi);
      45              : 
      46           70 :       Y.rank1Update(ci, xi);
      47              :     }
      48              :     // propagate factorization by inv(A)'
      49           20 :     const math::SquareMatrix<value_type, DimX_, true> invAT{invA.transpose()};
      50           20 :     Y.apaT(invAT);
      51           20 :   }
      52              :   else
      53              :   {
      54              :     // Discrete kalman filter tutorial
      55              :     // GA Terejanu
      56              :     // University at Buffalo, Department of Computer Science and Engineering, NY 14260
      57              :     // https://scholar.google.com/citations?view_op=view_citation&hl=en&user=Z7LP12kAAAAJ&citation_for_view=Z7LP12kAAAAJ:_FxGoFyzp5QC
      58           20 :     auto M{Y};
      59              :     // calc M = invA'*Y*invA as Y is the inverse of P
      60           20 :     const math::SquareMatrix<value_type, DimX_, false> invA = A.inverse();
      61           20 :     M.apaT(math::SquareMatrix<value_type, DimX_, true>{invA.transpose()});
      62              :     // solve now H * Y = M with H = (I + M * G*Q*G') using QR as H is not symmetric
      63           20 :     const auto H =
      64              :         math::SquareMatrix<value_type, DimX_>(math::SquareMatrix<value_type, DimX_>::Identity() + M * (G * Q * G.transpose()));
      65           20 :     math::SquareMatrix cov = H.qrSolve(std::move(M));
      66           20 :     cov.symmetrize();
      67              : 
      68              :     // prevent destroying the Information matrix, e.g. removing information from a zero Y matrix (no information)
      69           20 :     if (cov.isPositiveSemiDefinite())
      70              :     {
      71           20 :       Y = math::CovarianceMatrixFull<value_type, DimX_>{std::move(cov)};
      72              :     }
      73           20 :   }
      74           40 : }
      75              : 
      76              : } // namespace filter
      77              : } // namespace tracking
      78              : 
      79              : #endif // D0D9A45D_92DE_4848_87C8_9B309333C102