Line data    Source code

       1              : #ifndef B0B1C2CE_5E51_4440_A2C1_E90EE2C82FB6
       2              : #define B0B1C2CE_5E51_4440_A2C1_E90EE2C82FB6
       3              : 
       4              : #include "motion/generic_predict_common.h"
       5              : 
       6              : #include "env/ego_motion.hpp" // IWYU pragma: keep
       7              : 
       8              : namespace tracking
       9              : {
      10              : namespace motion
      11              : {
      12              : namespace generic
      13              : {
      14              : 
      15              : template <typename MotionModel_, typename CovarianceMatrixPolicy_>
      16           80 : inline void PredictCommon<MotionModel_, CovarianceMatrixPolicy_>::run(Storage&             data,
      17              :                                                                       const value_type     dt,
      18              :                                                                       const EgoMotionType& egoMotion)
      19              : {
      20           80 :   assert(dt >= static_cast<value_type>(0));
      21           80 :   auto& underlying = static_cast<MotionModel_&>(*this);
      22              : 
      23              :   // Compute ego motion compensation matrices
      24           80 :   underlying.computeEgoMotionCompensationMatrices(data.Ge, data.Go, egoMotion);
      25           80 :   if (!egoMotion.getDisplacementCog().vec.isZeros())
      26              :   {
      27              :     // Exact nonlinear compensation in case the model is nonlinear
      28           40 :     underlying.compensateState(egoMotion);
      29              :   }
      30              : 
      31              :   // Compute state transition matrix on compensated state
      32           80 :   underlying.computeA(data.A, dt);
      33              : 
      34              :   // Exact nonlinear state prediction in case the model is nonlinear
      35           80 :   underlying.applyProcessModel(dt);
      36              : 
      37              :   // Compute process noise matrices
      38           80 :   underlying.computeQ(data.Q, dt);
      39           80 :   underlying.computeG(data.G, dt);
      40              : 
      41              :   if constexpr (CovarianceMatrixPolicy_::is_factored)
      42              :   {
      43           40 :     data.AGo = typename MotionModel_::StateMatrix{data.A * data.Go};
      44              :     // clang-format off
      45              :     // fill a diagonal matrix representing the augmented process noise matrix Qstar = [De 0; 0 Q]
      46           40 :     data.Qstar.template setBlock<EgoMotionType::DS_NUM_VARIABLES, EgoMotionType::DS_NUM_VARIABLES, 0, 0>(
      47           40 :         egoMotion.getDisplacementCog().cov.D()); // set De starting at dest row 0
      48              :     data.Qstar.template setBlock<MotionModel_::NUM_PROC_NOISE_VARIABLES,
      49              :                                   MotionModel_::NUM_PROC_NOISE_VARIABLES,
      50              :                                   0,
      51           40 :                                   EgoMotionType::DS_NUM_VARIABLES>(
      52              :         data.Q); // set Q starting at dest row DS_NUM_VARIABLES
      53              : 
      54              :     // fill a normal matrix representing the augmented process noise mapping matrix Gstar = [A*Ge*Ue G]
      55              :     data.Gstar.template setBlock<MotionModel_::NUM_STATE_VARIABLES, EgoMotionType::DS_NUM_VARIABLES,
      56              :                                   MotionModel_::NUM_STATE_VARIABLES, EgoMotionType::DS_NUM_VARIABLES,
      57              :                                   0, 0, true,
      58           40 :                                   0, 0>(
      59           40 :         data.A * data.Ge * egoMotion.getDisplacementCog().cov.U()); // set A*Ge*Ue starting at dest (0,0)
      60              :     data.Gstar.template setBlock<MotionModel_::NUM_STATE_VARIABLES, MotionModel_::NUM_PROC_NOISE_VARIABLES,
      61              :                                   MotionModel_::NUM_STATE_VARIABLES, MotionModel_::NUM_PROC_NOISE_VARIABLES,
      62              :                                   0, 0, true,
      63           40 :                                   0, EgoMotionType::DS_NUM_VARIABLES>(
      64              :         data.G); // set G starting at dest (0, DS_NUM_VARIABLES)
      65              :     // clang-format on
      66              :   }
      67           80 : }
      68              : 
      69              : } // namespace generic
      70              : } // namespace motion
      71              : } // namespace tracking
      72              : 
      73              : #endif // B0B1C2CE_5E51_4440_A2C1_E90EE2C82FB6