27 #include <TDecompChol.h>
28 #include <TMatrixDSymEigen.h>
29 #include <TMatrixTSymCramerInv.h>
42 if (!(mat<1.E100) || !(mat>-1.E100)){
43 Exception e(
"Tools::invertMatrix() - cannot invert matrix, entries too big (>1e100)",
49 if (mat.GetNrows() == 1){
50 if (determinant !=
nullptr) *determinant = mat(0,0);
51 inv(0,0) = 1./mat(0,0);
55 if (mat.GetNrows() == 2){
56 double det = mat(0,0)*mat(1,1) - mat(1,0)*mat(1,0);
57 if (determinant !=
nullptr) *determinant = det;
58 if(fabs(det) < 1
E-50){
59 Exception e(
"Tools::invertMatrix() - cannot invert matrix , determinant = 0",
65 inv(0,0) = det * mat(1,1);
66 inv(0,1) = inv(1,0) = -det * mat(1,0);
67 inv(1,1) = det * mat(0,0);
73 TDecompChol invertAlgo(mat, 1
E-50);
75 status = invertAlgo.Invert(inv);
77 Exception e(
"Tools::invertMatrix() - cannot invert matrix, status = 0",
83 if (determinant !=
nullptr) {
85 invertAlgo.Det(d1, d2);
86 *determinant = ldexp(d1, d2);
92 if (!(mat<1.E100) || !(mat>-1.E100)){
93 Exception e(
"Tools::invertMatrix() - cannot invert matrix, entries too big (>1e100)",
99 if (mat.GetNrows() == 1){
100 if (determinant !=
nullptr) *determinant = mat(0,0);
101 mat(0,0) = 1./mat(0,0);
105 if (mat.GetNrows() == 2){
106 double *
arr = mat.GetMatrixArray();
107 double det = arr[0]*arr[3] - arr[1]*arr[1];
108 if (determinant !=
nullptr) *determinant = det;
109 if(fabs(det) < 1
E-50){
110 Exception e(
"Tools::invertMatrix() - cannot invert matrix, determinant = 0",
117 temp[0] = det * arr[3];
118 temp[1] = -det * arr[1];
119 temp[2] = det * arr[0];
122 arr[1] = arr[2] = temp[1];
129 TDecompChol invertAlgo(mat, 1
E-50);
131 status = invertAlgo.Invert(mat);
133 Exception e(
"Tools::invertMatrix() - cannot invert matrix, status = 0",
139 if (determinant !=
nullptr) {
141 invertAlgo.Det(d1, d2);
142 *determinant = ldexp(d1, d2);
152 size_t n = R.GetNrows();
153 double *
const bk = b.GetMatrixArray();
154 const double *
const Rk = R.GetMatrixArray();
155 for (
unsigned int i = 0;
i <
n; ++
i) {
157 for (
unsigned int j = 0;
j <
i; ++
j) {
158 sum -= bk[
j]*Rk[
j*n +
i];
162 bk[
i] = sum / Rk[i*n +
i];
172 size_t n = R.GetNrows();
173 double *
const bk = b.GetMatrixArray() + nCol;
174 const double *
const Rk = R.GetMatrixArray();
175 for (
unsigned int i = nCol;
i <
n; ++
i) {
176 double sum = (
i == (size_t)nCol);
177 for (
unsigned int j = 0;
j <
i; ++
j) {
178 sum -= bk[
j*
n]*Rk[
j*n +
i];
182 bk[i*
n] = sum / Rk[i*n +
i];
194 double *
const invk = inv.GetMatrixArray();
195 int nRows = inv.GetNrows();
196 for (
int i = 0;
i < nRows; ++
i)
197 for (
int j = 0;
j < nRows; ++
j)
198 invk[
i*nRows +
j] = (
i ==
j);
200 for (
int i = 0;
i < inv.GetNcols(); ++
i)
211 int nCols = A.GetNcols();
212 int nRows = A.GetNrows();
219 double *
const ak = A.GetMatrixArray();
221 double *
const u = (
double *)alloca(
sizeof(
double)*nRows);
224 for (
int k = 0;
k < nCols; ++
k) {
225 double akk = ak[
k*nCols +
k];
227 double sum = akk*akk;
229 for (
int i =
k + 1;
i < nRows; ++
i) {
230 sum += ak[
i*nCols +
k]*ak[
i*nCols +
k];
231 u[
i] = ak[
i*nCols +
k];
233 double sigma = sqrt(sum);
234 double beta = 1/(sum + sigma*fabs(akk));
240 for (
int i =
k;
i < nCols; ++
i) {
242 for (
int j =
k;
j < nRows; ++
j)
243 y += u[
j]*ak[
j*nCols +
i];
246 for (
int j =
k;
j < nRows; ++
j)
247 ak[
j*nCols +
i] -= u[
j]*y;
252 for (
int i = 1;
i < nCols; ++
i)
253 for (
int j = 0;
j <
i; ++
j)
254 ak[i*nCols +
j] = 0.;
255 for (
int i = nCols; i < nRows; ++
i)
256 for (
int j = 0;
j < nCols; ++
j)
257 ak[i*nCols +
j] = 0.;
272 int nCols = A.GetNcols();
273 int nRows = A.GetNrows();
275 assert(b.GetNrows() == nRows);
283 double *
const ak = A.GetMatrixArray();
284 double *
const bk = b.GetMatrixArray();
290 for (
int k = 0;
k < nCols; ++
k) {
291 double akk = ak[
k*nCols +
k];
293 double sum = akk*akk;
295 for (
int i =
k + 1;
i < nRows; ++
i) {
296 sum += ak[
i*nCols +
k]*ak[
i*nCols +
k];
297 u[
i] = ak[
i*nCols +
k];
299 double sigma = sqrt(sum);
300 double beta = 1/(sum + sigma*fabs(akk));
307 for (
int j =
k;
j < nRows; ++
j)
311 for (
int j =
k;
j < nRows; ++
j)
316 for (
int i =
k;
i < nCols; ++
i) {
318 for (
int j =
k;
j < nRows; ++
j)
319 y += u[
j]*ak[
j*nCols +
i];
322 for (
int j =
k;
j < nRows; ++
j)
323 ak[
j*nCols +
i] -= u[
j]*y;
328 for (
int i = 1;
i < nCols; ++
i)
329 for (
int j = 0;
j <
i; ++
j)
330 ak[i*nCols +
j] = 0.;
331 for (
int i = nCols; i < nRows; ++
i)
332 for (
int j = 0;
j < nCols; ++
j)
333 ak[i*nCols +
j] = 0.;
343 TMatrixDSymEigen eig(noise);
344 noiseSqrt.ResizeTo(noise);
345 noiseSqrt = eig.GetEigenVectors();
346 double* pNoiseSqrt = noiseSqrt.GetMatrixArray();
347 const TVectorD& evs(eig.GetEigenValues());
348 const double* pEvs = evs.GetMatrixArray();
354 for (; iCol < noiseSqrt.GetNrows(); ++iCol) {
355 double ev = pEvs[iCol] > 0 ? sqrt(pEvs[iCol]) : 0;
358 for (
int j = 0;
j < noiseSqrt.GetNrows(); ++
j) {
359 pNoiseSqrt[
j*noiseSqrt.GetNcols() + iCol] *= ev;
362 if (iCol < noiseSqrt.GetNcols()) {
364 noiseSqrt.ResizeTo(noiseSqrt.GetNrows(), iCol);
378 const TMatrixD& F,
const TMatrixD&
Q,
381 Snew.ResizeTo(S.GetNrows() + Q.GetNcols(),
385 Snew.SetSub(0, 0, TMatrixD(S, TMatrixD::kMultTranspose, F));
386 if (Q.GetNcols() != 0)
387 Snew.SetSub(S.GetNrows(), 0, TMatrixD(TMatrixD::kTransposed, Q));
392 Snew.ResizeTo(S.GetNrows(), S.GetNrows());
403 const TVectorD& res,
const TMatrixD&
R,
405 TVectorD&
update, TMatrixD& SNew)
407 TMatrixD pre(S.GetNrows() + R.GetNrows(),
408 S.GetNcols() + R.GetNcols());
410 pre.SetSub(R.GetNrows(), 0, H->
MHt(S)); pre.SetSub(R.GetNrows(), R.GetNcols(),
S);
413 const TMatrixD&
r = pre;
415 const TMatrixD&
a(r.GetSub(0, R.GetNrows()-1,
417 TMatrixD K(TMatrixD::kTransposed, r.GetSub(0, R.GetNrows()-1, R.GetNcols(), pre.GetNcols()-1));
418 SNew = r.GetSub(R.GetNrows(), pre.GetNrows()-1, R.GetNcols(), pre.GetNcols()-1);
420 update.ResizeTo(res);