Line data    Source code

       1              : #ifndef BB251A7C_F2DC_4075_B478_BA5931DF6CEC
       2              : #define BB251A7C_F2DC_4075_B478_BA5931DF6CEC
       3              : 
       4              : #include "math/linalg/modified_gram_schmidt.h"
       5              : 
       6              : #include "math/linalg/diagonal_matrix.hpp"   // IWYU pragma: keep
       7              : #include "math/linalg/matrix.hpp"            // IWYU pragma: keep
       8              : #include "math/linalg/square_matrix.hpp"     // IWYU pragma: keep
       9              : #include "math/linalg/triangular_matrix.hpp" // IWYU pragma: keep
      10              : 
      11              : namespace tracking
      12              : {
      13              : namespace math
      14              : {
      15              : template <typename ValueType_, sint32 Size_>
      16           46 : void ModifiedGramSchmidt<ValueType_, Size_>::run(TriangularMatrix<ValueType_, Size_, false, true>& u,
      17              :                                                  DiagonalMatrix<ValueType_, Size_>&                d,
      18              :                                                  const SquareMatrix<ValueType_, Size_, true>&      Phi)
      19              : {
      20              :   // M. S. Grewal and A. P. Andrews
      21              :   // Kalman Filtering: Theory and Practice Using MATLAB, 4th Edition
      22              :   // Wiley, 2014.
      23              :   //
      24              :   // Catherine Thornton's modified weighted Gram-Schmidt orthogonalization method
      25              :   // TODO(matthias): Grewal, p. 260 -> inplace product Phi*U
      26              :   // TODO(matthias): optimization - eliminate allocations in modified Gram-Schmidt for in-place updates
      27              :   // beneficial for small n without complexity overhead; add template-based loop unrolling for Size_ <= 10
      28           46 :   auto PhiU = Phi * u;
      29           46 :   auto Din  = d;
      30           46 :   u.setIdentity();
      31              :   ValueType_ sigma;
      32          240 :   for (sint32 i = Size_ - 1; i >= 0; --i)
      33              :   {
      34          194 :     sigma = static_cast<ValueType_>(0.0);
      35         1080 :     for (sint32 j = 0; j < Size_; ++j)
      36              :     {
      37          886 :       sigma += (PhiU.at_unsafe(i, j) * PhiU.at_unsafe(i, j)) * Din.at_unsafe(j);
      38              :     }
      39          222 :     d.at_unsafe(i) = std::max(sigma, std::numeric_limits<ValueType_>::epsilon());
      40          540 :     for (sint32 j = 0; j < i; ++j)
      41              :     {
      42          346 :       sigma = static_cast<ValueType_>(0.0);
      43         2076 :       for (sint32 k = 0; k < Size_; ++k)
      44              :       {
      45         1730 :         sigma += PhiU.at_unsafe(i, k) * Din.at_unsafe(k) * PhiU.at_unsafe(j, k);
      46              :       }
      47              : 
      48          346 :       u.at_unsafe(j, i) = sigma / d.at_unsafe(i);
      49              : 
      50         2076 :       for (sint32 k = 0; k < Size_; ++k)
      51              :       {
      52         1730 :         PhiU.at_unsafe(j, k) -= u.at_unsafe(j, i) * PhiU.at_unsafe(i, k);
      53              :       }
      54              :     }
      55              :   }
      56           46 : }
      57              : 
      58              : template <typename ValueType_, sint32 Size_>
      59              : template <sint32 SizeQ_>
      60           29 : void ModifiedGramSchmidt<ValueType_, Size_>::run(TriangularMatrix<ValueType_, Size_, false, true>& u,
      61              :                                                  DiagonalMatrix<ValueType_, Size_>&                d,
      62              :                                                  const SquareMatrix<ValueType_, Size_, true>&      Phi,
      63              :                                                  const Matrix<ValueType_, Size_, SizeQ_, true>&    G,
      64              :                                                  const DiagonalMatrix<ValueType_, SizeQ_>&         Q)
      65              : {
      66              :   // M. S. Grewal and A. P. Andrews
      67              :   // Kalman Filtering: Theory and Practice Using MATLAB, 4th Edition
      68              :   // Wiley, 2014.
      69              :   //
      70              :   // Catherine Thornton's modified weighted Gram-Schmidt orthogonalization method
      71              :   // for the predictor update of the U-D factors of the covariance matrix
      72              :   // of estimation uncertainty in Kalman filtering
      73              : 
      74              :   // TODO(matthias): Grewal, p. 260 -> inplace product Phi*U
      75              :   // TODO(matthias): optimization - eliminate allocations in modified Gram-Schmidt for in-place updates
      76              :   // beneficial for small n without complexity overhead; add template-based loop unrolling for Size_ <= 10
      77           29 :   auto PhiU = Phi * u;
      78           29 :   auto Din  = d;
      79           29 :   auto Gin  = G;
      80           29 :   u.setIdentity();
      81              :   ValueType_ sigma;
      82          160 :   for (sint32 i = Size_ - 1; i >= 0; --i)
      83              :   {
      84          131 :     sigma = static_cast<ValueType_>(0.0);
      85              :     // process all state columns
      86          770 :     for (sint32 j = 0; j < Size_; ++j)
      87              :     {
      88          639 :       sigma += (PhiU.at_unsafe(i, j) * PhiU.at_unsafe(i, j)) * Din.at_unsafe(j);
      89              :     }
      90              :     // process all process noise columns
      91          539 :     for (sint32 j = 0; j < SizeQ_; ++j)
      92              :     {
      93          408 :       sigma += Gin.at_unsafe(i, j) * Gin.at_unsafe(i, j) * Q.at_unsafe(j);
      94              :     }
      95          131 :     d.at_unsafe(i) = std::max(sigma, std::numeric_limits<ValueType_>::epsilon());
      96          385 :     for (sint32 j = 0; j < i; ++j)
      97              :     {
      98          254 :       sigma = static_cast<ValueType_>(0.0);
      99         1587 :       for (sint32 k = 0; k < Size_; ++k)
     100              :       {
     101         1333 :         sigma += PhiU.at_unsafe(i, k) * Din.at_unsafe(k) * PhiU.at_unsafe(j, k);
     102              :       }
     103         1084 :       for (sint32 k = 0; k < SizeQ_; ++k)
     104              :       {
     105          830 :         sigma += Gin.at_unsafe(i, k) * Q.at_unsafe(k) * Gin.at_unsafe(j, k);
     106              :       }
     107              : 
     108          254 :       u.at_unsafe(j, i) = sigma / d.at_unsafe(i);
     109              : 
     110         1587 :       for (sint32 k = 0; k < Size_; ++k)
     111              :       {
     112         1333 :         PhiU.at_unsafe(j, k) -= u.at_unsafe(j, i) * PhiU.at_unsafe(i, k);
     113              :       }
     114         1084 :       for (sint32 k = 0; k < SizeQ_; ++k)
     115              :       {
     116          830 :         Gin.at_unsafe(j, k) -= u.at_unsafe(j, i) * Gin.at_unsafe(i, k);
     117              :       }
     118              :     }
     119              :   }
     120           29 : }
     121              : 
     122              : } // namespace math
     123              : } // namespace tracking
     124              : 
     125              : #endif // BB251A7C_F2DC_4075_B478_BA5931DF6CEC