10FitWLSQ::FitWLSQ(Double_t *xx, Double_t cSigma1, Double_t cSigma2, Double_t cP1, Int_t nNPoints,
11 Int_t nNParams, Bool_t RBF, Bool_t LHf, Int_t RBfn) {
12 for (
int i = 0;
i < 3;
i++) {
14 for (
int j = 0; j < 3; j++) {
19 if (RB_TYPE == 4 || RB_TYPE == 5) {
35 w =
new Double_t[nNPoints];
36 wrb =
new Double_t[nNPoints];
37 x =
new Double_t[nNPoints];
39 for (
int i = 0;
i < NPoints;
i++) {
53inline Double_t FitWLSQ::det3(Double_t a[3][3]) {
55 val = a[0][0] * a[1][1] * a[2][2]
56 + a[0][1] * a[1][2] * a[2][0]
57 + a[0][2] * a[1][0] * a[2][1]
58 - a[0][1] * a[1][0] * a[2][2]
59 - a[0][0] * a[1][2] * a[2][1]
60 - a[0][2] * a[1][1] * a[2][0];
64Double_t FitWLSQ::alg_add(Double_t a[3][3], Int_t row, Int_t column) {
66 Double_t alg_value = 0;
71 for (
i = 0;
i < 2;
i++) {
72 for (j = 0; j < 2; j++) {
82 b[
i][j] = a[i_r][i_c];
85 alg_value = pow(-1, row + column + 2) * (b[0][0] * b[1][1] - b[0][1] * b[1][0]);
99 alg_value = pow(-1, row + column + 2) * bb;
104void FitWLSQ::cov_matrix(Double_t a[3][3]) {
109 for (
i = 0;
i < 3;
i++) {
110 for (j = 0; j < 3; j++) {
117 for (
i = 0;
i < 3;
i++) {
118 for (j = 0; j < 3; j++) {
119 cov[
i][j] = dfact * alg_add(a,
i, j);
124 dfact = 1 / (a[0][0] * a[1][1] - a[0][1] * a[1][0]);
125 for (
i = 0;
i < 2;
i++) {
126 for (j = 0; j < 2; j++) {
127 cov[
i][j] = dfact * alg_add(a,
i, j);
134Double_t FitWLSQ::f_lh(Double_t *yy, Double_t *zz, Double_t *wlh, Double_t *par) {
135 Double_t dcoef1, dcoef2, darg, deksp;
136 Double_t sum1, sum2, sum3;
140 dcoef1 = (Sigma2 * Sigma2 - Sigma1 * Sigma1);
141 dcoef1 = dcoef1 / (2. * Sigma1 * Sigma1 * Sigma2 * Sigma2);
142 dcoef2 = (1. - P1) * Sigma1 / (P1 * Sigma2);
146 for (
i = 0;
i < NPoints;
i++) {
149 darg = (par[1] * zz[
i] + par[0] - yy[
i]) * (par[1] * zz[
i] + par[0] - yy[
i]);
152 darg = (par[2] * zz[
i] * zz[
i] + par[1] * zz[
i] + par[0] - yy[
i]) *
153 (par[2] * zz[
i] * zz[
i] + par[1] * zz[
i] + par[0] - yy[
i]);
155 if ((dcoef1 * darg) < 600.) {
156 deksp =
exp(dcoef1 * darg);
157 sum1 = sum1 - darg / (2. * Sigma1 * Sigma1);
158 sum2 = sum2 +
log(1. + dcoef2 * deksp);
160 sum1 = sum1 - darg / (2. * Sigma1 * Sigma1);
161 sum2 = sum2 +
log(dcoef2) + dcoef1 * darg;
166 sum3 = npoint *
log(P1 / (Sigma1 *
sqrt(2. * 3.141593)));
168 return -sum1 - sum2 - sum3;
171Bool_t FitWLSQ::Initial(Double_t *trk) {
173 Double_t fmin = 999999.;
174 Double_t
f, rk1, rk2, rk3;
175 Double_t axmin[3] = {0};
176 Double_t axx[3] = {0};
179 for (
int i = 0;
i < NPoints;
i++) {
183 cout <<
"!!!Wrong number of points(<3) -> exit " << NPoints << endl;
187 if (NParams < 2 || NParams > 3) {
188 cout <<
"!!!Wrong number of parameters -> exit" << NParams << endl;
194 for (
int i = 1;
i < NPoints;
i++) {
195 for (
int j = 0; j <
i; j++) {
197 axx[1] = (trk[
i] - trk[j]) / (x[
i] - x[j]);
198 axx[0] = (x[
i] * trk[j] - x[j] * trk[
i]) / (x[
i] - x[j]);
199 f = f_lh(trk, x, w, axx);
202 for (
int k = 0; k < 3; k++) axmin[k] = axx[k];
211 for (
int i = 1;
i < NPoints;
i++) {
212 for (
int j = 0; j <
i; j++) {
213 for (
int k = 0; k < j; k++) {
214 if (w[
i] && w[j] && w[k]) {
215 rk1 = trk[
i] / ((x[
i] - x[j]) * (x[
i] - x[k]));
216 rk2 = trk[j] / ((x[j] - x[
i]) * (x[j] - x[k]));
217 rk3 = trk[k] / ((x[k] - x[
i]) * (x[k] - x[j]));
218 axx[2] = rk1 + rk2 + rk3;
219 axx[1] = -rk1 * (x[j] + x[k]) - rk2 * (x[
i] + x[k]) - rk3 * (x[
i] + x[j]);
220 axx[0] = rk1 * x[j] * x[k] + rk2 * x[
i] * x[k] + rk3 * x[
i] * x[j];
221 f = f_lh(trk, x, w, axx);
224 for (
int n = 0; n < 3; n++) axmin[n] = axx[n];
231 for (
int k = 0; k < 3; k++)
param[k] = axmin[k];
233 cout <<
"!!!NOT VALID TRACK PARAMETERS!!!!!" << endl;
239void FitWLSQ::CheckMatrix(Double_t *pdA, Double_t *pdB, TString sz) {
240 assert(pdA && pdB && NParams && sz);
244 cout <<
"\nMatrix A[n,n]:\n";
245 for (Int_t
i = 0;
i < NParams;
i++) {
246 for (Int_t j = 0; j < NParams; j++)
247 cout << setw(12) << pdA[
i + j * NParams];
250 cout <<
"Matrix B[n]:\n";
251 for (Int_t
i = 0;
i < NParams;
i++)
252 cout << setw(12) << pdB[
i];
254 cout <<
"\nMatrix P[n]:\n";
255 for (Int_t
i = 0;
i < NParams;
i++)
256 cout << setw(12) <<
param[
i];
258 cout <<
'\n' << flush;
262Bool_t FitWLSQ::SymMatrix(Double_t *pdA, Double_t *pdB) {
263 assert(pdA && pdB &&
param && NParams);
264 Int_t n1 = NParams - 1;
265 for (Int_t
i = 0u;
i < n1;
i++) {
266 Int_t in =
i * NParams;
267 Double_t dPivot = -pdA[
i + in];
268 Double_t dB = pdB[
i];
270 for (Int_t j = i1; j < NParams; j++) {
271 Double_t dA = pdA[j + in];
272 Int_t jn = j * NParams;
273 pdA[jn] = dA / dPivot;
276 for (Int_t k = i1; k <= j; k++) {
277 Int_t kn = k * NParams;
278 pdA[j + kn] += dA * pdA[kn];
280 pdB[j] += dB * pdA[jn];
283 Double_t dA = pdA[NParams * NParams - 1];
285 CheckMatrix(pdA, pdB,
"matrix singularity!");
288 param[n1] = pdB[n1] /= dA;
291 Double_t dB = -pdB[ii];
292 Int_t iin = ii * NParams;
293 for (Int_t j = ii + 1u; j < NParams; j++)
294 dB += pdA[j + iin] *
param[j];
295 param[ii] = -dB / pdA[ii + iin];
300Bool_t FitWLSQ::WLSQFit(Double_t *pdY) {
302 if (NPoints < 2 || !NParams)
304 Int_t nn = NParams * NParams;
305 Int_t nnn = nn + NParams;
306 Double_t *pdA =
new Double_t[nnn];
309 Double_t *pdB = pdA + nn;
310 for (Int_t
i = 0u;
i < nnn;
i++)
313 for (Int_t n = 0u; n < NPoints; n++) {
315 Double_t dY = pdY[n];
319 for (Int_t
i = 0u;
i < NParams;
i++) {
320 pdB[
i] += dW * dY * pow(dX,
i);
321 for (Int_t j = 0u; j <=
i; j++) {
322 pdA[
i + j * NParams] += dW * pow(dX, j +
i);
328 for (Int_t
i = 0;
i < NParams;
i++) {
329 for (Int_t j = 0; j < NParams; j++) {
330 left[
i][j] = pdA[
i + j * NParams];
334 left[0][1] = left[1][0];
335 left[0][2] = left[2][0];
336 left[1][2] = left[2][1];
341 Bool_t b = SymMatrix(pdA, pdB);
346Double_t FitWLSQ::WLSQGet(Double_t dX) {
347 assert(
param && NParams);
349 Double_t dY =
param[0];
351 for (Int_t
i = 1u;
i < NParams;
i++) {
359 Double_t dChi2 = 0.0;
361 for (Int_t
i = 0u;
i < NPoints;
i++) {
363 Double_t dY = pdY[
i];
364 Double_t dW =
wrb[
i];
369 dChi2 += dY * dY * dW;
371 return sqrt(dChi2 / dSW);
374Bool_t FitWLSQ::par_check(Double_t *par, Double_t *par1) {
380 for (
i = 0;
i < NParams;
i++) {
381 val = (par1[
i] - par[
i]) / par1[
i];
382 if (val < 0) val *= -1;
383 if (val > 0.001) value = kFALSE;
389void FitWLSQ::RB(Double_t *yy, Int_t itera_rb) {
395 Double_t sigma_param;
403 sigma_param = 5 - itera_rb * 0.5;
404 if (sigma_param < 3) sigma_param = 3;
408 sigma_k = resolution;
409 if (sigma_k < resolution)
410 sigma_k = resolution;
413 if (!RB_FIT || (!LH_INI && !itera_rb)) {
414 for (
i = 0;
i < NPoints;
i++) {
418 if ((LH_INI && RB_FIT) || (!LH_INI && itera_rb && RB_FIT)) {
419 for (
i = 0;
i < NPoints;
i++) {
420 if (NParams == 2) fitval =
param[0] +
param[1] * x[
i];
423 if (di < 0) di *= -1;
425 dval = 1 - di * di / (sigma_param * sigma_param * sigma_k * sigma_k);
426 if (di > sigma_param * sigma_k)
wrb[
i] = 0;
427 if (di < sigma_param * sigma_k)
wrb[
i] = dval * dval;
429 sum_wd +=
wrb[
i] * di * di;
433 if ((LH_INI && RB_FIT) || (!LH_INI && itera_rb && RB_FIT))
434 sigma_k =
sqrt((Double_t) sum_wd / sum_w);
439void FitWLSQ::RBN(Double_t *yy, Int_t itera_rb) {
459 sigma_p2 = 25 - itera_rb * 0.2;
460 sigma_p4 = 5 - itera_rb * 0.2;
461 sigma_pw = 4 - itera_rb * 0.2;
462 if (sigma_p2 < 21) sigma_p2 = 21;
463 if (sigma_p4 < 3.5) sigma_p4 = 3.5;
464 if (sigma_pw < 3) sigma_pw = 3;
468 sigma_k = resolution;
469 if (sigma_k < resolution)
470 sigma_k = resolution;
472 if (!RB_FIT || (!LH_INI && !itera_rb)) {
473 for (
i = 0;
i < NPoints;
i++) {
477 if ((LH_INI && RB_FIT) || (!LH_INI && itera_rb && RB_FIT)) {
478 for (
i = 0;
i < NPoints;
i++) {
479 if (NParams == 2) fitval =
param[0] +
param[1] * x[
i];
482 if (di < 0) di *= -1;
484 if (di > sigma_p2 * sigma_k)
wrb[
i] = 0;
485 if (di < sigma_p2 * sigma_k && di > sigma_pw * sigma_k) {
486 dval = 1 - pow((di / (sigma_p2 * sigma_k)), 2);
487 wrb[
i] = dval * dval * Sigma1 / (Sigma2 / P1);
489 if (di < sigma_pw * sigma_k) {
490 dval = 1 - pow((di / (sigma_p4 * sigma_k)), 4);
491 wrb[
i] = dval * dval;
495 sum_wd +=
wrb[
i] * di * di;
499 if ((LH_INI && RB_FIT) || (!LH_INI && itera_rb && RB_FIT))
500 sigma_k =
sqrt((Double_t) sum_wd / sum_w);
505void FitWLSQ::RBM(Double_t *yy, Int_t itera_rb) {
520 sigma_p = 5. - itera_rb * 0.2;
521 if (sigma_p < 3.3) sigma_p = 3.3;
525 sigma_k = resolution;
526 if (sigma_k < resolution)
527 sigma_k = resolution;
529 if (!RB_FIT || (!LH_INI && !itera_rb)) {
530 for (
i = 0;
i < NPoints;
i++) {
534 if ((LH_INI && RB_FIT) || (!LH_INI && itera_rb && RB_FIT)) {
535 for (
i = 0;
i < NPoints;
i++) {
536 if (NParams == 2) fitval =
param[0] +
param[1] * x[
i];
539 if (di < 0) di *= -1;
541 if (di > sigma_p * sigma_k)
wrb[
i] = 0;
542 if (di < sigma_p * Sigma2 && di > sigma_p * Sigma1) {
543 dval = 1 - pow((di / (sigma_p * sigma_k)), 2);
544 wrb[
i] = dval * dval * Sigma1 / (Sigma2 / P1);
546 if (di < sigma_p * Sigma1) {
547 dval = 1 - pow((di / (sigma_p * sigma_k)), 8);
548 wrb[
i] = dval * dval;
552 sum_wd +=
wrb[
i] * di * di;
556 if ((LH_INI && RB_FIT) || (!LH_INI && itera_rb && RB_FIT))
557 sigma_k =
sqrt((Double_t) sum_wd / sum_w);
562void FitWLSQ::RB_OPT(Double_t *yy, Int_t itera_rb) {
568 Double_t sigma_param;
569 Double_t exp1, exp2, a, b, c,
d;
576 sigma_param = 5 - itera_rb;
577 if (sigma_param < 3) sigma_param = 3;
581 sigma_k = resolution;
582 if (sigma_k < resolution)
583 sigma_k = resolution;
586 if (!RB_FIT || (!LH_INI && !itera_rb)) {
587 for (
i = 0;
i < NPoints;
i++) {
591 if ((LH_INI && RB_FIT) || (!LH_INI && itera_rb && RB_FIT)) {
592 for (
i = 0;
i < NPoints;
i++) {
593 if (NParams == 2) fitval =
param[0] +
param[1] * x[
i];
596 if (di < 0) di *= -1;
598 if (di > sigma_param * sigma_k)
wrb[
i] = 0;
599 if (di < sigma_param * sigma_k) {
600 a = P1 * Sigma2 * Sigma2 * Sigma2;
601 b = (1 - P1) * Sigma1 * Sigma1 * Sigma1;
603 d = (1 - P1) * Sigma1;
604 exp1 =
exp((-1. * di * di) / (2. * Sigma1 * Sigma1));
605 exp2 =
exp((-1. * di * di) / (2. * Sigma2 * Sigma2));
606 wrb[
i] = ((a * exp1 + b * exp2) * (c +
d)) / ((c * exp1 +
d * exp2) * (a + b));
612 sum_wd +=
wrb[
i] * di * di;
616 if ((LH_INI && RB_FIT) || (!LH_INI && itera_rb && RB_FIT))
617 sigma_k =
sqrt((Double_t) sum_wd / sum_w);
624 Bool_t end_fit = kFALSE;
626 Double_t par_c[3] = {0};
631 yyf =
new Double_t[NPoints];
637 sigma_k = resolution;
640 for (
int i = 0;
i < NParams;
i++) par_c[
i] =
param[
i];
641 if (RB_TYPE == 0 || RB_TYPE == 1)
643 if (RB_TYPE == 2 || RB_TYPE == 3)
645 if (RB_TYPE == 4 || RB_TYPE == 5)
647 if (RB_TYPE == 6 || RB_TYPE == 7)
653 for (
int i = 0;
i < NPoints;
i++) {
654 if (
wrb[
i]) sumwrb++;
662 for (
int i = 0;
i < NPoints;
i++) {
670 for (
int i = 0;
i < NParams;
i++)
672 end_fit = par_check(
param, par_c);
673 if (!stat)
return kFALSE;
687void FitWLSQ::CovarianceMatrixPrint(
void) {
689 cout <<
"WLSQ Covariance Matrix" << endl;
690 for (
int i = 0;
i < 3;
i++) {
691 for (
int j = 0; j < 3; j++) {
692 cout <<
" " <<
cov[
i][j];
const Float_t d
Z-ccordinate of the first GEM-station.
friend F32vec4 sqrt(const F32vec4 &a)
friend F32vec4 fabs(const F32vec4 &a)
friend F32vec4 log(const F32vec4 &a)
friend F32vec4 exp(const F32vec4 &a)
FitWLSQ(Double_t *xx, Double_t cSigma1=0.01, Double_t cSigma2=0.07, Double_t cP1=0.9, Int_t nNPoints=6, Int_t nNParams=2, Bool_t RBF=kTRUE, Bool_t LHf=kTRUE, Int_t RBfn=7)
Double_t WLSQRms(Double_t *pdY)