00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00024
00025 #include<iostream>
00026 #include<cmath>
00027 #include<cassert>
00028
00029 #include "jbltools/kinfit/WWFitter.h"
00030
00031 #include "jbltools/kinfit/BaseFitObject.h"
00032 #include "jbltools/kinfit/BaseConstraint.h"
00033 #include "jbltools/kinfit/ftypes.h"
00034 #include "jbltools/kinfit/cernlib.h"
00035
00036 using std::cout;
00037 using std::cerr;
00038 using std::endl;
00039 using std::abs;
00040
00041 static int debug = 0;
00042
00043
00044 WWFitter::WWFitter() : npar(0), nmea(0), nunm(0), ncon(0), ierr (0), nit (0) {};
00045
00046
00047 WWFitter::~WWFitter() {};
00048
00049
00050 double WWFitter::fit() {
00051
00052
00053 int inverr = 0;
00054
00055
00056 initialize();
00057
00058
00059 FDouble eta[NPARMAX];
00060 FDouble etasv[NPARMAX];
00061 FDouble y[NPARMAX];
00062
00063 for (unsigned int i = 0; i < fitobjects.size(); ++i) {
00064 for (int ilocal = 0; ilocal < fitobjects[i]->getNPar(); ++ilocal) {
00065 int iglobal = fitobjects[i]->getGlobalParNum (ilocal);
00066 if (iglobal >= 0) {
00067 assert (iglobal >= 0 && iglobal < NPARMAX);
00068 y[iglobal] = eta[iglobal] = fitobjects[i]->getParam(ilocal);
00069 }
00070
00071
00072 }
00073 }
00074
00075 assert (ncon <= NCONMAX);
00077 FDouble dfeta[NCONMAX][NPARMAX];
00078 for (int k=0; k < ncon; k++) {
00079 for (int j=0; j < npar; j++) dfeta[k][j]=0;
00080 constraints[k]->getDerivatives(NPARMAX, dfeta[k]);
00081 if (debug>1) for (int j=0; j < npar; j++)
00082 if (dfeta[k][j]!= 0) cout << "1: dfeta[" << k << "][" << j << "] = " << dfeta[k][j] << endl;
00083 }
00084
00085
00086
00087 FDouble R[NCONMAX];
00088 FDouble S[NCONMAX][NCONMAX];
00089 FDouble W1[NUNMMAX][NUNMMAX];
00090 FDouble V[NPARMAX][NPARMAX];
00091 FDouble VINV[NPARMAX][NPARMAX];
00092 FDouble dxi[NUNMMAX];
00093 FDouble alam[NCONMAX];
00094
00095
00096 double chinew=0, chit=0, chik=0;
00097 double alph = 1.;
00098 nit = 0;
00099
00100
00101 int nitmax = 20;
00102 double chik0 = 100.;
00103 double chit0 = 100.;
00104 double dchikc = 1.0E-3;
00105 double dchitc = 1.0E-4;
00106 double dchikt = 1.0E-2;
00107 double dchik = 1.05;
00108 double chimxw = 10000.;
00109 double almin = 0.05;
00110
00111
00112
00113 bool repeat = true;
00114 bool scut = false;
00115 bool calcerr = true;
00116
00117
00118 while (repeat) {
00119 bool updatesuccess = true;
00120
00121
00122 if (scut) {
00123 for (int j=0; j < npar; j++) eta[j] = etasv[j];
00124 updatesuccess = updateFitObjects (eta);
00125 if (!updatesuccess) {
00126 std::cerr << "WWFitter::fit: old parameters are garbage!" << std::endl;
00127 return -1;
00128 }
00129 for (int k = 0; k < ncon; ++k) {
00130 for (int j=0; j < npar; j++) dfeta[k][j]=0;
00131 constraints[k]->getDerivatives(NPARMAX, dfeta[k]);
00132 if (debug>1) for (int j=0; j < npar; j++)
00133 if (dfeta[k][j]!= 0) cout << "2: dfeta[" << k << "][" << j << "] = " << dfeta[k][j] << endl;
00134 }
00135 }
00136 else {
00137 for (int j=0; j < npar; j++) etasv[j] = eta[j];
00138 chik0 = chik;
00139 chit0 = chit;
00140 }
00141
00142
00143 for (int i = 0; i < npar; ++i)
00144 for (int j = 0; j < npar; ++j) V[i][j] = 0;
00145 for (unsigned int ifitobj = 0; ifitobj<fitobjects.size(); ++ifitobj) {
00146 fitobjects[ifitobj]->addToGlobCov (&V[0][0], NPARMAX);
00147 }
00148 if (debug>1)
00149 for (int i = 0; i < npar; ++i)
00150 for (int j = 0; j < npar; ++j)
00151 if (V[i][j] != 0)
00152 cout << "V[" << i << "][" << j << "]=" << V[i][j] << endl;;
00153 for (int i = 0; i < nmea; ++i)
00154 for (int j = 0; j < nmea; ++j) VINV[i][j] = V[i][j];
00155
00156 if (debug>1)
00157 for (int i = 0; i < npar; ++i)
00158 for (int j = 0; j < npar; ++j)
00159 if (V[i][j] != 0) cout << "V[" << i << "][" << j << "] = " << V[i][j] << endl;
00160
00161
00162 dsinv(nmea, &VINV[0][0], NPARMAX, inverr);
00163 if (inverr != 0) {
00164 if (debug) cerr << "VINV: dsinv error " << inverr << endl;
00165 ierr = 6;
00166 calcerr = false;
00167 if (debug)
00168 for (int i = 0; i < nmea; ++i) {
00169 if (V[i][i] == 0) cout << " zero element! i = " << i << "\n";
00170 for (int j = i; j < nmea; ++j) {
00171 if (V[i][j] != 0)
00172 cout << "...V[" << i << "][" << j << "] = " << V[i][j]
00173 << ", r = " << V[i][j]/std::sqrt(V[i][i]*V[j][j])
00174 << endl;
00175 if (std::abs(V[i][j]-V[j][i])>1E-6*std::abs(V[i][j]+V[j][i]))
00176 cout << "asymmetric: V[i][j]-V[j][i] = " << V[i][j]-V[j][i]
00177 << ", V[i][j]+V[j][i]= " << V[i][j]+V[j][i] << endl;
00178 }
00179 }
00180
00181
00182 FInteger ir[NPARMAX];
00183 dinv(nmea, &VINV[0][0], NPARMAX, ir, inverr);
00184 if (inverr != 0) {
00185 std::cerr << "VINV: dsinv and dinv failed!\n";
00186 break;
00187 }
00188 else {
00189 if (debug) std::cerr << "... but dinv succeeded! => continue.\n";
00190 ierr = 0;
00191 calcerr = true;
00192 }
00193 }
00194
00195
00196 for (int k = 0; k < ncon; ++k) {
00197 R[k] = constraints[k]->getValue();
00198 if (debug>1) cout << "F[" << k << "] = " << R[k] << endl;
00199 for (int j = 0; j < nmea; ++j) {
00200 R[k] += dfeta[k][j]*(y[j]-eta[j]);
00201 }
00202 if (debug>1) cout << "R[" << k << "] = " << R[k] << endl;
00203 for (int l = 0; l < ncon; ++l) {
00204 S[k][l] = 0;
00205 for (int i = 0; i < nmea; ++i) {
00206 for (int j = 0; j < nmea; ++j) {
00207 S[k][l] += dfeta[k][i] * V[i][j] * dfeta[l][j];
00208 }
00209 }
00210
00211
00212
00213 for (int j = 0; j < nunm; ++j) {
00214 S[k][l] += dfeta[k][nmea+j] * dfeta[l][nmea+j];
00215 }
00216 }
00217 }
00218
00219
00220
00221 if (debug>1)
00222 for (int k = 0; k < ncon; ++k)
00223 for (int l = 0; l < ncon; ++l)
00224 if (S[k][l] != 0)
00225 cout << "S[" << k << "][" << l << "]=" << S[k][l] << endl;;
00226
00227
00228
00229
00230
00231
00232 dsinv(ncon, &S[0][0], NCONMAX, inverr);
00233 if (inverr != 0) {
00234 cerr << "S: dsinv error " << inverr << endl;
00235 ierr = 7;
00236 calcerr = false;
00237 break;
00238 }
00239
00240
00241 if (nunm > 0) {
00242 for (int i = 0; i < nunm; ++i) {
00243 for (int j = 0; j < nunm; ++j) {
00244 W1[i][j] = 0;
00245 for (int k = 0; k < ncon; ++k) {
00246 for (int l = 0; l < ncon; ++l) {
00247 W1[i][j] += dfeta[k][nmea+i] * S[k][l] * dfeta[l][nmea+j];
00248 }
00249 }
00250 if (debug>1) cout << "W1[" << i << "][" << j << "] = " << W1[i][j] << endl;
00251 }
00252 }
00253 if (debug > 1) {
00254
00255 for (int i = 0; i < nunm; ++i) {
00256 for (int j = 0; j < nunm; ++j) {
00257 if (abs(W1[i][j]-W1[j][i]) > 1E-3*abs(W1[i][j]+W1[j][i]))
00258 cout << "W1[" << i << "][" << j << "] = " << W1[i][j]
00259 << " W1[" << j << "][" << i << "] = " << W1[j][i]
00260 << " => diff=" << abs(W1[i][j]-W1[j][i])
00261 << " => tol=" << 1E-3*abs(W1[i][j]+W1[j][i])
00262 << endl;
00263 }
00264 }
00265 }
00266
00267
00268
00269
00270 dsinv(nunm, &W1[0][0], NUNMMAX, inverr);
00271 if (inverr != 0) {
00272 cerr << "W1: dsinv error " << inverr << endl;
00273 ierr = 8;
00274 calcerr = false;
00275 break;
00276 }
00277
00278 for (int i = 0; i < nunm; ++i) {
00279 dxi[i] = 0;
00280 for (int j = 0; j < nunm; ++j) {
00281 for (int k = 0; k < ncon; ++k) {
00282 for (int l = 0; l < ncon; ++l) {
00283 dxi[i] -= alph * W1[i][j] * dfeta[k][nmea+j]* S[k][l] * R[l];
00284 }
00285 }
00286 }
00287 if (debug>1) cout << "dxi[" << i << "] = " << dxi[i] << endl;
00288 }
00289 }
00290
00291
00292 for (int i = 0; i < nunm; ++i) {
00293 eta[nmea+i] += dxi[i];
00294 }
00295
00296
00297 for (int k = 0; k < ncon; ++k) {
00298 alam[k] = 0.;
00299 for (int l = 0; l < ncon; ++l) {
00300 alam[k] += S[k][l] * R[l];
00301 if (debug>2) cout << "alam[" << k << "] = " << alam[k] << endl;
00302 for (int j = 0; j < nunm; ++j) {
00303 alam[k] += S[k][l] * dfeta[l][nmea+j] * dxi[j];
00304 if (debug>2) cout << "change of alam[" << k <<
00305 "] for j,l = " << j << " , " << l <<
00306 " : " << S[k][l] * dfeta[l][nmea+j] * dxi[j] << endl;
00307 }
00308 if (debug>2) cout << "alam[" << k << "] = " << alam[k] << endl;
00309 }
00310 if (debug>1) cout << "alam[" << k << "] = " << alam[k] << endl;
00311 }
00312
00313
00314 for (int i = 0; i < nmea; ++i) {
00315 eta[i] = y[i];
00316 for (int j = 0; j < nmea; ++j) {
00317 for (int k = 0; k < ncon; ++k) {
00318 eta[i] -= V[i][j] * dfeta[k][j] * alam[k];
00319 }
00320 }
00321 if (debug>1) cout << "updated eta[" << i << "] = " << eta[i] << endl;
00322 }
00323
00324
00325
00326
00327
00328 updatesuccess = updateFitObjects (eta);
00329
00330 if (debug) {
00331 cout << "After adjustment of all parameters:\n";
00332 for (int k = 0; k < ncon; ++k) {
00333 cout << "Value of constraint " << k << " = " << constraints[k]->getValue()
00334 << endl;
00335 }
00336 }
00337 for (int k=0; k < ncon; k++) {
00338 for (int j=0; j < npar; j++) dfeta[k][j]=0;
00339 constraints[k]->getDerivatives(NPARMAX, dfeta[k]);
00340 if (debug>1)
00341 for (int j=0; j < npar; j++)
00342 if (dfeta[k][j]!= 0) cout << "dfeta[" << k << "][" << j << "] = " << dfeta[k][j] << endl;;
00343 }
00344
00345
00346 chit = 0;
00347 for (int i = 0; i < nmea; ++i)
00348 for (int j = 0; j < nmea; ++j) {
00349 double dchit = (y[i]-eta[i]) * VINV[i][j] * (y[j]-eta[j]);
00350 chit += dchit;
00351 if (debug>1 && dchit != 0)
00352 cout << "chit for i,j = " << i << " , " << j << " = "
00353 << (y[i]-eta[i]) * VINV[i][j] * (y[j]-eta[j]) << endl;
00354 }
00355 chik = 0;
00356 for (int k = 0; k < ncon; ++k) chik += std::abs(2*alam[k]*constraints[k]->getValue());
00357
00358 chinew = chit + chik;
00359
00360
00361 nit++;
00362
00363 bool sconv = (std::abs(chik-chik0) < dchikc)
00364 && (std::abs(chit-chit0) < dchitc*chit)
00365 && (chik < dchikt*chit);
00366
00367
00368
00369
00370
00371
00372
00373 double eps = 1E-6;
00374 bool sconv2 = true;
00375 for (int k = 0; sconv2 && (k < ncon); ++k)
00376 sconv2 &= (std::abs(constraints[k]->getValue()) < eps);
00377 if (sconv2 && debug)
00378 cout << "All constraints fulfilled to better than " << eps << endl;
00379
00380 for (int j = 0; sconv2 && (j < npar); ++j)
00381 sconv2 &= (std::abs(eta[j] - etasv[j]) < eps);
00382 if (sconv2 && debug)
00383 cout << "All parameters stable to better than " << eps << endl;
00384 sconv |= sconv2;
00385
00386 bool sbad = (chik > dchik*chik0)
00387 && (chik > dchikt*chit)
00388 && (chik > chik0 + 1.E-10);
00389
00390 scut = false;
00391
00392 if (nit > nitmax) {
00393
00394 repeat = false;
00395 ierr = 1;
00396 }
00397 else if (sconv && updatesuccess) {
00398
00399 repeat = false;
00400 ierr = 0;
00401 }
00402 else if ( nit > 2 && chinew > chimxw && updatesuccess) {
00403
00404 repeat = false;
00405 calcerr = false;
00406 ierr = 2;
00407 }
00408 else if (sbad && nit > 1 || !updatesuccess) {
00409
00410 if ( alph == almin ) {
00411 repeat = true;
00412 calcerr = false;
00413 ierr = 3;
00414 }
00415 else {
00416 alph = std::max (almin, 0.5 * alph);
00417 scut = true;
00418 repeat = true;
00419 ierr = 4;
00420 }
00421 }
00422 else {
00423
00424 alph = std::min (alph+0.1, 1. );
00425 repeat = true;
00426 ierr = 5;
00427 }
00428
00429 if (debug) cout << "======== NIT = " << nit << ", CHI2 = " << chinew
00430 << ", ierr = " << ierr << ", alph=" << alph << endl;
00431
00432 if (debug)
00433 for (unsigned int i = 0; i < fitobjects.size(); ++i)
00434 cout << "fitobject " << i << ": " << *fitobjects[i] << endl;
00435
00436 }
00437
00438
00439 FDouble VETA[NPARMAX][NPARMAX];
00440 for (int i = 0; i < npar; ++i)
00441 for (int j = 0; j < npar; ++j) VETA[i][j] = 0;
00442
00443 if (calcerr) {
00444
00445
00446 for (int k = 0; k < ncon; ++k) {
00447 for (int l = 0; l < ncon; ++l) {
00448 S[k][l] = 0;
00449 for (int i = 0; i < npar; ++i) {
00450 for (int j = 0; j < npar; ++j) {
00451 S[k][l] += dfeta[k][i] * V[i][j] * dfeta[l][j];
00452 }
00453 }
00454
00455
00456
00457 for (int j = 0; j < nunm; ++j) {
00458 S[k][l] += dfeta[k][nmea+j] * dfeta[l][nmea+j];
00459 }
00460 }
00461 }
00462
00463
00464
00465
00466
00467
00468 dsinv(ncon, &S[0][0], NCONMAX, inverr);
00469 if (inverr != 0) cerr << "S(2): dsinv error " << inverr << endl;
00470
00471
00472
00473 FDouble G[NPARMAX][NPARMAX];
00474 for (int i = 0; i < nmea; ++i) {
00475 for (int j = 0; j < nmea; ++j) {
00476 G[i][j] = 0;
00477 for (int k = 0; k < ncon; ++k) {
00478 for (int l = 0; l < ncon; ++l) {
00479 G[i][j] += dfeta[k][i] * S[k][l] * dfeta[l][j];
00480 }
00481 }
00482 }
00483 }
00484
00485 if (nunm > 0) {
00486
00487
00488 FDouble H[NPARMAX][NUNMMAX];
00489 for (int i = 0; i < nmea; ++i) {
00490 for (int j = 0; j < nunm; ++j) {
00491 H[i][j] = 0;
00492 for (int k = 0; k < ncon; ++k) {
00493 for (int l = 0; l < ncon; ++l) {
00494 H[i][j] += dfeta[k][i] * S[k][l] * dfeta[l][nmea+j];
00495 }
00496 }
00497 }
00498 }
00499
00500
00501
00502 FDouble U[NUNMMAX][NUNMMAX];
00503 for (int i = 0; i < nunm; ++i) {
00504 for (int j = 0; j < nunm; ++j) {
00505 U[i][j] = 0;
00506 for (int k = 0; k < ncon; ++k) {
00507 for (int l = 0; l < ncon; ++l) {
00508 U[i][j] += dfeta[k][nmea+i] * S[k][l] * dfeta[l][nmea+j];
00509 }
00510 }
00511 }
00512 }
00513 dsinv(nunm, &U[0][0], NUNMMAX, inverr);
00514 if (inverr != 0) {
00515 cerr << "U: dsinv error " << inverr << endl;
00516 return -1;
00517 }
00518
00519
00520 for (int i = 0; i < nunm; ++i) {
00521 for (int j = 0; j < nunm; ++j) {
00522 VETA[nmea+i][nmea+j] = U[i][j];
00523 }
00524 }
00525
00526
00527 for (int i = 0; i < nmea; ++i) {
00528 for (int j = 0; j < nunm; ++j) {
00529 VETA[i][nmea+j] = 0.;
00530 for (int ii = 0; ii < nmea; ++ii) {
00531 for (int jj = 0; jj < nunm; ++jj) {
00532 VETA[i][nmea+j] -= V[i][ii] * H[ii][jj] * U[jj][j];
00533 }
00534 }
00535 }
00536 }
00537
00538
00539 for (int i = 0; i < nmea; ++i) {
00540 for (int j = 0; j < nunm; ++j) {
00541 VETA[nmea+j][i] = VETA[i][nmea+j];
00542 }
00543 }
00544
00545
00546 for (int i = 0; i < nmea; ++i) {
00547 for (int j = 0; j < nmea; ++j) {
00548 for (int ii = 0; ii < nunm; ++ii) {
00549 for (int jj = 0; jj < nunm; ++jj) {
00550 G[i][j] -= H[i][ii] * U[ii][jj] * H[j][jj];
00551 }
00552 }
00553 }
00554 }
00555
00556 }
00557
00558
00559 FDouble GV[NPARMAX][NPARMAX];
00560 for (int i = 0; i < nmea; ++i) {
00561 for (int j = 0; j < nmea; ++j) {
00562 if (i == j) GV[i][j] = 1.;
00563 else GV[i][j] = 0.;
00564 for (int ii = 0; ii < nmea; ++ii) {
00565 GV[i][j] -= G[i][ii] * V[ii][j];
00566 }
00567 }
00568 }
00569
00570
00571 for (int i = 0; i < nmea; ++i) {
00572 for (int j = 0; j < nmea; ++j) {
00573 VETA[i][j] = 0.;
00574 for (int ii = 0; ii < nmea; ++ii) {
00575 VETA[i][j] += V[i][ii] * GV[ii][j];
00576 }
00577 }
00578 }
00579
00580
00581
00582 for (unsigned int ifitobj = 0; ifitobj < fitobjects.size(); ++ifitobj) {
00583 for (int ilocal = 0; ilocal < fitobjects[ifitobj]->getNPar(); ++ilocal) {
00584 int iglobal = fitobjects[ifitobj]->getGlobalParNum (ilocal);
00585 for (int jlocal = ilocal; jlocal < fitobjects[ifitobj]->getNPar(); ++jlocal) {
00586 int jglobal = fitobjects[ifitobj]->getGlobalParNum (jlocal);
00587 if (iglobal >= 0 && jglobal >= 0)
00588 fitobjects[ifitobj]->setCov(ilocal, jlocal, VETA[iglobal][jglobal]);
00589 }
00590 }
00591 }
00592
00593 }
00594
00595
00596 FReal chi = FReal(chinew);
00597 fitprob = (ncon-nunm > 0) ? prob(chi,ncon-nunm) : 0.5;
00598 chi2 = chinew;
00599
00600 return fitprob;
00601
00602 };
00603
00604 bool WWFitter::initialize() {
00605
00606 int iglobal = 0;
00607
00608 for (unsigned int ifitobj = 0; ifitobj < fitobjects.size(); ++ifitobj) {
00609 for (int ilocal = 0; ilocal < fitobjects[ifitobj]->getNPar(); ++ilocal) {
00610 if (fitobjects[ifitobj]->isParamMeasured(ilocal) &&
00611 !fitobjects[ifitobj]->isParamFixed(ilocal)) {
00612 fitobjects[ifitobj]->setGlobalParNum (ilocal, iglobal);
00613 if (debug)
00614 cout << "Object " << fitobjects[ifitobj]->getName()
00615 << " Parameter " << fitobjects[ifitobj]->getParamName(ilocal)
00616 << " is measured, global number " << iglobal << endl;
00617 ++iglobal;
00618 }
00619 }
00620 }
00621 nmea = iglobal;
00622
00623 for (unsigned int ifitobj = 0; ifitobj < fitobjects.size(); ++ifitobj) {
00624 for (int ilocal = 0; ilocal < fitobjects[ifitobj]->getNPar(); ++ilocal) {
00625 if (!fitobjects[ifitobj]->isParamMeasured(ilocal) &&
00626 !fitobjects[ifitobj]->isParamFixed(ilocal)) {
00627 fitobjects[ifitobj]->setGlobalParNum (ilocal, iglobal);
00628 if (debug)
00629 cout << "Object " << fitobjects[ifitobj]->getName()
00630 << " Parameter " << fitobjects[ifitobj]->getParamName(ilocal)
00631 << " is unmeasured, global number " << iglobal << endl;
00632 ++iglobal;
00633 }
00634 }
00635 }
00636 npar = iglobal;
00637 assert (npar <= NPARMAX);
00638 nunm = npar - nmea;
00639 assert (nunm <= NUNMMAX);
00640
00641
00642 ncon = constraints.size();
00643 assert (ncon <= NCONMAX);
00644
00645 return true;
00646
00647 };
00648
00649 bool WWFitter::updateFitObjects (double eta[]) {
00650 bool debug = false;
00651 bool result = true;
00652 for (unsigned int ifitobj = 0; ifitobj < fitobjects.size(); ++ifitobj) {
00653 for (int ilocal = 0; ilocal < fitobjects[ifitobj]->getNPar(); ++ilocal) {
00654 int iglobal = fitobjects[ifitobj]->getGlobalParNum (ilocal);
00655 if (!fitobjects[ifitobj]->isParamFixed (ilocal) && iglobal >= 0) {
00656 if (debug) cout << "Parameter " << iglobal
00657 << " (" << fitobjects[ifitobj]->getName()
00658 << ": " << fitobjects[ifitobj]->getParamName(ilocal)
00659 << ") set to " << eta[iglobal];
00660 result &= fitobjects[ifitobj]->setParam(ilocal, eta[iglobal]);
00661 eta[iglobal] = fitobjects[ifitobj]->getParam(ilocal);
00662 if (debug) cout << " => " << eta[iglobal] << endl;
00663 }
00664 }
00665 }
00666 return result;
00667 };
00668
00669 int WWFitter::getError() const {return ierr;}
00670 double WWFitter::getProbability() const {return fitprob;}
00671 double WWFitter::getChi2() const {return chi2;}
00672 int WWFitter::getIterations() const {return nit;}