Logo ROOT   6.10/09
Reference Guide
stressKalman.cxx
Go to the documentation of this file.
1 
2 #define RUN_ALL_POINTS
3 //#define HAVE_CLHEP
4 //#define DEBUG
5 
6 #include "Math/SVector.h"
7 #include "Math/SMatrix.h"
8 
9 #include "TMatrixD.h"
10 #include "TVectorD.h"
11 
12 #include "TFile.h"
13 #include "TSystem.h"
14 #include "TROOT.h"
15 
16 #include "TRandom3.h"
17 #include "TStopwatch.h"
18 
19 #include <iostream>
20 
21 #include <map>
22 
23 //#undef HAVE_CLHEP
24 
25 #ifdef HAVE_CLHEP
26 #include "CLHEP/Matrix/SymMatrix.h"
27 #include "CLHEP/Matrix/Matrix.h"
28 #include "CLHEP/Matrix/Vector.h"
29 #endif
30 
31 
32 #define NITER 1 // number of iterations
33 
34 #define NLOOP 1000000 // number of time the test is repeted
35 
36 
37 
38 template <unsigned int NDIM1, unsigned int NDIM2>
39 class TestRunner {
40 
41 public:
42 
43  // kalman test with normal matrices from SMatrix
44  int test_smatrix_kalman() ;
45  // kalman test with sym matrices from SMatrix
47 
48  // kalman test with matrices (normal and sym) from TMatrix
49  int test_tmatrix_kalman();
50 
51 #ifdef HAVE_CLHEP
52  // test with CLHEP matrix
53  int test_clhep_kalman();
54 #endif
55 
56  // run the all tests
57  int run() {
58  int iret = 0;
59  iret |= test_smatrix_sym_kalman();
60  iret |= test_smatrix_kalman();
61  iret |= test_tmatrix_kalman();
62 #ifdef HAVE_CLHEP
63  iret |= test_clhep_kalman();
64 #endif
65  std::cout << "\n\n";
66 
67  return iret;
68  }
69 
70 private:
71  double fX2sum;
72  double fC2sum;
73 
74 };
75 
76 
77 #define TR(N1,N2) \
78 { TestRunner<N1,N2> tr; if (tr.run()) return -1; }
79 
80 
81 int runTest() {
82 
83 #ifndef RUN_ALL_POINTS
84  TR(2,5)
85  //TR(2,10)
86 #else
87  TR(2,2)
88  TR(2,3)
89  TR(2,4)
90  TR(2,5)
91  TR(2,6)
92  TR(2,7)
93  TR(2,8)
94  TR(3,2)
95  TR(3,3)
96  TR(3,4)
97  TR(3,5)
98  TR(3,6)
99  TR(3,7)
100  TR(3,8)
101  TR(4,2)
102  TR(4,3)
103  TR(4,4)
104  TR(4,5)
105  TR(4,6)
106  TR(4,7)
107  TR(4,8)
108  TR(5,2)
109  TR(5,3)
110  TR(5,4)
111  TR(5,5)
112  TR(5,6)
113  TR(5,7)
114  TR(5,8)
115  TR(6,2)
116  TR(6,3)
117  TR(6,4)
118  TR(6,5)
119  TR(6,6)
120  TR(6,7)
121  TR(6,8)
122  TR(7,2)
123  TR(7,3)
124  TR(7,4)
125  TR(7,5)
126  TR(7,6)
127  TR(7,7)
128  TR(7,8)
129  TR(8,2)
130  TR(8,3)
131  TR(8,4)
132  TR(8,5)
133  TR(8,6)
134  TR(8,7)
135  TR(8,8)
136  TR(9,2)
137  TR(9,3)
138  TR(9,4)
139  TR(9,5)
140  TR(9,6)
141  TR(9,7)
142  TR(9,8)
143  TR(10,2)
144  TR(10,3)
145  TR(10,4)
146  TR(10,5)
147  TR(10,6)
148  TR(10,7)
149  TR(10,8)
150 #endif
151  return 0;
152 }
153 
154 
155 
156 // utility functions to fill matrices and vectors with random data
157 
158 template<class V>
159 void fillRandomVec(TRandom & r, V & v, unsigned int len, unsigned int start = 0, double offset = 1) {
160  for(unsigned int i = start; i < len+start; ++i)
161  v[i] = r.Rndm() + offset;
162 }
163 
164 template<class M>
165 void fillRandomMat(TRandom & r, M & m, unsigned int first, unsigned int second, unsigned int start = 0, double offset = 1) {
166  for(unsigned int i = start; i < first+start; ++i)
167  for(unsigned int j = start; j < second+start; ++j)
168  m(i,j) = r.Rndm() + offset;
169 }
170 
171 template<class M>
172 void fillRandomSym(TRandom & r, M & m, unsigned int first, unsigned int start = 0, double offset = 1) {
173  for(unsigned int i = start; i < first+start; ++i) {
174  for(unsigned int j = i; j < first+start; ++j) {
175  if ( i != j ) {
176  m(i,j) = r.Rndm() + offset;
177  m(j,i) = m(i,j);
178  }
179  else // add extra offset to make no singular when inverting
180  m(i,i) = r.Rndm() + 3*offset;
181  }
182  }
183 }
184 
185 
186 
187 // simple class to measure time
188 
189 
190 void printTime(TStopwatch & time, std::string s) {
191  int pr = std::cout.precision(4);
192  std::cout << std::setw(12) << s << "\t" << " Real time = " << time.RealTime() << "\t(sec)\tCPU time = "
193  << time.CpuTime() << "\t(sec)"
194  << std::endl;
195  std::cout.precision(pr);
196 }
197 
198 // reference times for sizes <=6 and > 6 on Linux slc3 P4 3Ghz ("SMatrix","SMatrix_sym","TMatrix")
199 double refTime1[4] = { 40.49, 53.75, 83.21,1000 };
200 double refTime2[4] = { 393.81, 462.16, 785.50,10000 };
201 
202 #define NMAX1 9 // matrix storese results from 2 to 10
203 #define NMAX2 7 // results from 2 to 8
204  // class to hold time results
205 class TimeReport {
206 
207  // typedef std::map<std::string, ROOT::Math::SMatrix<double,NMAX1,NMAX2,ROOT::Math::MatRepSym<double,NMAX1,NMAX2> > > ResultTable;
208  typedef std::map<std::string, double> a;
210  typedef std::map< std::string, M > ResultTable;
211 
212 public:
213 
214  TimeReport() {}
215 
216  ~TimeReport() { /*print(); */ }
217 
218  // set timing point
219  void Set(const std::string & name, int dim1, int dim2 ) {
220  fDim1 = dim1;
221  fDim2 = dim2;
222  fName = name;
223  // insert in map if not existing
224  if (fResult1.find(name) == fResult1.end() )
225  fResult1.insert(ResultTable::value_type(name, M() ) );
226  if (fResult2.find(name) == fResult2.end() )
227  fResult2.insert(ResultTable::value_type(name, M() ) );
228 
229  }
230 
231  std::string name() const { return fName; }
232 
233  void report(double rt, double ct) {
234  fResult1[fName](fDim1-2,fDim2-2) = rt;
235  fResult2[fName](fDim1-2,fDim2-2) = ct;
236  }
237 
238  double smallSum(const M & m, int cut = 6) {
239  // sum for sizes <= cut
240  double sum = 0;
241  for (int i = 0; i<cut-1; ++i)
242  for (int j = 0; j<cut-1; ++j)
243  sum+= m(i,j);
244 
245  return sum;
246  }
247 
248  double largeSum(const M & m, int cut = 6) {
249  // sum for sizes > cut
250  double sum = 0;
251  for (int i = 0; i<M::kRows; ++i)
252  for (int j = 0; j<M::kCols; ++j)
253  if ( i > cut-2 || j > cut-2)
254  sum+= m(i,j);
255 
256  return sum;
257  }
258 
259  void print(std::ostream & os) {
260  std::map<std::string,double> r1;
261  std::map<std::string,double> r2;
262  os << "Real time results " << std::endl;
263  for (ResultTable::iterator itr = fResult1.begin(); itr != fResult1.end(); ++itr) {
264  std::string type = itr->first;
265  os << " Results for " << type << std::endl;
266  os << "\n" << itr->second << std::endl << std::endl;
267  r1[type] = smallSum(itr->second);
268  r2[type] = largeSum(itr->second);
269  os << "\nTotal for N1,N2 <= 6 : " << r1[type] << std::endl;
270  os << "\nTotal for N1,N2 > 6 : " << r2[type] << std::endl;
271  }
272  os << "\n\nCPU time results " << std::endl;
273  for (ResultTable::iterator itr = fResult2.begin(); itr != fResult2.end(); ++itr) {
274  os << " Results for " << itr->first << std::endl;
275  os << "\n" << itr->second << std::endl << std::endl;
276  os << "\nTotal for N1,N2 <= 6 : " << smallSum(itr->second) << std::endl;
277  os << "\nTotal for N1,N2 > 6 : " << largeSum(itr->second) << std::endl;
278  }
279 
280  // print summary
281  os << "\n\n****************************************************************************\n";
282  os << "Root version: " << gROOT->GetVersion() << " "
283  << gROOT->GetVersionDate() << "/" << gROOT->GetVersionTime() << std::endl;
284  os << "****************************************************************************\n";
285  os << "\n\t ROOTMARKS for N1,N2 <= 6 \n\n";
286  int j = 0;
287  os.setf(std::ios::right,std::ios::adjustfield);
288  for (std::map<std::string,double>::iterator i = r1.begin(); i != r1.end(); ++i) {
289  std::string type = i->first;
290  os << std::setw(12) << type << "\t=\t" << refTime1[j]*800/r1[type] << std::endl;
291  j++;
292  }
293  os << "\n\t ROOTMARKS for N1,N2 > 6 \n\n";
294  j = 0;
295  for (std::map<std::string,double>::iterator i = r1.begin(); i != r1.end(); ++i) {
296  std::string type = i->first;
297  os << std::setw(12) << type << "\t=\t" << refTime2[j]*800/r2[type] << std::endl;
298  j++;
299  }
300 
301  }
302 
303  void save(const std::string & fileName) {
304  TFile file(fileName.c_str(),"RECREATE");
305  gSystem->Load("libSmatrix");
306 
307  // save RealTime results
308  for (ResultTable::iterator itr = fResult1.begin(); itr != fResult1.end(); ++itr) {
309  int ret = file.WriteObject(&(itr->second),(itr->first).c_str() );
310  if (ret ==0) std::cerr << "==> Error saving results in ROOT file " << fileName << std::endl;
311  }
312 
313  // save CPU time results
314  for (ResultTable::iterator itr = fResult2.begin(); itr != fResult2.end(); ++itr) {
315  std::string typeName = itr->first + "_2";
316  int ret = file.WriteObject(&(itr->second), typeName.c_str() );
317  if (ret ==0) std::cerr << "==> Error saving results in ROOT file " << fileName << std::endl;
318  }
319  file.Close();
320  }
321 
322 private:
323 
324  int fDim1;
325  int fDim2;
326  std::string fName;
327 
328  ResultTable fResult1;
329  ResultTable fResult2;
330 
331 };
332 
333 //global instance of time report
334 TimeReport gReporter;
335 
336 class TestTimer {
337 
338 public:
339 
340  // TestTimer(const std::string & s = "") :
341  // fName(s), fTime(0), fRep(0)
342  // {
343  // fWatch.Start();
344  // }
345  TestTimer(TimeReport & r ) :
346  fTime(0), fRep(&r)
347  {
348  fName = fRep->name();
349  fWatch.Start();
350  }
351  TestTimer(double & t, const std::string & s = "") : fName(s), fTime(&t), fRep(0)
352  {
353  fWatch.Start();
354  }
355 
356  ~TestTimer() {
357  fWatch.Stop();
358  printTime(fWatch,fName);
359  if (fRep) fRep->report( fWatch.RealTime(), fWatch.CpuTime() );
360  if (fTime) *fTime += fWatch.RealTime();
361  }
362 
363 
364 private:
365 
366  std::string fName;
367  double * fTime;
368  TStopwatch fWatch;
369  TimeReport * fRep;
370 
371 };
372 
373 
374 
375 template <unsigned int NDIM1, unsigned int NDIM2>
377 
378  gReporter.Set("SMatrix",NDIM1,NDIM2);
379 
380  // need to write explicitly the dimensions
381 
382 
387  typedef ROOT::Math::SMatrix<double, NDIM1 > MnSymMatrixNN;
388  typedef ROOT::Math::SMatrix<double, NDIM2 > MnSymMatrixMM;
389  typedef ROOT::Math::SVector<double, NDIM1> MnVectorN;
390  typedef ROOT::Math::SVector<double, NDIM2> MnVectorM;
391 
392 
393 
394  int first = NDIM1; //Can change the size of the matrices
395  int second = NDIM2;
396 
397 
398  std::cout << "****************************************************************************\n";
399  std::cout << "\t\tSMatrix kalman test " << first << " x " << second << std::endl;
400  std::cout << "****************************************************************************\n";
401 
402 
403 
404 
405  int npass = NITER;
406  TRandom3 r(111);
407  double x2sum = 0, c2sum = 0;
408  for (int k = 0; k < npass; k++) {
409 
410 
411 
412  MnMatrixNM H;
413  MnMatrixMN K0;
414  MnSymMatrixMM Cp;
415  MnSymMatrixNN V;
416  MnVectorN m;
417  MnVectorM xp;
418 
419 
420  {
421  // fill matrices with random data
422  fillRandomMat(r,H,first,second);
423  fillRandomMat(r,K0,second,first);
424  fillRandomSym(r,Cp,second);
425  fillRandomSym(r,V,first);
426  fillRandomVec(r,m,first);
427  fillRandomVec(r,xp,second);
428  }
429 
430 
431  MnSymMatrixMM I;
432  for(int i = 0; i < second; i++)
433  I(i,i) = 1;
434 
435 #ifdef DEBUG
436  std::cout << "pass " << k << std::endl;
437  if (k == 0) {
438  std::cout << " K0 = " << K0 << std::endl;
439  std::cout << " H = " << K0 << std::endl;
440  std::cout << " Cp = " << Cp << std::endl;
441  std::cout << " V = " << V << std::endl;
442  std::cout << " m = " << m << std::endl;
443  std::cout << " xp = " << xp << std::endl;
444  }
445 #endif
446 
447 
448  {
449  double x2 = 0,c2 = 0;
450  TestTimer t(gReporter);
451 
452  MnVectorM x;
453  MnMatrixMN tmp;
454  MnSymMatrixNN Rinv;
455  MnMatrixMN K;
456  MnSymMatrixMM C;
457  MnVectorN vtmp1;
458  MnVectorN vtmp;
459 
460  for (int l = 0; l < NLOOP; l++)
461  {
462 
463 
464 
465  vtmp1 = H*xp -m;
466  //x = xp + K0 * (m- H * xp);
467  x = xp - K0 * vtmp1;
468  tmp = Cp * Transpose(H);
469  Rinv = V; Rinv += H * tmp;
470 
471  bool test = Rinv.Invert();
472  if(!test) {
473  std::cout<<"inversion failed" <<std::endl;
474  std::cout << Rinv << std::endl;
475  }
476 
477  K = tmp * Rinv ;
478  C = Cp; C -= K * Transpose(tmp);
479  //C = ( I - K * H ) * Cp;
480  //x2 = Product(Rinv,m-H*xp); // this does not compile on WIN32
481  vtmp = m-H*xp;
482  x2 = Dot(vtmp, Rinv*vtmp);
483 
484 #ifdef DEBUG
485  if (l == 0) {
486  std::cout << " Rinv =\n " << Rinv << std::endl;
487  std::cout << " C =\n " << C << std::endl;
488  }
489 #endif
490 
491  }
492  //std::cout << k << " chi2 = " << x2 << std::endl;
493  x2sum += x2;
494  c2 = 0;
495  for (unsigned int i=0; i<NDIM2; ++i)
496  for (unsigned int j=0; j<NDIM2; ++j)
497  c2 += C(i,j);
498  c2sum += c2;
499  }
500  }
501  //tr.dump();
502 
503  int iret = 0;
504  double d = std::abs(x2sum-fX2sum);
505  if ( d > 1.E-6 * fX2sum ) {
506  std::cout << "ERROR: difference found in x2sum = " << x2sum << "\tref = " << fX2sum <<
507  "\tdiff = " << d << std::endl;
508  iret = 1;
509  }
510  d = std::abs(c2sum-fC2sum);
511  if ( d > 1.E-6 * fC2sum ) {
512  std::cout << "ERROR: difference found in c2sum = " << c2sum << "\tref = " << fC2sum <<
513  "\tdiff = " << d << std::endl;
514  iret = 1;
515  }
516 
517  return iret;
518 }
519 
520 template <unsigned int NDIM1, unsigned int NDIM2>
522 
523  gReporter.Set("SMatrix_sym",NDIM1,NDIM2);
524 
525  // need to write explicitly the dimensions
526 
527 
534  typedef ROOT::Math::SVector<double, NDIM1> MnVectorN;
535  typedef ROOT::Math::SVector<double, NDIM2> MnVectorM;
536  typedef ROOT::Math::SVector<double, NDIM1*(NDIM1+1)/2> MnVectorN2;
537  typedef ROOT::Math::SVector<double, NDIM2*(NDIM2+1)/2> MnVectorM2;
538 
539 
540 
541  int first = NDIM1; //Can change the size of the matrices
542  int second = NDIM2;
543 
544 
545  std::cout << "****************************************************************************\n";
546  std::cout << "\t\tSMatrix_Sym kalman test " << first << " x " << second << std::endl;
547  std::cout << "****************************************************************************\n";
548 
549 
550 
551 
552  int npass = NITER;
553  TRandom3 r(111);
554  double x2sum = 0, c2sum = 0;
555  for (int k = 0; k < npass; k++) {
556 
557 
558 
559  MnMatrixNM H;
560  MnMatrixMN K0;
561  MnSymMatrixMM Cp;
562  MnSymMatrixNN V;
563  MnVectorN m;
564  MnVectorM xp;
565 
566 
567  {
568  // fill matrices with random data
569  fillRandomMat(r,H,first,second);
570  fillRandomMat(r,K0,second,first);
571  fillRandomSym(r,Cp,second);
572  fillRandomSym(r,V,first);
573  fillRandomVec(r,m,first);
574  fillRandomVec(r,xp,second);
575  }
576 
577 
578  MnSymMatrixMM I;
579  for(int i = 0; i < second; i++)
580  I(i,i) = 1;
581 
582 #ifdef DEBUG
583  std::cout << "pass " << k << std::endl;
584  if (k == 0) {
585  std::cout << " K0 = " << K0 << std::endl;
586  std::cout << " H = " << K0 << std::endl;
587  std::cout << " Cp = " << Cp << std::endl;
588  std::cout << " V = " << V << std::endl;
589  std::cout << " m = " << m << std::endl;
590  std::cout << " xp = " << xp << std::endl;
591  }
592 
593 #endif
594 
595  {
596  double x2 = 0,c2 = 0;
597  TestTimer t(gReporter);
598 
599  MnVectorM x;
600  MnSymMatrixNN RinvSym;
601  MnMatrixMN K;
602  MnSymMatrixMM C;
603  MnSymMatrixMM Ctmp;
604  MnVectorN vtmp1;
605  MnVectorN vtmp;
606 #define OPTIMIZED_SMATRIX_SYM
607 #ifdef OPTIMIZED_SMATRIX_SYM
608  MnMatrixMN tmp;
609 #endif
610 
611  for (int l = 0; l < NLOOP; l++)
612  {
613 
614 
615 #ifdef OPTIMIZED_SMATRIX_SYM
616  vtmp1 = H*xp -m;
617  //x = xp + K0 * (m- H * xp);
618  x = xp - K0 * vtmp1;
619  tmp = Cp * Transpose(H);
620  // we are sure that H*tmp result is symmetric
621  ROOT::Math::AssignSym::Evaluate(RinvSym,H*tmp);
622  RinvSym += V;
623 
624  bool test = RinvSym.Invert();
625  if(!test) {
626  std::cout<<"inversion failed" <<std::endl;
627  std::cout << RinvSym << std::endl;
628  }
629 
630  K = tmp * RinvSym ;
631  // we profit from the fact that result of K*tmpT is symmetric
633  C = Cp; C -= Ctmp;
634  //C = ( I - K * H ) * Cp;
635  //x2 = Product(Rinv,m-H*xp); // this does not compile on WIN32
636  vtmp = m-H*xp;
637  x2 = ROOT::Math::Dot(vtmp, RinvSym*vtmp);
638 #else
639  // use similarity function
640  vtmp1 = H*xp -m;
641  x = xp - K0 * vtmp1;
642  RinvSym = V; RinvSym += Similarity(H,Cp);
643 
644  bool test = RinvSym.Invert();
645  if(!test) {
646  std::cout<<"inversion failed" <<std::endl;
647  std::cout << RinvSym << std::endl;
648  }
649 
650  Ctmp = ROOT::Math::SimilarityT(H, RinvSym);
651  C = Cp; C -= ROOT::Math::Similarity(Cp, Ctmp);
652  vtmp = m-H*xp;
653  x2 = ROOT::Math::Similarity(vtmp, RinvSym);
654 #endif
655 
656  }
657  //std::cout << k << " chi2 = " << x2 << std::endl;
658  x2sum += x2;
659  c2 = 0;
660  for (unsigned int i=0; i<NDIM2; ++i)
661  for (unsigned int j=0; j<NDIM2; ++j)
662  c2 += C(i,j);
663  c2sum += c2;
664  }
665  }
666 
667  // smatrix_sym is always first (skip check test)
668  fX2sum = x2sum;
669  fC2sum = c2sum;
670  if (x2sum == 0 || c2sum == 0) {
671  std::cout << "WARNING: x2sum = " << x2sum << "\tc2sum = " << c2sum << std::endl;
672  }
673 
674  return 0;
675 }
676 
677 
678 
679 // ROOT test
680 
681 
682 template <unsigned int NDIM1, unsigned int NDIM2>
684 
685  gReporter.Set("TMatrix",NDIM1,NDIM2);
686 
687 
688  typedef TMatrixD MnMatrix;
689  typedef TVectorD MnVector;
690 
691  // typedef boost::numeric::ublas::matrix<double> MnMatrix;
692  //typedef HepSymMatrix MnSymMatrixHep;
693 
694 
695  int first = NDIM1; //Can change the size of the matrices
696  int second = NDIM2;
697 
698 
699  std::cout << "****************************************************************************\n";
700  std::cout << "\t\tTMatrix Kalman test " << first << " x " << second << std::endl;
701  std::cout << "****************************************************************************\n";
702 
703  int npass = NITER;
704  TRandom3 r(111);
705  double x2sum = 0,c2sum = 0;
706 
707  for (int k = 0; k < npass; k++)
708  {
709 
710  MnMatrix H(first,second);
711  MnMatrix K0(second,first);
712  MnMatrix Cp(second,second);
713  MnMatrix V(first,first);
714  MnVector m(first);
715  MnVector xp(second);
716 
717  // fill matrices with random data
718  fillRandomMat(r,H,first,second);
719  fillRandomMat(r,K0,second,first);
720  fillRandomSym(r,Cp,second);
721  fillRandomSym(r,V,first);
722  fillRandomVec(r,m,first);
723  fillRandomVec(r,xp,second);
724 
725 
726  MnMatrix I(second,second);//Identity matrix
727  for (int i = 0; i < second; i++)
728  for(int j = 0; j <second; j++) {
729  I(i,j) = 0.0;
730  if (i==j) I(i,i) = 1.0;
731  }
732 
733  // if (k==0) {
734  // std::cout << " Cp " << std::endl;
735  // Cp.Print();
736  // }
737 
738  {
739  double x2 = 0,c2 = 0;
740  TVectorD x(second);
741  TMatrixD Rtmp(first,first);
742  TMatrixD Rinv(first,first);
743  TMatrixDSym RinvSym;
744  TMatrixD K(second,first);
745  TMatrixD C(second,second);
746  TMatrixD Ctmp(second,second);
747  TVectorD tmp1(first);
748  TMatrixD tmp2(second,first);
749 
750  TestTimer t(gReporter);
751  for (Int_t l = 0; l < NLOOP; l++)
752  {
753  tmp1 = m; Add(tmp1,-1.0,H,xp);
754  x = xp; Add(x,+1.0,K0,tmp1);
755  tmp2.MultT(Cp,H);
756  Rtmp.Mult(H,tmp2);
757  Rinv.Plus(V,Rtmp);
758  RinvSym.Use(first,Rinv.GetMatrixArray());
759  RinvSym.InvertFast();
760  K.Mult(tmp2,Rinv);
761  Ctmp.MultT(K,tmp2);
762  C.Minus(Cp,Ctmp);
763  x2 = RinvSym.Similarity(tmp1);
764 
765 #ifdef DEBUG
766  if (l == 0) {
767  std::cout << " Rinv =\n "; Rinv.Print();
768  std::cout << " RinvSym =\n "; RinvSym.Print();
769  std::cout << " C =\n "; C.Print();
770  }
771 #endif
772 
773  }
774  x2sum += x2;
775  c2 = 0;
776  for (unsigned int i=0; i<NDIM2; ++i)
777  for (unsigned int j=0; j<NDIM2; ++j)
778  c2 += C(i,j);
779  c2sum += c2;
780  }
781 
782  // }
783  }
784  //tr.dump();
785  //std::cout << "x2sum = " << x2sum << "\tc2sum = " << c2sum << std::endl;
786 
787  //gReporter.print();
788 
789  int iret = 0;
790  double d = std::abs(x2sum-fX2sum);
791  if ( d > 1.E-6 * fX2sum ) {
792  std::cout << "ERROR: difference found in x2sum = " << x2sum << "\tref = " << fX2sum <<
793  "\tdiff = " << d << std::endl;
794  iret = 1;
795  }
796  d = std::abs(c2sum-fC2sum);
797  if ( d > 1.E-6 * fC2sum ) {
798  std::cout << "ERROR: difference found in c2sum = " << c2sum << "\tref = " << fC2sum <<
799  "\tdiff = " << d << std::endl;
800  iret = 1;
801  }
802 
803 
804  return iret;
805 }
806 
807 
808 // test CLHEP Kalman
809 
810 #ifdef HAVE_CLHEP
811 template <unsigned int NDIM1, unsigned int NDIM2>
812 int TestRunner<NDIM1,NDIM2>::test_clhep_kalman() {
813 
814 
815  gReporter.Set("HepMatrix",NDIM1,NDIM2);
816 
817  typedef HepSymMatrix MnSymMatrix;
818  typedef HepMatrix MnMatrix;
819  typedef HepVector MnVector;
820 
821 
822  // typedef boost::numeric::ublas::matrix<double> MnMatrix;
823  //typedef HepSymMatrix MnSymMatrixHep;
824 
825 
826  int first = NDIM1; //Can change the size of the matrices
827  int second = NDIM2;
828 
829 
830  std::cout << "****************************************************************************\n";
831  std::cout << " CLHEP Kalman test " << first << " x " << second << std::endl;
832  std::cout << "****************************************************************************\n";
833 
834  int npass = NITER;
835  TRandom3 r(111);
836  double x2sum = 0,c2sum = 0;
837 
838  for (int k = 0; k < npass; k++)
839  {
840 
841  // in CLHEP index starts from 1
842  MnMatrix H(first,second);
843  MnMatrix K0(second,first);
844  MnMatrix Cp(second,second);
845  MnMatrix V(first,first);
846  MnVector m(first);
847  MnVector xp(second);
848 
849  // fill matrices with random data
850  fillRandomMat(r,H,first,second,1);
851  fillRandomMat(r,K0,second,first,1);
852  fillRandomSym(r,Cp,second,1);
853  fillRandomSym(r,V,first,1);
854  fillRandomVec(r,m,first);
855  fillRandomVec(r,xp,second);
856 
857  MnSymMatrix I(second,1);//Identity matrix
858 
859  {
860  double x2 = 0,c2 = 0;
861  MnVector x(second);
862  MnMatrix Rinv(first,first);
863  MnSymMatrix RinvSym(first);
864  MnMatrix K(second,first);
865  MnSymMatrix C(second);
866  MnVector vtmp1(first);
867  MnMatrix tmp(second,first);
868 
869  TestTimer t(gReporter);
870  int ifail;
871  for (Int_t l = 0; l < NLOOP; l++)
872  {
873 
874 
875  vtmp1 = H*xp -m;
876  //x = xp + K0 * (m- H * xp);
877  x = xp - K0 * vtmp1;
878  tmp = Cp * H.T();
879  Rinv = V; Rinv += H * tmp;
880  RinvSym.assign(Rinv);
881  RinvSym.invert(ifail);
882  if (ifail !=0) { std::cout << "Error inverting Rinv" << std::endl; break; }
883  K = tmp*RinvSym;
884  //C.assign( (I-K*H)*Cp);
885  //C = (I-K*H)*Cp;
886  C.assign( (I-K*H)*Cp );
887  x2= RinvSym.similarity(vtmp1);
888  if(ifail!=0) { std::cout << "Error inverting Rinv" << std::endl; break; }
889  }
890  x2sum += x2;
891 
892  c2 = 0;
893  for (unsigned int i=1; i<=NDIM2; ++i)
894  for (unsigned int j=1; j<=NDIM2; ++j)
895  c2 += C(i,j);
896  c2sum += c2;
897  }
898 
899  // }
900  }
901  //tr.dump();
902  //std::cout << "x2sum = " << x2sum << "\tc2sum = " << c2sum << std::endl;
903 
904  int iret = 0;
905  double d = std::abs(x2sum-fX2sum);
906  if ( d > 1.E-6 * fX2sum ) {
907  std::cout << "ERROR: difference found in x2sum = " << x2sum << "\tref = " << fX2sum <<
908  "\tdiff = " << d << std::endl;
909  iret = 1;
910  }
911  d = std::abs(c2sum-fC2sum);
912  if ( d > 1.E-6 * fC2sum ) {
913  std::cout << "ERROR: difference found in c2sum = " << c2sum << "\tref = " << fC2sum <<
914  "\tdiff = " << d << std::endl;
915  iret = 1;
916  }
917 
918  return iret;
919 }
920 #endif
921 
922 
923 int main(int argc, char *argv[]) {
924 
925 
926  if (runTest() ) {
927  std::cout << "\nERROR - stressKalman FAILED - exit!" << std::endl ;
928  return -1;
929  };
930 
931  gReporter.print(std::cout);
932  std::string fname = "kalman";
933  if (argc > 1) {
934  std::string platf(argv[1]);
935  fname = fname + "_" + platf;
936  }
937 
938  gReporter.save(fname+".root");
939 
940  return 0;
941 }
T Dot(const SVector< T, D > &lhs, const SVector< T, D > &rhs)
Vector dot product.
Definition: Functions.h:164
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 ...
int test_tmatrix_kalman()
Definition: testKalman.cxx:324
const int npass
Definition: testPermute.cxx:21
static long int sum(long int i)
Definition: Factory.cxx:2162
void fillRandomSym(TRandom &r, M &m, unsigned int first, unsigned int start=0, double offset=1)
constexpr Double_t K()
Definition: TMath.h:178
Random number generator class based on M.
Definition: TRandom3.h:27
int test_smatrix_kalman()
Definition: testKalman.cxx:44
Double_t RealTime()
Stop the stopwatch (if it is running) and return the realtime (in seconds) passed between the start a...
Definition: TStopwatch.cxx:110
TMatrixTSym< Element > & Use(Int_t row_lwb, Int_t row_upb, Element *data)
virtual const Element * GetMatrixArray() const
Definition: TMatrixT.h:222
A ROOT file is a suite of consecutive data records (TKey instances) with a well defined format...
Definition: TFile.h:46
TVectorT.
Definition: TMatrixTBase.h:77
int main(int argc, char *argv[])
#define gROOT
Definition: TROOT.h:375
#define H(x, y, z)
Definition: test.py:1
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.
virtual int Load(const char *module, const char *entry="", Bool_t system=kFALSE)
Load a shared library.
Definition: TSystem.cxx:1825
void fillRandomVec(TRandom &r, V &v, unsigned int len, unsigned int start=0, double offset=1)
Double_t CpuTime()
Stop the stopwatch (if it is running) and return the cputime (in seconds) passed between the start an...
Definition: TStopwatch.cxx:125
int Int_t
Definition: RtypesCore.h:41
TArc * a
Definition: textangle.C:12
TMatrixT.
Definition: TMatrixDfwd.h:22
#define NDIM2
Definition: testKalman.cxx:33
void Minus(const TMatrixT< Element > &a, const TMatrixT< Element > &b)
General matrix summation. Create a matrix C such that C = A - B.
Definition: TMatrixT.cxx:580
static const double x2[5]
Double_t x[n]
Definition: legend1.C:17
SMatrix: a generic fixed size D1 x D2 Matrix class.
This is the base class for the ROOT Random number generators.
Definition: TRandom.h:27
void MultT(const TMatrixT< Element > &a, const TMatrixT< Element > &b)
General matrix multiplication. Create a matrix C such that C = A * B^T.
Definition: TMatrixT.cxx:951
#define NITER
virtual Double_t Rndm()
Machine independent random number generator.
Definition: TRandom.cxx:512
int runTest()
static double C[]
TRandom2 r(17)
R__EXTERN TSystem * gSystem
Definition: TSystem.h:539
SVector< double, 2 > v
Definition: Dict.h:5
double refTime1[4]
unsigned int r1[N_CITIES]
Definition: simanTSP.cxx:321
TMarker * m
Definition: textangle.C:8
int test_smatrix_sym_kalman()
Definition: testKalman.cxx:168
TLine * l
Definition: textangle.C:4
#define TR(N1, N2)
void run(bool only_compile=false)
Definition: run.C:1
constexpr Double_t E()
Definition: TMath.h:74
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
Definition: HelperOps.h:156
void printTime(TStopwatch &time, std::string s)
Double_t Dot(const TGLVector3 &v1, const TGLVector3 &v2)
Definition: TGLUtil.h:318
void Add(THist< DIMENSIONS, PRECISION_TO, STAT_TO... > &to, const THist< DIMENSIONS, PRECISION_FROM, STAT_FROM... > &from)
Add two histograms.
Definition: THist.hxx:336
return c2
Definition: legend2.C:14
int type
Definition: TGX11.cxx:120
TMatrixTSym< Element > & InvertFast(Double_t *det=0)
Invert the matrix and calculate its determinant.
TimeReport gReporter
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 Print(Option_t *name="") const
Print the matrix as a table of elements.
void Plus(const TMatrixT< Element > &a, const TMatrixT< Element > &b)
General matrix summation. Create a matrix C such that C = A + B.
Definition: TMatrixT.cxx:512
double refTime2[4]
Definition: file.py:1
void fillRandomMat(TRandom &r, M &m, unsigned int first, unsigned int second, unsigned int start=0, double offset=1)
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...
#define NDIM1
Definition: testKalman.cxx:30
Definition: first.py:1
void Mult(const TMatrixT< Element > &a, const TMatrixT< Element > &b)
General matrix multiplication. Create a matrix C such that C = A * B.
Definition: TMatrixT.cxx:648
#define I(x, y, z)
unsigned int r2[N_CITIES]
Definition: simanTSP.cxx:322
#define NLOOP
SVector: a generic fixed size Vector class.
Stopwatch class.
Definition: TStopwatch.h:28