TrackFindingTMTT/src/KalmanState.cc

0001 #include "L1Trigger/TrackFindingTMTT/interface/KalmanState.h"
0002 #include "L1Trigger/TrackFindingTMTT/interface/Settings.h"
0003 #include "TMatrixD.h"
0004
0005 using namespace std;
0006
0007 namespace tmtt {
0008
0009   KalmanState::KalmanState(const Settings *settings,
0010                            const L1track3D &candidate,
0011                            unsigned nSkipped,
0012                            int kLayer,
0013                            const KalmanState *last_state,
0014                            const TVectorD &vecX,
0015                            const TMatrixD &matC,
0016                            const TMatrixD &matK,
0017                            const TMatrixD &matV,
0018                            Stub *stub,
0019                            double chi2rphi,
0020                            double chi2rz)
0021       : settings_(settings),
0022         kLayer_(kLayer),
0023         last_state_(last_state),
0024         vecX_(vecX),
0025         stub_(stub),
0026         chi2rphi_(chi2rphi),
0027         chi2rz_(chi2rz),
0028         nSkipped_(nSkipped),
0029         l1track3D_(candidate) {
0030     matC_.Clear();
0031     matC_.ResizeTo(matC.GetNrows(), matC.GetNcols());
0032     matC_ = matC;
0033     matK_.ResizeTo(matK.GetNrows(), matK.GetNcols());
0034     matK_ = matK;
0035     matV_.ResizeTo(matV.GetNrows(), matV.GetNcols());
0036     matV_ = matV;
0037     kalmanChi2RphiScale_ = settings_->kalmanChi2RphiScale();
0038
0039     hitPattern_ = 0;
0040     if (last_state != nullptr)
0041       hitPattern_ = last_state->hitPattern();  // Bit encoded list of hit layers
0042     if (stub != nullptr && kLayer_ >= 0)
0043       hitPattern_ |= (1 << (kLayer_));
0044
0045     r_ = 0.1;
0046     z_ = 0;
0047     barrel_ = true;
0048
0049     if (stub != nullptr) {
0050       r_ = stub->r();
0051       z_ = stub->z();
0052       barrel_ = stub->barrel();
0053     }
0054
0055     n_stubs_ = 1 + kLayer_ - nSkipped_;
0056   }
0057
0058   bool KalmanState::good(const TP *tp) const {
0059     const KalmanState *state = this;
0060     while (state) {
0061       Stub *stub = state->stub();
0062       if (stub != nullptr) {
0063         const set<const TP *> &tps = stub->assocTPs();
0064         if (tps.find(tp) == tps.end())
0065           return false;
0066       }
0067       state = state->last_state();
0068     }
0069     return true;
0070   }
0071
0072   double KalmanState::reducedChi2() const {
0073     if (2 * n_stubs_ - vecX_.GetNrows() > 0)
0074       return (this->chi2()) / (2 * n_stubs_ - vecX_.GetNrows());
0075     else
0076       return 0;
0077   }
0078
0079   const KalmanState *KalmanState::last_update_state() const {
0080     const KalmanState *state = this;
0081     while (state) {
0082       if (state->stub() != nullptr)
0083         return state;
0084       state = state->last_state();
0085     }
0086     return nullptr;
0087   }
0088
0089   std::vector<Stub *> KalmanState::stubs() const {
0090     std::vector<Stub *> all_stubs;
0091
0092     const KalmanState *state = this;
0093     while (state) {
0094       Stub *stub = state->stub();
0095       if (stub != nullptr)
0096         all_stubs.push_back(stub);
0097       state = state->last_state();
0098     }
0099     std::reverse(all_stubs.begin(), all_stubs.end());  // Put innermost stub first.
0100     return all_stubs;
0101   }
0102
0103 }  // namespace tmtt