Newer
Older
TB_Chris / TbEvent / src / .svn / text-base / TbKalmanPixelMeasurement.cpp.svn-base
  1. #include "Event/TbKalmanPixelMeasurement.h"
  2. #include "Event/TbKalmanTrack.h"
  3.  
  4. namespace {
  5. /// Helper function to filter one hit
  6. inline double filterX(double& x, double& tx, double& covXX, double& covXTx,
  7. double& covTxTx, const double xhit,
  8. const double xhitcov) {
  9. const double predcovXX = covXX;
  10. const double predcovXTx = covXTx;
  11. const double predcovTxTx = covTxTx;
  12. const double predx = x;
  13. const double predtx = tx;
  14.  
  15. // compute the gain matrix
  16. const double R = xhitcov + predcovXX;
  17. const double Kx = predcovXX / R;
  18. const double KTx = predcovXTx / R;
  19. // update the state vector
  20. double r = xhit - predx;
  21. x = predx + Kx * r;
  22. tx = predtx + KTx * r;
  23. // update the covariance matrix. we can write it in many ways ...
  24. covXX /*= predcovXX - Kx * predcovXX */ = (1 - Kx) * predcovXX;
  25. covXTx /*= predcovXTx - predcovXX * predcovXTx / R */ = (1 - Kx) * predcovXTx;
  26. covTxTx = predcovTxTx - KTx * predcovXTx;
  27. // return the chi2
  28. return r * r / R;
  29. }
  30. }
  31.  
  32. namespace LHCb {
  33.  
  34. LHCb::ChiSquare TbKalmanPixelMeasurement::filter(State& state) const {
  35. // filter X and Y independently. could easily be complicated if needed.
  36.  
  37. double chi2(0);
  38. Gaudi::SymMatrix4x4& stateCov = state.covariance();
  39. Gaudi::Vector4& stateVec = state.parameters();
  40.  
  41. // first X
  42. chi2 += filterX(stateVec(0), stateVec(2), stateCov(0, 0), stateCov(0, 2),
  43. stateCov(2, 2), m_x, m_covx);
  44.  
  45. // then Y
  46. chi2 += filterX(stateVec(1), stateVec(3), stateCov(1, 1), stateCov(1, 3),
  47. stateCov(3, 3), m_y, m_covy);
  48. // chi2 has 2 dofs
  49.  
  50. return LHCb::ChiSquare(chi2, 2);
  51. }
  52.  
  53. // compute residual with respect to given (smoothed) state
  54. void TbKalmanPixelMeasurement::updateResidual(const State& state) {
  55. m_residualX = m_x - state.x();
  56. m_residualY = m_y - state.y();
  57. int sign = m_isactive ? -1 : +1;
  58. m_residualCovX = m_covx + sign * state.covariance()(0, 0);
  59. m_residualCovY = m_covy + sign * state.covariance()(1, 1);
  60. }
  61.  
  62. void TbKalmanPixelMeasurement::deactivateMeasurement(bool deactivate) {
  63. // only do something if this is actually an active hit
  64. if ((deactivate && m_isactive) || (!deactivate && !m_isactive)) {
  65. // set type to outlier
  66. m_isactive = !deactivate;
  67. // this will take care of upstream and downstream nodes as well:
  68. // they will be reset to initialized. we need to check this
  69. // carefully
  70. resetFilterStatus(Predicted);
  71. // make sure the KalmanFitResult knows something has changed
  72. if (parent()) parent()->resetCache();
  73. // now make sure others do not rely on this one anymore
  74. if (!hasInfoUpstream(Forward)) resetHasInfoUpstream(Forward);
  75. if (!hasInfoUpstream(Backward)) resetHasInfoUpstream(Backward);
  76. }
  77. }
  78.  
  79. }