Line data    Source code

       1              : #ifndef D7AF2A2A_FB74_4C03_A85F_5AC092A11582
       2              : #define D7AF2A2A_FB74_4C03_A85F_5AC092A11582
       3              : 
       4              : #include "filter/kalman_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.hpp"                     // IWYU pragma: keep
      10              : #include "math/linalg/square_matrix.hpp"              // IWYU pragma: keep
      11              : 
      12              : namespace tracking
      13              : {
      14              : namespace filter
      15              : {
      16              : 
      17              : template <typename CovarianceMatrixPolicy_>
      18              : template <sint32 DimX_, sint32 DimQ_>
      19           40 : inline void KalmanFilter<CovarianceMatrixPolicy_>::predictCovariance(CovarianceMatrixType<DimX_>&                   P,
      20              :                                                                      const math::SquareMatrix<value_type, DimX_>&   A,
      21              :                                                                      const math::Matrix<value_type, DimX_, DimQ_>&  G,
      22              :                                                                      const math::DiagonalMatrix<value_type, DimQ_>& Q)
      23              : {
      24              :   if constexpr (CovarianceMatrixPolicy_::is_factored)
      25              :   {
      26           20 :     P.thornton(A, G, Q);
      27              :   }
      28              :   else
      29              :   {
      30           40 :     P = math::CovarianceMatrixFull<value_type, DimX_>{
      31           20 :         typename math::CovarianceMatrixFull<value_type, DimX_>::SquareMatrix{A * P * A.transpose() + G * Q * G.transpose()}};
      32              :   }
      33           40 : }
      34              : 
      35              : } // namespace filter
      36              : } // namespace tracking
      37              : 
      38              : #endif // D7AF2A2A_FB74_4C03_A85F_5AC092A11582