27 #include <TDecompChol.h>
28 #include <TMatrixDSymEigen.h>
29 #include <TMatrixTSymCramerInv.h>
32 #include "AbsHMatrix.h"
33 #include "Exception.h"
38 void tools::invertMatrix(
const TMatrixDSym& mat, TMatrixDSym& inv,
double* determinant){
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);
90 void tools::invertMatrix(TMatrixDSym& mat,
double* determinant){
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);
150 bool tools::transposedForwardSubstitution(
const TMatrixD&
R, TVectorD& b)
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];
170 bool tools::transposedForwardSubstitution(
const TMatrixD&
R, TMatrixD& b,
int nCol)
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];
189 bool tools::transposedInvert(
const TMatrixD&
R, TMatrixD& inv)
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)
201 result = result && transposedForwardSubstitution(
R, inv, i);
209 void tools::QR(TMatrixD& A)
211 int nCols = A.GetNcols();
212 int nRows = A.GetNrows();
213 assert(nRows >= nCols);
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));
236 u[k] = copysign(sigma + fabs(akk), 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.;
270 void tools::QR(TMatrixD& A, TVectorD& b)
272 int nCols = A.GetNcols();
273 int nRows = A.GetNrows();
274 assert(nRows >= nCols);
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));
302 u[k] = copysign(sigma + fabs(akk), 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.;
338 tools::noiseMatrixSqrt(
const TMatrixDSym& noise,
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);
377 tools::kalmanPredictionCovSqrt(
const TMatrixD& S,
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());
402 tools::kalmanUpdateSqrt(
const TMatrixD& S,
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);
422 tools::transposedForwardSubstitution(a, update);
double sqrt(double a)
sqrt for double
Defines for I/O streams used for error and debug printing.