File indexing completed on 2022-10-14 01:43:54
0001 #include "L1Trigger/TrackerTFP/interface/KalmanFilterFormats.h"
0002
0003 #include <vector>
0004 #include <deque>
0005 #include <cmath>
0006 #include <tuple>
0007 #include <iterator>
0008 #include <algorithm>
0009 #include <limits>
0010 #include <cstring>
0011
0012 using namespace std;
0013 using namespace edm;
0014 using namespace tt;
0015
0016 namespace trackerTFP {
0017
0018 constexpr auto variableKFstrs_ = {
0019 "x0", "x1", "x2", "x3", "H00", "H12", "m0", "m1", "v0",
0020 "v1", "r0", "r1", "S00", "S01", "S12", "S13", "K00", "K10",
0021 "K21", "K31", "R00", "R11", "R00Rough", "R11Rough", "invR00Approx", "invR11Approx", "invR00Cor",
0022 "invR11Cor", "invR00", "invR11", "C00", "C01", "C11", "C22", "C23", "C33"};
0023
0024 void KalmanFilterFormats::endJob() {
0025 const int wName =
0026 strlen(*max_element(variableKFstrs_.begin(), variableKFstrs_.end(), [](const auto& a, const auto& b) {
0027 return strlen(a) < strlen(b);
0028 }));
0029 static constexpr int wWidth = 3;
0030 for (VariableKF v = VariableKF::begin; v != VariableKF::end; v = VariableKF(+v + 1)) {
0031 const pair<double, double>& range = format(v).rangeActual();
0032 const double r = format(v).twos() ? max(abs(range.first), abs(range.second)) * 2. : range.second;
0033 const int width = ceil(log2(r / format(v).base()));
0034 cout << setw(wName) << *next(variableKFstrs_.begin(), +v) << ": " << setw(wWidth) << width << " " << setw(wWidth)
0035 << format(v).width() << " | " << setw(wWidth) << format(v).width() - width << endl;
0036 }
0037 }
0038
0039 KalmanFilterFormats::KalmanFilterFormats() : iConfig_(), dataFormats_(nullptr), setup_(nullptr) {
0040 formats_.reserve(+VariableKF::end);
0041 }
0042
0043 KalmanFilterFormats::KalmanFilterFormats(const ParameterSet& iConfig, const DataFormats* dataFormats)
0044 : iConfig_(dataFormats->hybrid() ? iConfig.getParameter<ParameterSet>("hybrid")
0045 : iConfig.getParameter<ParameterSet>("tmtt")),
0046 dataFormats_(dataFormats),
0047 setup_(dataFormats_->setup()) {
0048 formats_.reserve(+VariableKF::end);
0049 fillFormats();
0050 }
0051
0052 template <VariableKF it>
0053 void KalmanFilterFormats::fillFormats() {
0054 formats_.emplace_back(FormatKF<it>(dataFormats_, iConfig_));
0055 if constexpr (++it != VariableKF::end)
0056 fillFormats<++it>();
0057 }
0058
0059 DataFormatKF::DataFormatKF(const VariableKF& v, bool twos)
0060 : v_(v),
0061 twos_(twos),
0062 width_(0),
0063 base_(1.),
0064 range_(0.),
0065 rangeActual_(numeric_limits<double>::max(), numeric_limits<double>::lowest()) {}
0066
0067
0068 bool DataFormatKF::inRange(double d) const {
0069 if (twos_)
0070 return d >= -range_ / 2. && d < range_ / 2.;
0071 return d >= 0 && d < range_;
0072 }
0073
0074 void DataFormatKF::updateRangeActual(double d) {
0075 rangeActual_ = make_pair(min(rangeActual_.first, d), max(rangeActual_.second, d));
0076 if (!inRange(d)) {
0077 string v = *next(variableKFstrs_.begin(), +v_);
0078 cms::Exception exception("out_of_range");
0079 exception.addContext("trackerTFP:DataFormatKF::updateRangeActual");
0080 exception << "Variable " << v << " = " << d << " is out of range " << (twos_ ? -range_ / 2. : 0) << " to "
0081 << (twos_ ? range_ / 2. : range_) << "." << endl;
0082 if (twos_ || d >= 0.)
0083 exception.addAdditionalInfo("Consider raising BaseShift" + v + " in KalmnaFilterFormats_cfi.py.");
0084 throw exception;
0085 }
0086 }
0087
0088 template <>
0089 FormatKF<VariableKF::x0>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0090 : DataFormatKF(VariableKF::x0, true) {
0091 const DataFormat& input = dataFormats->format(Variable::inv2R, Process::kf);
0092 const int baseShift = iConfig.getParameter<int>("BaseShiftx0");
0093 base_ = pow(2, baseShift) * input.base();
0094 width_ = dataFormats->setup()->widthDSPbb();
0095 calcRange();
0096 }
0097
0098 template <>
0099 FormatKF<VariableKF::x1>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0100 : DataFormatKF(VariableKF::x1, true) {
0101 const DataFormat& input = dataFormats->format(Variable::phiT, Process::kf);
0102 const int baseShift = iConfig.getParameter<int>("BaseShiftx1");
0103 base_ = pow(2, baseShift) * input.base();
0104 width_ = dataFormats->setup()->widthDSPbb();
0105 calcRange();
0106 }
0107
0108 template <>
0109 FormatKF<VariableKF::x2>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0110 : DataFormatKF(VariableKF::x2, true) {
0111 const DataFormat& input = dataFormats->format(Variable::cot, Process::kf);
0112 const int baseShift = iConfig.getParameter<int>("BaseShiftx2");
0113 base_ = pow(2, baseShift) * input.base();
0114 width_ = dataFormats->setup()->widthDSPbb();
0115 calcRange();
0116 }
0117
0118 template <>
0119 FormatKF<VariableKF::x3>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0120 : DataFormatKF(VariableKF::x3, true) {
0121 const DataFormat& input = dataFormats->format(Variable::zT, Process::kf);
0122 const int baseShift = iConfig.getParameter<int>("BaseShiftx3");
0123 base_ = pow(2, baseShift) * input.base();
0124 width_ = dataFormats->setup()->widthDSPbb();
0125 calcRange();
0126 }
0127
0128 template <>
0129 FormatKF<VariableKF::H00>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0130 : DataFormatKF(VariableKF::H00, true) {
0131 const DataFormat& kfin = dataFormats->format(Variable::r, Process::kfin);
0132 base_ = kfin.base();
0133 width_ = kfin.width();
0134 range_ = kfin.range();
0135 }
0136
0137 template <>
0138 FormatKF<VariableKF::H12>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0139 : DataFormatKF(VariableKF::H12, true) {
0140 const Setup* setup = dataFormats->setup();
0141 const DataFormat& kfin = dataFormats->format(Variable::r, Process::kfin);
0142 base_ = kfin.base();
0143 range_ = 2. * max(abs(setup->outerRadius() - setup->chosenRofZ()), abs(setup->innerRadius() - setup->chosenRofZ()));
0144 width_ = ceil(log2(range_ / base_));
0145 }
0146
0147 template <>
0148 FormatKF<VariableKF::m0>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0149 : DataFormatKF(VariableKF::m0, true) {
0150 const DataFormat& kfin = dataFormats->format(Variable::phi, Process::kfin);
0151 base_ = kfin.base();
0152 width_ = kfin.width();
0153 range_ = kfin.range();
0154 }
0155
0156 template <>
0157 FormatKF<VariableKF::m1>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0158 : DataFormatKF(VariableKF::m1, true) {
0159 const DataFormat& kfin = dataFormats->format(Variable::z, Process::kfin);
0160 base_ = kfin.base();
0161 width_ = kfin.width();
0162 range_ = kfin.range();
0163 }
0164
0165 template <>
0166 FormatKF<VariableKF::v0>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0167 : DataFormatKF(VariableKF::v0, false) {
0168 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0169 const int baseShift = iConfig.getParameter<int>("BaseShiftv0");
0170 base_ = pow(2., baseShift) * x1.base() * x1.base();
0171 width_ = dataFormats->setup()->widthDSPbu();
0172 calcRange();
0173 }
0174
0175 template <>
0176 FormatKF<VariableKF::v1>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0177 : DataFormatKF(VariableKF::v1, true) {
0178 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0179 const int baseShift = iConfig.getParameter<int>("BaseShiftv1");
0180 base_ = pow(2., baseShift) * x3.base() * x3.base();
0181 width_ = dataFormats->setup()->widthDSPbu();
0182 calcRange();
0183 }
0184
0185 template <>
0186 FormatKF<VariableKF::r0>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0187 : DataFormatKF(VariableKF::r0, true) {
0188 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0189 const int baseShift = iConfig.getParameter<int>("BaseShiftr0");
0190 base_ = pow(2., baseShift) * x1.base();
0191 width_ = dataFormats->setup()->widthDSPbb();
0192 calcRange();
0193 }
0194
0195 template <>
0196 FormatKF<VariableKF::r1>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0197 : DataFormatKF(VariableKF::r1, true) {
0198 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0199 const int baseShift = iConfig.getParameter<int>("BaseShiftr1");
0200 base_ = pow(2., baseShift) * x3.base();
0201 width_ = dataFormats->setup()->widthDSPbb();
0202 calcRange();
0203 }
0204
0205 template <>
0206 FormatKF<VariableKF::S00>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0207 : DataFormatKF(VariableKF::S00, true) {
0208 const DataFormat& x0 = dataFormats->format(Variable::inv2R, Process::kf);
0209 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0210 const int baseShift = iConfig.getParameter<int>("BaseShiftS00");
0211 base_ = pow(2., baseShift) * x0.base() * x1.base();
0212 width_ = dataFormats->setup()->widthDSPbb();
0213 calcRange();
0214 }
0215
0216 template <>
0217 FormatKF<VariableKF::S01>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0218 : DataFormatKF(VariableKF::S01, true) {
0219 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0220 const int baseShift = iConfig.getParameter<int>("BaseShiftS01");
0221 base_ = pow(2., baseShift) * x1.base() * x1.base();
0222 width_ = dataFormats->setup()->widthDSPbb();
0223 calcRange();
0224 }
0225
0226 template <>
0227 FormatKF<VariableKF::S12>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0228 : DataFormatKF(VariableKF::S12, true) {
0229 const DataFormat& x2 = dataFormats->format(Variable::cot, Process::kf);
0230 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0231 const int baseShift = iConfig.getParameter<int>("BaseShiftS12");
0232 base_ = pow(2., baseShift) * x2.base() * x3.base();
0233 width_ = dataFormats->setup()->widthDSPbb();
0234 calcRange();
0235 }
0236
0237 template <>
0238 FormatKF<VariableKF::S13>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0239 : DataFormatKF(VariableKF::S13, true) {
0240 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0241 const int baseShift = iConfig.getParameter<int>("BaseShiftS13");
0242 base_ = pow(2., baseShift) * x3.base() * x3.base();
0243 width_ = dataFormats->setup()->widthDSPbb();
0244 calcRange();
0245 }
0246
0247 template <>
0248 FormatKF<VariableKF::K00>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0249 : DataFormatKF(VariableKF::K00, true) {
0250 const DataFormat& x0 = dataFormats->format(Variable::inv2R, Process::kf);
0251 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0252 const int baseShift = iConfig.getParameter<int>("BaseShiftK00");
0253 base_ = pow(2., baseShift) * x0.base() / x1.base();
0254 width_ = dataFormats->setup()->widthDSPab();
0255 calcRange();
0256 }
0257
0258 template <>
0259 FormatKF<VariableKF::K10>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0260 : DataFormatKF(VariableKF::K10, true) {
0261 const int baseShift = iConfig.getParameter<int>("BaseShiftK10");
0262 base_ = pow(2., baseShift);
0263 width_ = dataFormats->setup()->widthDSPab();
0264 calcRange();
0265 }
0266
0267 template <>
0268 FormatKF<VariableKF::K21>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0269 : DataFormatKF(VariableKF::K21, true) {
0270 const DataFormat& x2 = dataFormats->format(Variable::cot, Process::kf);
0271 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0272 const int baseShift = iConfig.getParameter<int>("BaseShiftK21");
0273 base_ = pow(2., baseShift) * x2.base() / x3.base();
0274 width_ = dataFormats->setup()->widthDSPab();
0275 calcRange();
0276 }
0277
0278 template <>
0279 FormatKF<VariableKF::K31>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0280 : DataFormatKF(VariableKF::K31, true) {
0281 const int baseShift = iConfig.getParameter<int>("BaseShiftK31");
0282 base_ = pow(2., baseShift);
0283 width_ = dataFormats->setup()->widthDSPab();
0284 calcRange();
0285 }
0286
0287 template <>
0288 FormatKF<VariableKF::R00>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0289 : DataFormatKF(VariableKF::R00, false) {
0290 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0291 const int baseShift = iConfig.getParameter<int>("BaseShiftR00");
0292 base_ = pow(2., baseShift) * x1.base() * x1.base();
0293 width_ = dataFormats->setup()->widthDSPbu();
0294 calcRange();
0295 }
0296
0297 template <>
0298 FormatKF<VariableKF::R11>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0299 : DataFormatKF(VariableKF::R11, false) {
0300 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0301 const int baseShift = iConfig.getParameter<int>("BaseShiftR11");
0302 base_ = pow(2., baseShift) * x3.base() * x3.base();
0303 width_ = dataFormats->setup()->widthDSPbu();
0304 calcRange();
0305 }
0306
0307 template <>
0308 FormatKF<VariableKF::R00Rough>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0309 : DataFormatKF(VariableKF::R00Rough, false) {
0310 const FormatKF<VariableKF::R00> R00(dataFormats, iConfig);
0311 width_ = dataFormats->setup()->widthAddrBRAM18();
0312 range_ = R00.range();
0313 const int baseShift = R00.width() - width_;
0314 base_ = pow(2., baseShift) * R00.base();
0315 }
0316
0317 template <>
0318 FormatKF<VariableKF::R11Rough>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0319 : DataFormatKF(VariableKF::R11Rough, false) {
0320 const FormatKF<VariableKF::R11> R11(dataFormats, iConfig);
0321 width_ = dataFormats->setup()->widthAddrBRAM18();
0322 range_ = R11.range();
0323 const int baseShift = R11.width() - width_;
0324 base_ = pow(2., baseShift) * R11.base();
0325 }
0326
0327 template <>
0328 FormatKF<VariableKF::invR00Approx>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0329 : DataFormatKF(VariableKF::invR00Approx, false) {
0330 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0331 const int baseShift = iConfig.getParameter<int>("BaseShiftInvR00Approx");
0332 base_ = pow(2., baseShift) / x1.base() / x1.base();
0333 width_ = dataFormats->setup()->widthDSPbu();
0334 calcRange();
0335 }
0336
0337 template <>
0338 FormatKF<VariableKF::invR11Approx>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0339 : DataFormatKF(VariableKF::invR11Approx, false) {
0340 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0341 const int baseShift = iConfig.getParameter<int>("BaseShiftInvR11Approx");
0342 base_ = pow(2., baseShift) / x3.base() / x3.base();
0343 width_ = dataFormats->setup()->widthDSPbu();
0344 calcRange();
0345 }
0346
0347 template <>
0348 FormatKF<VariableKF::invR00Cor>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0349 : DataFormatKF(VariableKF::invR00Cor, false) {
0350 const int baseShift = iConfig.getParameter<int>("BaseShiftInvR00Cor");
0351 base_ = pow(2., baseShift);
0352 width_ = dataFormats->setup()->widthDSPbu();
0353 calcRange();
0354 }
0355
0356 template <>
0357 FormatKF<VariableKF::invR11Cor>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0358 : DataFormatKF(VariableKF::invR11Cor, false) {
0359 const int baseShift = iConfig.getParameter<int>("BaseShiftInvR11Cor");
0360 base_ = pow(2., baseShift);
0361 width_ = dataFormats->setup()->widthDSPbu();
0362 calcRange();
0363 }
0364
0365 template <>
0366 FormatKF<VariableKF::invR00>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0367 : DataFormatKF(VariableKF::invR00, false) {
0368 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0369 const int baseShift = iConfig.getParameter<int>("BaseShiftInvR00");
0370 base_ = pow(2., baseShift) / x1.base() / x1.base();
0371 width_ = dataFormats->setup()->widthDSPau();
0372 calcRange();
0373 }
0374
0375 template <>
0376 FormatKF<VariableKF::invR11>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0377 : DataFormatKF(VariableKF::invR11, false) {
0378 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0379 const int baseShift = iConfig.getParameter<int>("BaseShiftInvR11");
0380 base_ = pow(2., baseShift) / x3.base() / x3.base();
0381 width_ = dataFormats->setup()->widthDSPau();
0382 calcRange();
0383 }
0384
0385 template <>
0386 FormatKF<VariableKF::C00>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0387 : DataFormatKF(VariableKF::C00, false) {
0388 const DataFormat& x0 = dataFormats->format(Variable::inv2R, Process::kf);
0389 const int baseShift = iConfig.getParameter<int>("BaseShiftC00");
0390 base_ = pow(2., baseShift) * x0.base() * x0.base();
0391 width_ = dataFormats->setup()->widthDSPbu();
0392 calcRange();
0393 }
0394
0395 template <>
0396 FormatKF<VariableKF::C01>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0397 : DataFormatKF(VariableKF::C01, true) {
0398 const DataFormat& x0 = dataFormats->format(Variable::inv2R, Process::kf);
0399 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0400 const int baseShift = iConfig.getParameter<int>("BaseShiftC01");
0401 base_ = pow(2., baseShift) * x0.base() * x1.base();
0402 width_ = dataFormats->setup()->widthDSPbb();
0403 calcRange();
0404 }
0405
0406 template <>
0407 FormatKF<VariableKF::C11>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0408 : DataFormatKF(VariableKF::C11, false) {
0409 const DataFormat& x1 = dataFormats->format(Variable::phiT, Process::kf);
0410 const int baseShift = iConfig.getParameter<int>("BaseShiftC11");
0411 base_ = pow(2., baseShift) * x1.base() * x1.base();
0412 width_ = dataFormats->setup()->widthDSPbu();
0413 calcRange();
0414 }
0415
0416 template <>
0417 FormatKF<VariableKF::C22>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0418 : DataFormatKF(VariableKF::C22, false) {
0419 const DataFormat& x2 = dataFormats->format(Variable::cot, Process::kf);
0420 const int baseShift = iConfig.getParameter<int>("BaseShiftC22");
0421 base_ = pow(2., baseShift) * x2.base() * x2.base();
0422 width_ = dataFormats->setup()->widthDSPbu();
0423 calcRange();
0424 }
0425
0426 template <>
0427 FormatKF<VariableKF::C23>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0428 : DataFormatKF(VariableKF::C23, true) {
0429 const DataFormat& x2 = dataFormats->format(Variable::cot, Process::kf);
0430 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0431 const int baseShift = iConfig.getParameter<int>("BaseShiftC23");
0432 base_ = pow(2., baseShift) * x2.base() * x3.base();
0433 width_ = dataFormats->setup()->widthDSPbb();
0434 calcRange();
0435 }
0436
0437 template <>
0438 FormatKF<VariableKF::C33>::FormatKF(const DataFormats* dataFormats, const edm::ParameterSet& iConfig)
0439 : DataFormatKF(VariableKF::C33, false) {
0440 const DataFormat& x3 = dataFormats->format(Variable::zT, Process::kf);
0441 const int baseShift = iConfig.getParameter<int>("BaseShiftC33");
0442 base_ = pow(2., baseShift) * x3.base() * x3.base();
0443 width_ = dataFormats->setup()->widthDSPbu();
0444 calcRange();
0445 }
0446
0447 }
