Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2025-06-03 00:12:20

0001 #ifndef L1Trigger_TrackFindingTracklet_KalmanFilterFormats_h
0002 #define L1Trigger_TrackFindingTracklet_KalmanFilterFormats_h
0003 
0004 /*----------------------------------------------------------------------
0005 Classes to calculate and provide dataformats used by Kalman Filter emulator
0006 enabling tuning of bit widths
0007 ----------------------------------------------------------------------*/
0008 
0009 #include "FWCore/ParameterSet/interface/ParameterSet.h"
0010 #include "L1Trigger/TrackFindingTracklet/interface/DataFormats.h"
0011 
0012 #include <vector>
0013 #include <cmath>
0014 #include <initializer_list>
0015 #include <tuple>
0016 #include <utility>
0017 #include <array>
0018 #include <string>
0019 
0020 namespace trklet {
0021 
0022   /*
0023   *          All variable names & equations come from Fruhwirth KF paper
0024   *          http://dx.doi.org/10.1016/0168-9002%2887%2990887-4
0025   *          Summary of variables:
0026   *          m = hit position (phi,z)
0027   *          V = hit position 2x2 covariance matrix in (phi,z).
0028   *          x = helix params
0029   *          C = helix params 4x4 covariance matrix
0030   *          r = residuals
0031   *          H = 2x4 derivative matrix (expected stub position w.r.t. helix params)
0032   *          K = KF gain 2x2 matrix
0033   *          x' & C': Updated values of x & C after KF iteration
0034   *          Boring: F = unit matrix; pxcov = C
0035   *          Summary of equations:
0036   *          S = H*C (2x4 matrix); St = Transpose S
0037   *          R = V + H*C*Ht (KF paper) = V + H*St (used here at simpler): 2x2 matrix
0038   *          Rinv = Inverse R
0039   *          K = St * Rinv : 2x2 Kalman gain matrix * det(R)
0040   *          r = m - H*x
0041   *          x' = x + K*r
0042   *          C' = C - K*H*C (KF paper) = C - K*S (used here as simpler)
0043   */
0044   enum class VariableKF {
0045     begin,
0046     x0 = begin,
0047     x1,
0048     x2,
0049     x3,
0050     H00,
0051     H12,
0052     m0,
0053     m1,
0054     v0,
0055     v1,
0056     r0,
0057     r1,
0058     S00,
0059     S01,
0060     S12,
0061     S13,
0062     S00Shifted,
0063     S01Shifted,
0064     S12Shifted,
0065     S13Shifted,
0066     K00,
0067     K10,
0068     K21,
0069     K31,
0070     R00,
0071     R11,
0072     R00Rough,
0073     R11Rough,
0074     invR00Approx,
0075     invR11Approx,
0076     invR00Cor,
0077     invR11Cor,
0078     invR00,
0079     invR11,
0080     C00,
0081     C01,
0082     C11,
0083     C22,
0084     C23,
0085     C33,
0086     dH,
0087     invdH,
0088     invdH2,
0089     H2,
0090     Hm0,
0091     Hm1,
0092     Hv0,
0093     Hv1,
0094     H2v0,
0095     H2v1,
0096     end
0097   };
0098   inline constexpr int operator+(VariableKF v) { return static_cast<int>(v); }
0099   inline constexpr VariableKF operator+(VariableKF v, int i) { return VariableKF(+v + i); }
0100 
0101   // Configuration
0102   struct ConfigKF {
0103     bool enableIntegerEmulation_;
0104     int widthR00_;
0105     int widthR11_;
0106     int widthC00_;
0107     int widthC01_;
0108     int widthC11_;
0109     int widthC22_;
0110     int widthC23_;
0111     int widthC33_;
0112     int baseShiftx0_;
0113     int baseShiftx1_;
0114     int baseShiftx2_;
0115     int baseShiftx3_;
0116     int baseShiftr0_;
0117     int baseShiftr1_;
0118     int baseShiftS00_;
0119     int baseShiftS01_;
0120     int baseShiftS12_;
0121     int baseShiftS13_;
0122     int baseShiftR00_;
0123     int baseShiftR11_;
0124     int baseShiftInvR00Approx_;
0125     int baseShiftInvR11Approx_;
0126     int baseShiftInvR00Cor_;
0127     int baseShiftInvR11Cor_;
0128     int baseShiftInvR00_;
0129     int baseShiftInvR11_;
0130     int baseShiftS00Shifted_;
0131     int baseShiftS01Shifted_;
0132     int baseShiftS12Shifted_;
0133     int baseShiftS13Shifted_;
0134     int baseShiftK00_;
0135     int baseShiftK10_;
0136     int baseShiftK21_;
0137     int baseShiftK31_;
0138     int baseShiftC00_;
0139     int baseShiftC01_;
0140     int baseShiftC11_;
0141     int baseShiftC22_;
0142     int baseShiftC23_;
0143     int baseShiftC33_;
0144   };
0145 
0146   class DataFormatKF {
0147   public:
0148     DataFormatKF(const VariableKF& v, bool twos, bool enableIntegerEmulation, int width, double base, double range);
0149     ~DataFormatKF() = default;
0150     double digi(double val) const {
0151       return enableIntegerEmulation_ ? (std::floor(val / base_ + 1.e-11) + .5) * base_ : val;
0152     }
0153     bool twos() const { return twos_; }
0154     int width() const { return width_; }
0155     double base() const { return base_; }
0156     double range() const { return range_; }
0157     double min() const { return min_; }
0158     double abs() const { return abs_; }
0159     double max() const { return max_; }
0160     // returns false if data format would oferflow for this double value
0161     bool inRange(double d) const;
0162     void updateRangeActual(double d);
0163     int integer(double d) const { return floor(d / base_ + 1.e-11); }
0164 
0165   protected:
0166     VariableKF v_;
0167     bool twos_;
0168     bool enableIntegerEmulation_;
0169     int width_;
0170     double base_;
0171     double range_;
0172     double min_;
0173     double abs_;
0174     double max_;
0175   };
0176 
0177   class KalmanFilterFormats {
0178   public:
0179     KalmanFilterFormats();
0180     ~KalmanFilterFormats() = default;
0181     DataFormatKF& format(VariableKF v) { return formats_[+v]; }
0182     const tt::Setup* setup() const { return dataFormats_->setup(); }
0183     const DataFormats* dataFormats() const { return dataFormats_; }
0184     void consume(const DataFormats* dataFormats, const ConfigKF& iConfig);
0185     void endJob(std::stringstream& ss);
0186 
0187   private:
0188     template <VariableKF it = VariableKF::begin>
0189     void fillFormats();
0190     ConfigKF iConfig_;
0191     const DataFormats* dataFormats_;
0192     std::vector<DataFormatKF> formats_;
0193   };
0194 
0195   // function template for DataFormat generation
0196   template <VariableKF v>
0197   DataFormatKF makeDataFormat(const DataFormats* dataFormats, const ConfigKF& iConfig);
0198 
0199   template <>
0200   DataFormatKF makeDataFormat<VariableKF::x0>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0201   template <>
0202   DataFormatKF makeDataFormat<VariableKF::x1>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0203   template <>
0204   DataFormatKF makeDataFormat<VariableKF::x2>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0205   template <>
0206   DataFormatKF makeDataFormat<VariableKF::x3>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0207   template <>
0208   DataFormatKF makeDataFormat<VariableKF::H00>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0209   template <>
0210   DataFormatKF makeDataFormat<VariableKF::H12>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0211   template <>
0212   DataFormatKF makeDataFormat<VariableKF::m0>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0213   template <>
0214   DataFormatKF makeDataFormat<VariableKF::m1>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0215   template <>
0216   DataFormatKF makeDataFormat<VariableKF::v0>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0217   template <>
0218   DataFormatKF makeDataFormat<VariableKF::v1>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0219   template <>
0220   DataFormatKF makeDataFormat<VariableKF::r0>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0221   template <>
0222   DataFormatKF makeDataFormat<VariableKF::r1>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0223   template <>
0224   DataFormatKF makeDataFormat<VariableKF::S00>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0225   template <>
0226   DataFormatKF makeDataFormat<VariableKF::S01>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0227   template <>
0228   DataFormatKF makeDataFormat<VariableKF::S12>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0229   template <>
0230   DataFormatKF makeDataFormat<VariableKF::S13>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0231   template <>
0232   DataFormatKF makeDataFormat<VariableKF::S00Shifted>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0233   template <>
0234   DataFormatKF makeDataFormat<VariableKF::S01Shifted>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0235   template <>
0236   DataFormatKF makeDataFormat<VariableKF::S12Shifted>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0237   template <>
0238   DataFormatKF makeDataFormat<VariableKF::S13Shifted>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0239   template <>
0240   DataFormatKF makeDataFormat<VariableKF::K00>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0241   template <>
0242   DataFormatKF makeDataFormat<VariableKF::K10>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0243   template <>
0244   DataFormatKF makeDataFormat<VariableKF::K21>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0245   template <>
0246   DataFormatKF makeDataFormat<VariableKF::K31>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0247   template <>
0248   DataFormatKF makeDataFormat<VariableKF::R00>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0249   template <>
0250   DataFormatKF makeDataFormat<VariableKF::R11>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0251   template <>
0252   DataFormatKF makeDataFormat<VariableKF::R00Rough>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0253   template <>
0254   DataFormatKF makeDataFormat<VariableKF::R11Rough>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0255   template <>
0256   DataFormatKF makeDataFormat<VariableKF::invR00Approx>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0257   template <>
0258   DataFormatKF makeDataFormat<VariableKF::invR11Approx>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0259   template <>
0260   DataFormatKF makeDataFormat<VariableKF::invR00Cor>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0261   template <>
0262   DataFormatKF makeDataFormat<VariableKF::invR11Cor>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0263   template <>
0264   DataFormatKF makeDataFormat<VariableKF::invR00>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0265   template <>
0266   DataFormatKF makeDataFormat<VariableKF::invR11>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0267   template <>
0268   DataFormatKF makeDataFormat<VariableKF::C00>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0269   template <>
0270   DataFormatKF makeDataFormat<VariableKF::C01>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0271   template <>
0272   DataFormatKF makeDataFormat<VariableKF::C11>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0273   template <>
0274   DataFormatKF makeDataFormat<VariableKF::C22>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0275   template <>
0276   DataFormatKF makeDataFormat<VariableKF::C23>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0277   template <>
0278   DataFormatKF makeDataFormat<VariableKF::C33>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0279 
0280   template <>
0281   DataFormatKF makeDataFormat<VariableKF::dH>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0282   template <>
0283   DataFormatKF makeDataFormat<VariableKF::invdH>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0284   template <>
0285   DataFormatKF makeDataFormat<VariableKF::invdH2>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0286   template <>
0287   DataFormatKF makeDataFormat<VariableKF::H2>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0288   template <>
0289   DataFormatKF makeDataFormat<VariableKF::Hm0>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0290   template <>
0291   DataFormatKF makeDataFormat<VariableKF::Hm1>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0292   template <>
0293   DataFormatKF makeDataFormat<VariableKF::Hv0>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0294   template <>
0295   DataFormatKF makeDataFormat<VariableKF::Hv1>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0296   template <>
0297   DataFormatKF makeDataFormat<VariableKF::H2v0>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0298   template <>
0299   DataFormatKF makeDataFormat<VariableKF::H2v1>(const DataFormats* dataFormats, const ConfigKF& iConfig);
0300 
0301 }  // namespace trklet
0302 
0303 #endif