27 #include <TDecompChol.h>
28 #include <TMatrixDSymEigen.h>
29 #include <TMatrixTSymCramerInv.h>
44 if (!(mat<1.E100) || !(mat>-1.E100)){
45 Exception e(
"Tools::invertMatrix() - cannot invert matrix, entries too big (>1e100)",
51 if (mat.GetNrows() == 1){
52 if (determinant != NULL) *determinant = mat(0,0);
53 inv(0,0) = 1./mat(0,0);
57 if (mat.GetNrows() == 2){
58 double det = mat(0,0)*mat(1,1) - mat(1,0)*mat(1,0);
59 if (determinant != NULL) *determinant = det;
60 if(fabs(det) < 1E-50){
61 Exception e(
"Tools::invertMatrix() - cannot invert matrix , determinant = 0",
67 inv(0,0) = det * mat(1,1);
68 inv(0,1) = inv(1,0) = -det * mat(1,0);
69 inv(1,1) = det * mat(0,0);
75 Bool_t (*inversion)(TMatrixDSym&, Double_t*) = 0;
78 switch (mat.GetNrows()) {
80 inversion = TMatrixTSymCramerInv::Inv3x3;
break;
82 inversion = TMatrixTSymCramerInv::Inv4x4;
break;
84 inversion = TMatrixTSymCramerInv::Inv5x5;
break;
86 inversion = TMatrixTSymCramerInv::Inv6x6;
break;
89 Bool_t success = inversion(inv, determinant);
91 Exception e(
"Tools::invertMatrix() - cannot invert matrix, determinant = 0",
101 TDecompChol invertAlgo(mat, 1E-50);
103 status = invertAlgo.Invert(inv);
105 Exception e(
"Tools::invertMatrix() - cannot invert matrix, status = 0",
111 if (determinant != NULL) {
113 invertAlgo.Det(d1, d2);
114 *determinant = ldexp(d1, d2);
120 if (!(mat<1.E100) || !(mat>-1.E100)){
121 Exception e(
"Tools::invertMatrix() - cannot invert matrix, entries too big (>1e100)",
127 if (mat.GetNrows() == 1){
128 if (determinant != NULL) *determinant = mat(0,0);
129 mat(0,0) = 1./mat(0,0);
133 if (mat.GetNrows() == 2){
134 double *arr = mat.GetMatrixArray();
135 double det = arr[0]*arr[3] - arr[1]*arr[1];
136 if (determinant != NULL) *determinant = det;
137 if(fabs(det) < 1E-50){
138 Exception e(
"Tools::invertMatrix() - cannot invert matrix, determinant = 0",
145 temp[0] = det * arr[3];
146 temp[1] = -det * arr[1];
147 temp[2] = det * arr[0];
150 arr[1] = arr[2] = temp[1];
156 Bool_t (*inversion)(TMatrixDSym&, Double_t*) = 0;
157 switch (mat.GetNrows()) {
159 inversion = TMatrixTSymCramerInv::Inv3x3;
break;
161 inversion = TMatrixTSymCramerInv::Inv4x4;
break;
163 inversion = TMatrixTSymCramerInv::Inv5x5;
break;
165 inversion = TMatrixTSymCramerInv::Inv6x6;
break;
168 Bool_t success = inversion(mat, determinant);
170 Exception e(
"Tools::invertMatrix() - cannot invert matrix, determinant = 0",
180 TDecompChol invertAlgo(mat, 1E-50);
182 status = invertAlgo.Invert(mat);
184 Exception e(
"Tools::invertMatrix() - cannot invert matrix, status = 0",
190 if (determinant != NULL) {
192 invertAlgo.Det(d1, d2);
193 *determinant = ldexp(d1, d2);
203 size_t n = R.GetNrows();
204 double *
const bk = b.GetMatrixArray();
205 const double *
const Rk = R.GetMatrixArray();
206 for (
unsigned int i = 0; i < n; ++i) {
208 for (
unsigned int j = 0; j < i; ++j) {
209 sum -= bk[j]*Rk[j*n + i];
213 bk[i] = sum / Rk[i*n + i];
223 size_t n = R.GetNrows();
224 double *
const bk = b.GetMatrixArray() + nCol;
225 const double *
const Rk = R.GetMatrixArray();
226 for (
unsigned int i = nCol; i < n; ++i) {
227 double sum = (i == (size_t)nCol);
228 for (
unsigned int j = 0; j < i; ++j) {
229 sum -= bk[j*n]*Rk[j*n + i];
233 bk[i*n] = sum / Rk[i*n + i];
245 double *
const invk = inv.GetMatrixArray();
246 int nRows = inv.GetNrows();
247 for (
int i = 0; i < nRows; ++i)
248 for (
int j = 0; j < nRows; ++j)
249 invk[i*nRows + j] = (i == j);
251 for (
int i = 0; i < inv.GetNcols(); ++i)
262 int nCols = A.GetNcols();
263 int nRows = A.GetNrows();
264 assert(nRows >= nCols);
270 double *
const ak = A.GetMatrixArray();
272 double *
const u = (
double *)alloca(
sizeof(
double)*nRows);
275 for (
int k = 0; k < nCols; ++k) {
276 double akk = ak[k*nCols + k];
278 double sum = akk*akk;
280 for (
int i = k + 1; i < nRows; ++i) {
281 sum += ak[i*nCols + k]*ak[i*nCols + k];
282 u[i] = ak[i*nCols + k];
284 double sigma = sqrt(sum);
285 double beta = 1/(sum + sigma*fabs(akk));
287 u[k] = copysign(sigma + fabs(akk), akk);
291 for (
int i = k; i < nCols; ++i) {
293 for (
int j = k; j < nRows; ++j)
294 y += u[j]*ak[j*nCols + i];
297 for (
int j = k; j < nRows; ++j)
298 ak[j*nCols + i] -= u[j]*y;
303 for (
int i = 1; i < nCols; ++i)
304 for (
int j = 0; j < i; ++j)
305 ak[i*nCols + j] = 0.;
306 for (
int i = nCols; i < nRows; ++i)
307 for (
int j = 0; j < nCols; ++j)
308 ak[i*nCols + j] = 0.;
323 int nCols = A.GetNcols();
324 int nRows = A.GetNrows();
325 assert(nRows >= nCols);
326 assert(b.GetNrows() == nRows);
334 double *
const ak = A.GetMatrixArray();
335 double *
const bk = b.GetMatrixArray();
341 for (
int k = 0; k < nCols; ++k) {
342 double akk = ak[k*nCols + k];
344 double sum = akk*akk;
346 for (
int i = k + 1; i < nRows; ++i) {
347 sum += ak[i*nCols + k]*ak[i*nCols + k];
348 u[i] = ak[i*nCols + k];
350 double sigma = sqrt(sum);
351 double beta = 1/(sum + sigma*fabs(akk));
353 u[k] = copysign(sigma + fabs(akk), akk);
358 for (
int j = k; j < nRows; ++j)
362 for (
int j = k; j < nRows; ++j)
367 for (
int i = k; i < nCols; ++i) {
369 for (
int j = k; j < nRows; ++j)
370 y += u[j]*ak[j*nCols + i];
373 for (
int j = k; j < nRows; ++j)
374 ak[j*nCols + i] -= u[j]*y;
379 for (
int i = 1; i < nCols; ++i)
380 for (
int j = 0; j < i; ++j)
381 ak[i*nCols + j] = 0.;
382 for (
int i = nCols; i < nRows; ++i)
383 for (
int j = 0; j < nCols; ++j)
384 ak[i*nCols + j] = 0.;
411 TDecompChol dec1(C1);
413 TDecompChol dec2(C2);
416 const TMatrixD& S1 = dec1.GetU();
417 const TMatrixD& S2 = dec2.GetU();
419 TMatrixD S1inv, S2inv;
423 TMatrixD A(2 * S1.GetNrows(), S1.GetNcols());
424 for (
int i = 0; i < S1.GetNrows(); ++i) {
425 for (
int j = 0; j < S2.GetNcols(); ++j) {
426 A(i, j) = S1inv(i, j);
427 A(i + S1.GetNrows(), j) = S2inv(i, j);
432 A.ResizeTo(S1.GetNrows(), S1.GetNrows());
437 result.ResizeTo(inv.GetNcols(), inv.GetNcols());
438 result = TMatrixDSym(TMatrixDSym::kAtA, inv);
448 TMatrixDSymEigen eig(noise);
449 noiseSqrt.ResizeTo(noise);
450 noiseSqrt = eig.GetEigenVectors();
451 double* pNoiseSqrt = noiseSqrt.GetMatrixArray();
452 const TVectorD& evs(eig.GetEigenValues());
453 const double* pEvs = evs.GetMatrixArray();
459 for (; iCol < noiseSqrt.GetNrows(); ++iCol) {
460 double ev = pEvs[iCol] > 0 ? sqrt(pEvs[iCol]) : 0;
463 for (
int j = 0; j < noiseSqrt.GetNrows(); ++j) {
464 pNoiseSqrt[j*noiseSqrt.GetNcols() + iCol] *= ev;
467 if (iCol < noiseSqrt.GetNcols()) {
469 noiseSqrt.ResizeTo(noiseSqrt.GetNrows(), iCol);
478 const TVectorD& delta,
const TMatrixD& F,
493 const TMatrixD& F,
const TMatrixD& Q,
496 Snew.ResizeTo(S.GetNrows() + Q.GetNcols(),
500 Snew.SetSub(0, 0, TMatrixD(S, TMatrixD::kMultTranspose, F));
501 if (Q.GetNcols() != 0)
502 Snew.SetSub(S.GetNrows(), 0, TMatrixD(TMatrixD::kTransposed, Q));
507 Snew.ResizeTo(S.GetNrows(), S.GetNrows());
518 const TVectorD& res,
const TMatrixD& R,
520 TVectorD& update, TMatrixD& SNew)
522 TMatrixD pre(S.GetNrows() + R.GetNrows(),
523 S.GetNcols() + R.GetNcols());
525 pre.SetSub(R.GetNrows(), 0, H->
MHt(S)); pre.SetSub(R.GetNrows(), R.GetNcols(), S);
528 const TMatrixD& r = pre;
530 const TMatrixD& a(r.GetSub(0, R.GetNrows()-1,
532 TMatrixD K(TMatrixD::kTransposed, r.GetSub(0, R.GetNrows()-1, R.GetNcols(), pre.GetNcols()-1));
533 SNew = r.GetSub(R.GetNrows(), pre.GetNrows()-1, R.GetNcols(), pre.GetNcols()-1);
535 update.ResizeTo(res);
549 const TMatrixD& F,
const TMatrixD& Q,
550 const TVectorD& res,
const TMatrixD& R,
552 TVectorD& update, TMatrixD& SNew)
554 TMatrixD pre(S.GetNrows() + Q.GetNrows() + R.GetNrows(),
555 S.GetNcols() + R.GetNcols());
556 TMatrixD SFt(S, TMatrixD::kMultTranspose, F);
557 pre.SetSub( 0, 0, R);
558 pre.SetSub( R.GetNrows(), 0,H->
MHt(SFt)); pre.SetSub(R.GetNrows(), R.GetNcols(),SFt);
559 if (Q.GetNcols() > 0) {
560 TMatrixD Qt(TMatrixD::kTransposed, Q);
561 pre.SetSub(S.GetNrows()+R.GetNrows(),0,H->
MHt(Qt)); pre.SetSub(S.GetNrows()+R.GetNrows(),R.GetNcols(), Qt);
565 const TMatrixD& r = pre;
567 TMatrixD a(r.GetSub(0, R.GetNrows()-1, 0, R.GetNcols()-1));
568 TMatrixD K(TMatrixD::kTransposed, r.GetSub(0, R.GetNrows()-1, R.GetNcols(), pre.GetNcols()-1));
569 SNew = r.GetSub(R.GetNrows(), R.GetNrows() + S.GetNrows() - 1,
570 R.GetNcols(), pre.GetNcols() - 1);
572 update.ResizeTo(res);