29 #include "CLHEP/Matrix/SymMatrix.h"
30 #include "CLHEP/Matrix/Matrix.h"
31 #include "CLHEP/Matrix/Vector.h"
41 #include <sys/times.h>
48 return ((
double) usage.tms_utime) / sysconf(_SC_CLK_TCK);
54 return ((
double) times(&usage)) / sysconf(_SC_CLK_TCK);
65 #define NITER 1 // number of iterations
67 #define NLOOP 500000 // number of time the test is repeted
68 #define NLISTSIZE 64 // size of matrix/vector lists
70 using namespace ROOT::Math;
107 std::cout <<
"************************************************\n";
108 std::cout <<
" SMatrix kalman test " << first <<
" x " << second << std::endl;
109 std::cout <<
"************************************************\n";
119 for (
int ipass = 0; ipass <
npass; ipass++) {
123 MnMatrixMN K0[
NLIST];
124 MnSymMatrixMM Cp[
NLIST];
125 MnSymMatrixNN V[
NLIST];
144 std::cout <<
"pass " << ipass << std::endl;
146 std::cout <<
" K0 = " << K0[0] << std::endl;
147 std::cout <<
" H = " << K0[0] << std::endl;
148 std::cout <<
" Cp = " << Cp[0] << std::endl;
149 std::cout <<
" V = " << V[0] << std::endl;
150 std::cout <<
" m = " << m[0] << std::endl;
151 std::cout <<
" xp = " << xp[0] << std::endl;
171 for (
int k = 0; k <
NLIST; k++) {
175 vtmp1 = H[k]*xp[k] -m[k];
177 x = xp[k] - K0[k] * vtmp1;
179 Rinv = V[k]; Rinv += H[k] * tmp;
181 bool test = Rinv.InvertFast();
183 std::cout<<
"inversion failed" <<std::endl;
184 std::cout << Rinv << std::endl;
191 vtmp = m[k]-H[k]*xp[k];
192 x2 =
Dot(vtmp, Rinv*vtmp);
198 for (
int i=0; i<
NDIM2; ++i)
199 for (
int j=0; j<
NDIM2; ++j)
209 std::cerr <<
"SMatrix: x2sum = " << x2sum <<
"\tc2sum = " << c2sum << std::endl;
211 double sx2=0;
double sc2=0;
216 std::cerr <<
"SMatrix: x2sum = " << sx2 <<
"\tc2sum = " << sc2 << std::endl;
247 std::cout <<
"************************************************\n";
248 std::cout <<
" SMatrix_SyM kalman test " << first <<
" x " << second << std::endl;
249 std::cout <<
"************************************************\n";
255 Stype x2sum = 0.0, c2sum = 0.0;
257 for (
int ipass = 0; ipass <
npass; ipass++) {
261 MnMatrixMN K0[
NLIST];
262 MnSymMatrixMM Cp[
NLIST];
263 MnSymMatrixNN V[
NLIST];
283 std::cout <<
"pass " << ipass << std::endl;
285 std::cout <<
" K0 = " << K0 << std::endl;
286 std::cout <<
" H = " << K0 << std::endl;
287 std::cout <<
" Cp = " << Cp << std::endl;
288 std::cout <<
" V = " << V << std::endl;
289 std::cout <<
" m = " << m << std::endl;
290 std::cout <<
" xp = " << xp << std::endl;
297 Stype x2 = 0.0,
c2 = 0.0;
301 MnSymMatrixNN RinvSym;
307 #define OPTIMIZED_SMATRIX_SYM
308 #ifdef OPTIMIZED_SMATRIX_SYM
313 for (
int k = 0; k <
NLIST; k++) {
316 #ifdef OPTIMIZED_SMATRIX_SYM
317 vtmp1 = H[k]*xp[k] -m[k];
319 x = xp[k] - K0[k] * vtmp1;
325 bool test = RinvSym.InvertFast();
327 std::cout<<
"inversion failed" <<std::endl;
328 std::cout << RinvSym << std::endl;
334 C = Cp[k]; C -= Ctmp;
337 vtmp = m[k]-H[k]*xp[k];
338 x2 =
Dot(vtmp, RinvSym*vtmp);
341 vtmp1 = H[k]*xp[k] -m[k];
342 x = xp[k] - K0[k] * vtmp1;
343 RinvSym = V[k]; RinvSym +=
Similarity(H[k],Cp[k]);
345 bool test = RinvSym.InvertFast();
347 std::cout<<
"inversion failed" <<std::endl;
348 std::cout << RinvSym << std::endl;
353 vtmp = m[k]-H[k]*xp[k];
359 for (
int i=0; i<
NDIM2; ++i)
360 for (
int j=0; j<
NDIM2; ++j)
374 std::cerr <<
"SMatrixSym: x2sum = " << x2sum <<
"\tc2sum = " << c2sum << std::endl;
376 double sx2=0;
double sc2=0;
381 std::cerr <<
"SMatrixSym: x2sum = " << sx2 <<
"\tc2sum = " << sc2 << std::endl;
414 std::cout <<
"************************************************\n";
415 std::cout <<
" TMatrix Kalman test " << first <<
" x " << second << std::endl;
416 std::cout <<
"************************************************\n";
423 Stype x2sum = 0,c2sum = 0;
425 for (
int k = 0; k <
npass; k++)
428 MnMatrix
H(first,second);
429 MnMatrix K0(second,first);
430 MnMatrix Cp(second,second);
431 MnMatrix V(first,first);
444 MnMatrix
I(second,second);
445 for (
int i = 0; i < second; i++)
446 for(
int j = 0; j <second; j++) {
448 if (i==j)
I(i,i) = 1.0;
467 #define OPTIMIZED_TMATRIX
468 #ifndef OPTIMIZED_TMATRIX
476 #ifdef OPTIMIZED_TMATRIX
477 tmp1 =
m;
Add(tmp1,-1.0,H,xp);
478 x = xp;
Add(x,+1.0,K0,tmp1);
494 Rinv = V; Rinv += H * tmp2;
505 for (
int i=0; i<
NDIM2; ++i)
506 for (
int j=0; j<
NDIM2; ++j)
514 std::cerr <<
"TMatrix: x2sum = " << x2sum <<
"\tc2sum = " << c2sum << std::endl;
526 int test_clhep_kalman() {
530 typedef HepSymMatrix MnSymMatrix;
531 typedef HepMatrix MnMatrix;
532 typedef HepVector MnVector;
543 std::cout <<
"************************************************\n";
544 std::cout <<
" CLHEP Kalman test " << first <<
" x " << second << std::endl;
545 std::cout <<
"************************************************\n";
551 Stype x2sum = 0,c2sum = 0;
553 for (
int k = 0; k <
npass; k++)
557 MnMatrix
H(first,second);
558 MnMatrix K0(second,first);
559 MnMatrix Cp(second,second);
560 MnMatrix V(first,first);
572 MnSymMatrix
I(second,1);
577 MnMatrix Rinv(first,first);
578 MnSymMatrix RinvSym(first);
579 MnMatrix
K(second,first);
580 MnSymMatrix
C(second);
581 MnVector vtmp1(first);
582 MnMatrix tmp(second,first);
594 Rinv = V; Rinv += H * tmp;
595 RinvSym.assign(Rinv);
596 RinvSym.invert(ifail);
597 if (ifail !=0) { std::cout <<
"Error inverting Rinv" << std::endl;
break; }
601 C.assign( (
I-
K*H)*Cp );
602 x2= RinvSym.similarity(vtmp1);
603 if(ifail!=0) { std::cout <<
"Error inverting Rinv" << std::endl;
break; }
609 for (
int i=1; i<=
NDIM2; ++i)
610 for (
int j=1; j<=
NDIM2; ++j)
618 std::cerr <<
"x2sum = " << x2sum <<
"\tc2sum = " << c2sum << std::endl;
634 std::cout <<
" making vector/matrix lists of size = " <<
NLIST << std::endl;
T Dot(const SVector< T, D > &lhs, const SVector< T, D > &rhs)
Vector dot product.
T Similarity(const SMatrix< T, D, D, R > &lhs, const SVector< T, D > &rhs)
Similarity Vector - Matrix Product: v^T * A * v returning a scalar value of type T ...
Random number generator class based on M.
TMatrixT< Element > & Transpose(const TMatrixT< Element > &source)
Transpose matrix source.
TMatrixTSym< Element > & Use(Int_t row_lwb, Int_t row_upb, Element *data)
TMatrixT< Element > & Add(TMatrixT< Element > &target, Element scalar, const TMatrixT< Element > &source)
Modify addition: target += scalar * source.
Expr< TransposeOp< SMatrix< T, D1, D2, R >, T, D1, D2 >, T, D2, D1, typename TranspPolicy< T, D1, D2, R >::RepType > Transpose(const SMatrix< T, D1, D2, R > &rhs)
Matrix Transpose B(i,j) = A(j,i) returning a matrix expression.
void fillRandomVec(TRandom &r, V &v, unsigned int len, unsigned int start=0, double offset=1)
void fillRandomSym(TRandom &r, M &m, unsigned int first, unsigned int start=0, double offset=1)
void Minus(const TMatrixT< Element > &a, const TMatrixT< Element > &b)
General matrix summation. Create a matrix C such that C = A - B.
static const double x2[5]
SMatrix: a generic fixed size D1 x D2 Matrix class.
void MultT(const TMatrixT< Element > &a, const TMatrixT< Element > &b)
General matrix multiplication. Create a matrix C such that C = A * B^T.
int test_smatrix_kalman()
RooCmdArg Timer(Bool_t flag=kTRUE)
VECTOR_NAMESPACE::double_v double_v
int test_tmatrix_kalman()
virtual const Element * GetMatrixArray() const
R__EXTERN Int_t gMatrixCheck
static void Evaluate(SMatrix< T, D, D, MatRepSym< T, D > > &lhs, const Expr< A, T, D, D, R > &rhs)
assign a symmetric matrix from an expression
void fillRandomMat(TRandom &r, M &m, unsigned int first, unsigned int second, unsigned int start=0, double offset=1)
TMatrixTSym< Element > & InvertFast(Double_t *det=0)
Invert the matrix and calculate its determinant.
TMatrixTSym< Element > & Similarity(const TMatrixT< Element > &n)
Calculate B * (*this) * B^T , final matrix will be (nrowsb x nrowsb) This is a similarity transform w...
void Plus(const TMatrixT< Element > &a, const TMatrixT< Element > &b)
General matrix summation. Create a matrix C such that C = A + B.
SMatrix< T, D2, D2, MatRepSym< T, D2 > > SimilarityT(const SMatrix< T, D1, D2, R > &lhs, const SMatrix< T, D1, D1, MatRepSym< T, D1 > > &rhs)
Transpose Similarity Matrix Product : B = U^T * A * U for A symmetric returning a symmetric matrix ex...
void Mult(const TMatrixT< Element > &a, const TMatrixT< Element > &b)
General matrix multiplication. Create a matrix C such that C = A * B.
int test_smatrix_sym_kalman()
SVector: a generic fixed size Vector class.