Newer
Older
TB_Chris / TbEvent / src / .svn / text-base / TbKalmanNode.cpp.svn-base
  1. // local
  2. #include "Event/TbKalmanNode.h"
  3.  
  4. /** @file FitNode.cpp
  5. *
  6. * This File contains the implementation of the FitNode.
  7. * A FitNode is a basket of objects at a given z position.
  8. * The information inside the FitNode has to be sufficient
  9. * to allow smoothing and refitting.
  10. * At the moment a FitNode contains or allows you to access
  11. * info on the the (kth) measurement,
  12. * transport from k --> k + 1 , predicted state at k+1
  13. * (predicted from filter step) and the best state at k
  14. * (notation note filter procedes from k -> k + 1 -> k + 2 ......)
  15. *
  16. * @author Victor Coco and Wouter Hulsbergen (moved K-math here)
  17. * @date 2011-02-01
  18. *
  19. * @author Eduardo Rodrigues (adaptations to the new track event model)
  20. * @date 2005-04-15
  21. *
  22. * @author Matt Needham
  23. * @date 11-11-1999
  24. */
  25.  
  26. namespace LHCb {
  27.  
  28. /// Standard constructor, initializes variables
  29. // TbKalmanNode::TbKalmanNode():
  30. // m_prevNode(0),
  31. // m_nextNode(0),
  32. // m_parent(0)
  33. // {
  34. // // TbKalmanNode default constructor
  35. // m_filterStatus[Forward] = m_filterStatus[Backward] = Uninitialized ;
  36. // m_hasInfoUpstream[Forward] = m_hasInfoUpstream[Backward] = Unknown ;
  37. // }
  38.  
  39. /// Constructor from a z position
  40. TbKalmanNode::TbKalmanNode(TbKalmanTrack& parent, double z)
  41. : m_parent(&parent), m_prevNode(0), m_nextNode(0), m_Q(0) {
  42. m_state.setZ(z);
  43. m_filterStatus[Forward] = m_filterStatus[Backward] = Uninitialized;
  44. m_hasInfoUpstream[Forward] = m_hasInfoUpstream[Backward] = Unknown;
  45. }
  46.  
  47. /// Destructor
  48. TbKalmanNode::~TbKalmanNode() {
  49. if (m_prevNode && m_prevNode->m_nextNode == this) m_prevNode->m_nextNode = 0;
  50. if (m_nextNode && m_nextNode->m_prevNode == this) m_nextNode->m_prevNode = 0;
  51. }
  52.  
  53. // Clone the node
  54. //LHCb::TbKalmanNode* TbKalmanNode::clone() const
  55. //{
  56. // LHCb::TbKalmanNode* rc = new LHCb::TbKalmanNode(*this) ;
  57. // rc->m_prevNode = rc->m_nextNode = 0 ;
  58. // return rc ;
  59. //}
  60.  
  61. //=========================================================================
  62. // Helper function to decide if we need to use the upstream filtered state
  63. //=========================================================================
  64. bool TbKalmanNode::hasInfoUpstream(int direction) const {
  65. if (m_hasInfoUpstream[direction] == LHCb::TbKalmanNode::Unknown) {
  66. bool rc = false;
  67. const TbKalmanNode* prev = prevNode(direction);
  68. if (prev) {
  69. if (prev->hasInfo())
  70. rc = true;
  71. else
  72. rc = prev->hasInfoUpstream(direction);
  73. }
  74. unConst().m_hasInfoUpstream[direction] =
  75. rc ? LHCb::TbKalmanNode::True : LHCb::TbKalmanNode::False;
  76. }
  77. return (m_hasInfoUpstream[direction] == LHCb::TbKalmanNode::True);
  78. }
  79.  
  80. void TbKalmanNode::resetHasInfoUpstream(int direction) {
  81. m_hasInfoUpstream[direction] = False;
  82. if (!hasInfo()) {
  83. TbKalmanNode* next = const_cast<TbKalmanNode*>(nextNode(direction));
  84. if (next) next->resetHasInfoUpstream(direction);
  85. }
  86. }
  87.  
  88. //=========================================================================
  89. // Reset the status of this node
  90. //=========================================================================
  91. void TbKalmanNode::resetFilterStatus(int direction, FilterStatus s) {
  92. // The logic here is tedious, because of the smoothed states have
  93. // a strange depence, which depends on the type of smoother.
  94. if (m_filterStatus[direction] > s) {
  95. m_filterStatus[direction] = s;
  96.  
  97. if (s < Filtered) {
  98. // if the backward filter is in 'Smoothed' state, it needs to be
  99. // reset to filtered, because the bi-directional smoother relies
  100. // on both filtered states
  101. if (m_filterStatus[Backward] ==
  102. Smoothed) // Note: Backward=Smoothed means 'bi-directional smoother'
  103. m_filterStatus[Backward] = Filtered;
  104.  
  105. // reset the status of any node that depends on this one. now
  106. // be careful: if this node has been copied it may be pointing
  107. // to a wrong node.
  108. const TbKalmanNode* next = nextNode(direction);
  109. if (next && next->m_filterStatus[direction] > s &&
  110. next->prevNode(direction) == this)
  111. const_cast<TbKalmanNode*>(next)
  112. ->resetFilterStatus(direction, std::min(s, Initialized));
  113. }
  114.  
  115. if (direction == Forward) {
  116. // for the classical filter, we actually need to put the
  117. // upstream node back to filtered, if it is in a classicly
  118. // smoothed status
  119. const TbKalmanNode* prev = prevNode(Forward);
  120. if (prev && prev->m_filterStatus[Forward] == Smoothed &&
  121. prev->nextNode(Forward) ==
  122. this) // Note: Forward=Smoothed means 'classical smoother'
  123. const_cast<TbKalmanNode*>(prev)->resetFilterStatus(Forward, Filtered);
  124. }
  125. }
  126. }
  127.  
  128. //=========================================================================
  129. // Predict the state to this node
  130. //=========================================================================
  131. void TbKalmanNode::computePredictedState(int direction) {
  132. //std::cout << "In TbKalmanNode::computePredictedState( "
  133. //<< direction << " ) for node " << index() << std::endl ;
  134.  
  135. // get the filtered state from the previous node. if there wasn't
  136. // any, we will want to copy the reference vector and leave the
  137. // covariance the way it is
  138. m_predictedState[direction].setZ(z());
  139. StateParameters& stateVec = m_predictedState[direction].parameters();
  140. StateCovariance& stateCov = m_predictedState[direction].covariance();
  141.  
  142. const TbKalmanNode* prevnode = prevNode(direction);
  143. if (prevnode) {
  144. const State& prevState = prevnode->filteredState(direction);
  145. if (!hasInfoUpstream(direction)) {
  146. // just _copy_ the covariance matrix from upstream, assuming
  147. // that this is the seed matrix. (that saves us from copying
  148. // the seed matrix to every state from the start.
  149. stateCov = prevState.covariance();
  150. // new: start the backward filter from the forward filter
  151. if (direction == Backward) stateVec = filteredState(Forward).parameters();
  152. //std::cout << "no information upstream. copying seed." << index() <<
  153. //std::endl ;
  154. } else {
  155.  
  156. // For the testbeam, the transport is really trivial, assuming x and y are
  157. // uncorrelated
  158. double dz = z() - prevnode->z();
  159. stateVec = prevState.parameters();
  160. stateVec[0] += dz * stateVec[2];
  161. stateVec[1] += dz * stateVec[3];
  162.  
  163. // compute the predicted covariance
  164. stateCov = prevState.covariance();
  165. stateCov(0, 0) += 2 * dz * stateCov(0, 2) + dz * dz * stateCov(2, 2);
  166. stateCov(0, 2) += dz * stateCov(2, 2);
  167. stateCov(1, 1) += 2 * dz * stateCov(1, 3) + dz * dz * stateCov(3, 3);
  168. stateCov(1, 3) += dz * stateCov(3, 3);
  169.  
  170. // finally add the noise, on the direction only
  171. double Q = direction == Forward ? prevnode->m_Q : m_Q;
  172. stateCov(2, 2) += Q;
  173. stateCov(3, 3) += Q;
  174. }
  175. }
  176. // update the status flag
  177. m_filterStatus[direction] = Predicted;
  178. }
  179.  
  180. //=========================================================================
  181. // Filter this node
  182. //=========================================================================
  183. void TbKalmanNode::computeFilteredState(int direction) {
  184. //std::cout << "In TbKalmanNode::computeFilteredState( "
  185. // << direction << " ) for node " << index() << std::endl ;
  186.  
  187. // copy the predicted state
  188. State& state = m_filteredState[direction];
  189. state = predictedState(direction);
  190.  
  191. // apply the filter if needed
  192. m_deltaChi2[direction] = filter(state);
  193.  
  194. //std::cout << "Filtering node " << index() << " " << direction
  195. //<< " chi2 = "
  196. //<< m_deltaChi2[direction] << std::endl ;
  197.  
  198. m_filterStatus[direction] = Filtered;
  199. }
  200.  
  201. //=========================================================================
  202. // Bi-directional smoother
  203. //=========================================================================
  204. void TbKalmanNode::computeBiSmoothedState() {
  205. //std::cout << "In TbKalmanNode::computeBiSmoothedState() for node " <<
  206. //index() << std::endl ;
  207.  
  208. State& state = m_state;
  209. if (!hasInfoUpstream(Forward)) {
  210. // last node in backward direction
  211. state = filteredState(Backward);
  212. } else if (!hasInfoUpstream(Backward)) {
  213. // last node in forward direction
  214. state = filteredState(Forward);
  215. } else {
  216. // Take the weighted average of two states. We now need to
  217. // choose for which one we take the filtered state. AFAIU the
  218. // weighted average behaves better if the weights are more
  219. // equal. So, we filter the 'worst' prediction. In the end, it
  220. // all doesn't seem to make much difference.
  221.  
  222. const State* s1, *s2;
  223. if (predictedState(Backward).covariance()(0, 0) >
  224. predictedState(Forward).covariance()(0, 0)) {
  225. s1 = &(filteredState(Backward));
  226. s2 = &(predictedState(Forward));
  227. } else {
  228. s1 = &(filteredState(Forward));
  229. s2 = &(predictedState(Backward));
  230. }
  231.  
  232. const StateParameters& X1 = s1->parameters();
  233. const StateCovariance& C1 = s1->covariance();
  234. const StateParameters& X2 = s2->parameters();
  235. const StateCovariance& C2 = s2->covariance();
  236.  
  237. //state = predictedState(Forward) ;
  238. StateParameters& X = state.parameters();
  239. StateCovariance& C = state.covariance();
  240.  
  241. // compute the inverse of the covariance in the difference: R=(C1+C2)
  242. static StateCovariance invR;
  243. invR = C1 + C2;
  244. bool success = invR.InvertChol();
  245. if (!success) {
  246. std::cout << "error inverting cov matrix in smoother" << std::endl;
  247. //&& !m_parent->inError())
  248. //m_parent->setErrorFlag(2,KalmanFitResult::Smooth
  249. //,KalmanFitResult::MatrixInversion ) ;
  250. }
  251. // compute the gain matrix:
  252. static ROOT::Math::SMatrix<double, 4, 4> K;
  253. K = C1 * invR;
  254. X = X1 + K * (X2 - X1);
  255. ROOT::Math::AssignSym::Evaluate(C, K * C2);
  256. // the following used to be more stable, but isn't any longer, it seems:
  257. //ROOT::Math::AssignSym::Evaluate(C, -2 * K * C1) ;
  258. //C += C1 + ROOT::Math::Similarity(K,R) ;
  259.  
  260. //std::cout << "smoothing two states with errors on slope: "
  261. //<< std::sqrt(C1(2,2)) << " " << std::sqrt(C2(2,2)) << " "
  262. //<< std::sqrt(C(2,2)) << std::endl ;
  263.  
  264. }
  265. //if(!isPositiveDiagonal(state.covariance())&& !m_parent->inError()){
  266. // m_parent->setErrorFlag(2,KalmanFitResult::Smooth
  267. // ,KalmanFitResult::AlgError ) ;
  268. //}
  269. updateResidual(state);
  270.  
  271. // bug fix: we cannot set backward to state 'Smoothed', unless we have passed
  272. // its filter step!
  273. filteredState(Backward);
  274. m_filterStatus[Backward] = Smoothed;
  275. }
  276.  
  277. int TbKalmanNode::index() const {
  278. int rc = 0;
  279. if (m_prevNode) rc = m_prevNode->index() + 1;
  280. return rc;
  281. }
  282. }