TrackFindingTracklet/interface/KalmanFilter.h

0001 #ifndef L1Trigger_TrackFindingTracklet_KalmanFilter_h
0002 #define L1Trigger_TrackFindingTracklet_KalmanFilter_h
0003
0004 #include "L1Trigger/TrackTrigger/interface/Setup.h"
0005 #include "L1Trigger/TrackFindingTracklet/interface/DataFormats.h"
0006 #include "L1Trigger/TrackFindingTracklet/interface/KalmanFilterFormats.h"
0007 #include "L1Trigger/TrackFindingTracklet/interface/State.h"
0008 #include "DataFormats/L1TrackTrigger/interface/TTTypes.h"
0009 #include "L1Trigger/TrackFindingTMTT/interface/Settings.h"
0010 #include "L1Trigger/TrackFindingTMTT/interface/KFParamsComb.h"
0011
0012 #include <vector>
0013 #include <deque>
0014
0015 namespace trklet {
0016
0017   /*! \class  trklet::KalmanFilter
0018    *  \brief  Class to do helix fit to all tracks in a region.
0019    *          All variable names & equations come from Fruhwirth KF paper
0020    *          http://dx.doi.org/10.1016/0168-9002%2887%2990887-4
0021    *          Summary of variables:
0022    *          m = hit position (phi,z)
0023    *          V = hit position 2x2 covariance matrix in (phi,z).
0024    *          x = helix params
0025    *          C = helix params 4x4 covariance matrix
0026    *          r = residuals
0027    *          H = 2x4 derivative matrix (expected stub position w.r.t. helix params)
0028    *          K = KF gain 2x2 matrix
0029    *          x' & C': Updated values of x & C after KF iteration
0030    *          Boring: F = unit matrix; pxcov = C
0031    *          Summary of equations:
0032    *          S = H*C (2x4 matrix); St = Transpose S
0033    *          R = V + H*C*Ht (KF paper) = V + H*St (used here at simpler): 2x2 matrix
0034    *          Rinv = Inverse R
0035    *          K = St * Rinv : 2x2 Kalman gain matrix * det(R)
0036    *          r = m - H*x
0037    *          x' = x + K*r
0038    *          C' = C - K*H*C (KF paper) = C - K*S (used here as simpler)
0039    *  \author Thomas Schuh
0040    *  \date   2024, Sep
0041    */
0042   class KalmanFilter {
0043   public:
0044     typedef State::Stub Stub;
0045     KalmanFilter(const tt::Setup* setup,
0046                  const DataFormats* dataFormats,
0047                  KalmanFilterFormats* kalmanFilterFormats,
0048                  tmtt::Settings* settings,
0049                  tmtt::KFParamsComb* tmtt,
0050                  int region,
0051                  tt::TTTracks& ttTracks);
0052     ~KalmanFilter() = default;
0053     // read in and organize input tracks and stubs
0054     void consume(const tt::StreamsTrack& streamsTrack, const tt::StreamsStub& streamsStub);
0055     // fill output products
0056     void produce(tt::StreamsStub& streamsStub,
0057                  tt::StreamsTrack& streamsTrack,
0058                  int& numAcceptedStates,
0059                  int& numLostStates);
0060
0061   private:
0062     //
0063     struct Track {
0064       Track() {}
0065       Track(int trackId,
0066             int numConsistent,
0067             int numConsistentPS,
0068             double d0,
0069             const TTBV& hitPattern,
0070             const TrackKF& trackKF,
0071             const std::vector<StubKF>& stubsKF)
0072           : trackId_(trackId),
0073             numConsistent_(numConsistent),
0074             numConsistentPS_(numConsistentPS),
0075             d0_(d0),
0076             hitPattern_(hitPattern),
0077             trackKF_(trackKF),
0078             stubsKF_(stubsKF) {}
0079       int trackId_;
0080       int numConsistent_;
0081       int numConsistentPS_;
0082       double d0_;
0083       TTBV hitPattern_;
0084       TrackKF trackKF_;
0085       std::vector<StubKF> stubsKF_;
0086     };
0087     // call old KF
0088     void simulate(tt::StreamsStub& streamsStub, tt::StreamsTrack& streamsTrack);
0089     // constraints double precision
0090     double digi(VariableKF var, double val) { return kalmanFilterFormats_->format(var).digi(val); }
0091     //
0092     int integer(VariableKF var, double val) { return kalmanFilterFormats_->format(var).integer(val); }
0093     //
0094     void updateRangeActual(VariableKF var, double val) {
0095       return kalmanFilterFormats_->format(var).updateRangeActual(val);
0096     }
0097     //
0098     double base(VariableKF var) { return kalmanFilterFormats_->format(var).base(); }
0099     //
0100     int width(VariableKF var) { return kalmanFilterFormats_->format(var).width(); }
0101     // remove and return first element of deque, returns nullptr if empty
0102     template <class T>
0103     T* pop_front(std::deque<T*>& ts) const;
0104     // calculates the helix params & their cov. matrix from a pair of stubs
0105     void calcSeeds();
0106     // Transform States into output products
0107     void conv(tt::StreamsStub& streamsStub, tt::StreamsTrack& streamsTrack);
0108     // adds a layer to states, bool indicating if in seeding process
0109     void addLayer(bool seed = false);
0110     // apply final cuts
0111     void finalize();
0112     // best state selection
0113     void accumulator();
0114     // updates state using 4 paramter fit
0115     void update4(State*& state);
0116     // updates state using 5 parameter fit
0117     void update5(State*& state);
0118
0119     // provides run-time constants
0120     const tt::Setup* setup_;
0121     // provides dataformats
0122     const DataFormats* dataFormats_;
0123     // provides dataformats of Kalman filter internals
0124     KalmanFilterFormats* kalmanFilterFormats_;
0125     //
0126     tmtt::Settings* settings_;
0127     //
0128     tmtt::KFParamsComb* tmtt_;
0129     // processing region
0130     int region_;
0131     //
0132     tt::TTTracks& ttTracks_;
0133     // container of tracks
0134     std::vector<TrackDR> tracks_;
0135     // container of stubs
0136     std::vector<Stub> stubs_;
0137     // container of all Kalman Filter states
0138     std::deque<State> states_;
0139     // processing stream
0140     std::deque<State*> stream_;
0141     //
0142     std::vector<Track> finals_;
0143     // current layer used during state propagation
0144     int layer_;
0145     //
0146     std::vector<double> zTs_;
0147   };
0148
0149 }  // namespace trklet
0150
0151 #endif