ATLAS Offline Software
KalmanUpdator_xk.cxx
Go to the documentation of this file.
1 /*
2  Copyright (C) 2002-2022 CERN for the benefit of the ATLAS collaboration
3 */
4 
6 // Implementation file for class Trk::KalmanUpdator_xk
8 // (c) ATLAS Detector software
11 // Version 1.0 07/06/2005 I.Gavrilenko
13 
14 #include "TrkMeasurementUpdator_xk/KalmanUpdator_xk.h"
15 #include "TrkEventPrimitives/LocalParameters.h"
16 #include "TrkPatternParameters/PatternTrackParameters.h"
17 #include "TrkParameters/TrackParameters.h"
18 
20 // Constructor
22 
23 Trk::KalmanUpdator_xk::KalmanUpdator_xk
24 (const std::string& t,const std::string& n,const IInterface* p)
25  : AthAlgTool(t,n,p)
26 {
27  mapKeyProduction();
28  m_covBoundary = 1.;
29 
30  m_cov0.push_back( 10.);
31  m_cov0.push_back( 10.);
32  m_cov0.push_back( .025);
33  m_cov0.push_back( .025);
34  m_cov0.push_back(.0001);
35 
36  declareInterface<IUpdator>( this );
37  declareInterface<IPatternParametersUpdator>( this );
38  declareProperty("InitialCovariances",m_cov0);
39  declareProperty("BoundaryCovariances",m_covBoundary);
40 }
41 
43 // Destructor
45 
46 Trk::KalmanUpdator_xk::~KalmanUpdator_xk()
47 = default;
48 
50 // Initialisation
52 
53 StatusCode Trk::KalmanUpdator_xk::initialize()
54 {
55  // init message stream
56  //
57  msg(MSG::INFO) << "initialize() successful in " << name() << endmsg;
58 
59  if( m_cov0.size()!=5) {
60 
61  m_cov0.erase(m_cov0.begin(),m_cov0.end());
62  m_cov0.push_back( 10.);
63  m_cov0.push_back( 10.);
64  m_cov0.push_back( .025);
65  m_cov0.push_back( .025);
66  m_cov0.push_back(.0001);
67  }
68  return StatusCode::SUCCESS;
69 }
70 
72 // Finalize
74 
75 StatusCode Trk::KalmanUpdator_xk::finalize()
76 {
77  return StatusCode::SUCCESS;
78 }
79 
81 // Add local position together with erro matrix without Xi2 calculation
83 
84 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::addToState
85 (const Trk::TrackParameters& T,
86  const Amg::Vector2D & P,
87  const Amg::MatrixX & E) const
88 {
89  Trk::FitQualityOnSurface* Q=nullptr;
90  return update(T,P,E,Q, 1,false);
91 }
92 
94 
95 bool Trk::KalmanUpdator_xk::addToState
96 (Trk::PatternTrackParameters& T,
97  const Amg::Vector2D & P,
98  const Amg::MatrixX & E,
99  Trk::PatternTrackParameters& Ta) const
100 {
101  int n ;
102  double x2;
103  return update(T,P,E, 1,false,Ta,x2,n);
104 }
105 
107 
108 bool Trk::KalmanUpdator_xk::addToStateOneDimension
109 (Trk::PatternTrackParameters& T,
110  const Amg::Vector2D & P,
111  const Amg::MatrixX & E,
112  Trk::PatternTrackParameters& Ta) const
113 {
114  double x2;
115  return updateOneDimension(T,P,E, 1,false,Ta,x2);
116 }
117 
119 // Remove local position together with erro matrix without Xi2 calculation
121 
122 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::removeFromState
123 (const Trk::TrackParameters& T,
124  const Amg::Vector2D & P,
125  const Amg::MatrixX & E) const
126 {
127  Trk::FitQualityOnSurface* Q=nullptr;
128  return update(T,P,E,Q,-1,false);
129 }
130 
132 
133 bool Trk::KalmanUpdator_xk::removeFromState
134 (Trk::PatternTrackParameters& T,
135  const Amg::Vector2D & P,
136  const Amg::MatrixX & E,
137  Trk::PatternTrackParameters& Ta) const
138 {
139  int n ;
140  double x2;
141  return update(T,P,E,-1,false,Ta,x2,n);
142 }
143 
145 // Add local parameters together with error matrix without Xi2 calculation
147 
148 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::addToState
149 (const Trk::TrackParameters& T,
150  const Trk::LocalParameters& P,
151  const Amg::MatrixX & E) const
152 {
153  Trk::FitQualityOnSurface* Q=nullptr;
154  return update(T,P,E,Q, 1,false);
155 }
156 
158 
159 bool Trk::KalmanUpdator_xk::addToState
160 (Trk::PatternTrackParameters& T,
161  const LocalParameters & P,
162  const Amg::MatrixX & E,
163  Trk::PatternTrackParameters& Ta) const
164 {
165  int n ;
166  double x2;
167  return update(T,P,E, 1,false,Ta,x2,n);
168 }
169 
171 // Remove local parameters together with error matrix without Xi2 calculation
173 
174 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::removeFromState
175 (const Trk::TrackParameters& T,
176  const Trk::LocalParameters& P,
177  const Amg::MatrixX & E) const
178 {
179  Trk::FitQualityOnSurface* Q=nullptr;
180  return update(T,P,E,Q,-1,false);
181 }
182 
184 
185 bool Trk::KalmanUpdator_xk::removeFromState
186 (Trk::PatternTrackParameters& T,
187  const LocalParameters & P,
188  const Amg::MatrixX & E,
189  Trk::PatternTrackParameters& Ta) const
190 {
191  int n ;
192  double x2;
193  return update(T,P,E,-1,false,Ta,x2,n);
194 }
195 
197 // Add local position together with error matrix with Xi2 calculation
199 
200 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::addToState
201 (const Trk::TrackParameters& T,
202  const Amg::Vector2D & P,
203  const Amg::MatrixX & E,
204  Trk::FitQualityOnSurface* & Q) const
205 {
206  return update(T,P,E,Q, 1,true);
207 }
208 
210 
211 bool Trk::KalmanUpdator_xk::addToState
212 (Trk::PatternTrackParameters& T ,
213  const Amg::Vector2D & P ,
214  const Amg::MatrixX & E ,
215  Trk::PatternTrackParameters& Ta,
216  double & Q ,
217  int & N ) const
218 {
219  return update(T,P,E, 1,true,Ta,Q,N);
220 }
221 
223 
224 bool Trk::KalmanUpdator_xk::addToStateOneDimension
225 (Trk::PatternTrackParameters& T ,
226  const Amg::Vector2D & P ,
227  const Amg::MatrixX & E ,
228  Trk::PatternTrackParameters& Ta,
229  double & Q ,
230  int & N ) const
231 {
232  N = 1;
233  return updateOneDimension(T,P,E, 1,true,Ta,Q);
234 }
235 
236 
238 // Remove local position together with error matrix with Xi2 calculation
240 
241 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::removeFromState
242 (const Trk::TrackParameters& T,
243  const Amg::Vector2D & P,
244  const Amg::MatrixX & E,
245  Trk::FitQualityOnSurface* & Q) const
246 {
247  return update(T,P,E,Q,-1,true);
248 }
249 
251 
252 bool Trk::KalmanUpdator_xk::removeFromState
253 (Trk::PatternTrackParameters& T ,
254  const Amg::Vector2D & P ,
255  const Amg::MatrixX & E ,
256  Trk::PatternTrackParameters& Ta,
257  double & Q ,
258  int & N ) const
259 {
260  return update(T,P,E,-1,true,Ta,Q,N);
261 }
262 
264 // Add local parameters together with error matrix with Xi2 calculation
266 
267 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::addToState
268 (const Trk::TrackParameters& T,
269  const Trk::LocalParameters& P,
270  const Amg::MatrixX & E,
271  Trk::FitQualityOnSurface* & Q) const
272 {
273  return update(T,P,E,Q, 1,true);
274 }
275 
277 // Add local parameters together with error matrix with Xi2 calculation
279 
280 std::pair<AmgVector(5),AmgSymMatrix(5)>* Trk::KalmanUpdator_xk::updateParameterDifference
281 (const AmgVector(5) & T ,
282  const AmgSymMatrix(5) & Et,
283  const Amg::VectorX & P ,
284  const Amg::MatrixX & Ep,
285  int K ,
286  Trk::FitQualityOnSurface*& Q ,
287  bool X) const
288 {
289  Q = nullptr;
290 
291  // Conversion local parameters
292  //
293  int n = Ep.cols(); if(n==0 || n>5) return nullptr;
294  double m[5];
295  double mv[15];
296 
297  m [ 0]=P (0 );
298  mv[ 0]=Ep(0,0);
299 
300  if(n>1) {
301  m [ 1]=P (1 );
302  mv[ 1]=Ep(1,0); mv[ 2]=Ep(1,1);
303  }
304  if(n>2) {
305  m [ 2]=P (2 );
306  mv[ 3]=Ep(2,0); mv[ 4]=Ep(2,1); mv[ 5]=Ep(2,2);
307  }
308  if(n>3) {
309  m [ 3]=P (3 );
310  mv[ 6]=Ep(3,0); mv[ 7]=Ep(3,1); mv[ 8]=Ep(3,2); mv[ 9]=Ep(3,3);
311  }
312  if(n>4) {
313  m [ 4]=P(4 );
314  mv[10]=Ep(4,0); mv[11]=Ep(4,1); mv[12]=Ep(4,2); mv[13]=Ep(4,3); mv[14]=Ep(4,4);
315  }
316 
317  // Conversion track parameters to updator presentation
318  //
319  double p [ 5] = {T(0),T(1),T(2),T(3),T(4)};
320  double pv[15] = {Et(0,0),
321  Et(1,0),Et(1,1),
322  Et(2,0),Et(2,1),Et(2,2),
323  Et(3,0),Et(3,1),Et(3,2),Et(3,3),
324  Et(4,0),Et(4,1),Et(4,2),Et(4,3),Et(4,4)};
325 
326  bool update = false;
327  double x2;
328  if (n==2 && K==3) {
329  update = updateWithTwoDim(1,X,m,mv,p,pv,x2);
330  if(update && X) Q = new Trk::FitQualityOnSurface(x2,2);
331  }
332  else if(n==1 && K==1) {
333  update = updateWithOneDim(1,X,m,mv,p,pv,x2);
334  if(update && X) Q = new Trk::FitQualityOnSurface(x2,1);
335  }
336  else {
337  update = updateWithAnyDim(1,X,m,mv,p,pv,x2,n,K);
338  if(update && X) Q = new Trk::FitQualityOnSurface(x2,n);
339  }
340  if(!update) return nullptr;
341 
342  testAngles(p,pv);
343 
344  AmgVector(5) nT ; nT <<p[0],p[1],p[2],p[3],p[4];
345  AmgSymMatrix(5) nEt; nEt<<
346  pv[ 0],pv[ 1],pv[ 3],pv[ 6],pv[10],
347  pv[ 1],pv[ 2],pv[ 4],pv[ 7],pv[11],
348  pv[ 3],pv[ 4],pv[ 5],pv[ 8],pv[12],
349  pv[ 6],pv[ 7],pv[ 8],pv[ 9],pv[13],
350  pv[10],pv[11],pv[12],pv[13],pv[14];
351 
352  return new std::pair<AmgVector(5),AmgSymMatrix(5)>(std::make_pair(nT,nEt));
353 }
354 
356 
357 bool Trk::KalmanUpdator_xk::addToState
358 (Trk::PatternTrackParameters& T ,
359  const Trk::LocalParameters & P ,
360  const Amg::MatrixX & E ,
361  Trk::PatternTrackParameters& Ta,
362  double & Q ,
363  int & N ) const
364 {
365  return update(T,P,E, 1,true,Ta,Q,N);
366 }
367 
369 // Remove local parameters together with error matrix with Xi2 calculation
371 
372 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::removeFromState
373 (const Trk::TrackParameters& T,
374  const Trk::LocalParameters& P,
375  const Amg::MatrixX & E,
376  Trk::FitQualityOnSurface* & Q) const
377 {
378  return update(T,P,E,Q,-1,true);
379 }
380 
382 
383 bool Trk::KalmanUpdator_xk::removeFromState
384 (Trk::PatternTrackParameters& T ,
385  const Trk::LocalParameters & P ,
386  const Amg::MatrixX & E ,
387  Trk::PatternTrackParameters& Ta,
388  double & Q ,
389  int & N ) const
390 {
391  return update(T,P,E,-1,true,Ta,Q,N);
392 }
393 
395 // Combine two state without Xi2 calculation
397 
398 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::combineStates
399 (const Trk::TrackParameters& T1, const Trk::TrackParameters& T2) const
400 {
401  double M[5];
402  double MV[15]; if(!trackParametersToUpdator(T1,M,MV)) return nullptr;
403  double P[5];
404  double PV[15]; if(!trackParametersToUpdator(T2,P,PV)) return nullptr;
405 
406  double x2; double* m; double* mv; double* p; double* pv;
407  if(MV[14] > PV[14]) {m = &M[0]; mv = &MV[0]; p = &P[0]; pv = &PV[0];}
408  else {m = &P[0]; mv = &PV[0]; p = &M[0]; pv = &MV[0];}
409 
410  if(updateWithFiveDim(false,m,mv,p,pv,x2)) {
411  testAngles(p,pv); return updatorToTrackParameters(T1,p,pv);
412  }
413  return nullptr;
414 }
415 
417 
418 bool Trk::KalmanUpdator_xk::combineStates
419 (Trk::PatternTrackParameters& T1,
420  Trk::PatternTrackParameters& T2,
421  Trk::PatternTrackParameters& T3) const
422 {
423  double M[5];
424  double MV[15]; bool q1 = trackParametersToUpdator(T1,M,MV);
425  double P[5];
426  double PV[15]; bool q2 = trackParametersToUpdator(T2,P,PV);
427 
428  if(q1 && q2) {
429 
430  double x2; double* m; double* mv; double* p; double* pv;
431  if(MV[14] > PV[14]) {m = &M[0]; mv = &MV[0]; p = &P[0]; pv = &PV[0];}
432  else {m = &P[0]; mv = &PV[0]; p = &M[0]; pv = &MV[0];}
433 
434  if(updateWithFiveDim(false,m,mv,p,pv,x2)) {
435  testAngles(p,pv);
436  T3.setParametersWithCovariance(&T1.associatedSurface(),p,pv);
437  return true;
438  }
439  return false;
440  }
441  if(q1) {T3=T1; return true;}
442  if(q2) {T3=T2; return true;}
443  return false;
444 }
445 
446 
448 // Combine two state with Xi2 calculation
450 
451 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::combineStates
452 (const TrackParameters& T1, const TrackParameters& T2,
453  FitQualityOnSurface*& Q) const
454 {
455  double M[5];
456  double MV[15]; if(!trackParametersToUpdator(T1,M,MV)) return nullptr;
457  double P[5];
458  double PV[15]; if(!trackParametersToUpdator(T2,P,PV)) return nullptr;
459 
460  double x2; double* m; double* mv; double* p; double* pv;
461  if(MV[14] > PV[14]) {m = &M[0]; mv = &MV[0]; p = &P[0]; pv = &PV[0];}
462  else {m = &P[0]; mv = &PV[0]; p = &M[0]; pv = &MV[0];}
463 
464  if(updateWithFiveDim(true,m,mv,p,pv,x2)) {
465  Q = new Trk::FitQualityOnSurface(x2,5);
466  testAngles(p,pv); return updatorToTrackParameters(T1,p,pv);
467  }
468  return nullptr;
469 }
470 
472 
473 bool Trk::KalmanUpdator_xk::combineStates
474 (Trk::PatternTrackParameters& T1,
475  Trk::PatternTrackParameters& T2,
476  Trk::PatternTrackParameters& T3,
477  double & Q ) const
478 {
479 
480  double M[5];
481  double MV[15]; bool q1 = trackParametersToUpdator(T1,M,MV);
482  double P[5];
483  double PV[15]; bool q2 = trackParametersToUpdator(T2,P,PV);
484 
485  if(q1 && q2) {
486 
487  double* m; double* mv; double* p; double* pv;
488  if(MV[14] > PV[14]) {m = &M[0]; mv = &MV[0]; p = &P[0]; pv = &PV[0];}
489  else {m = &P[0]; mv = &PV[0]; p = &M[0]; pv = &MV[0];}
490 
491  if(updateWithFiveDim(true,m,mv,p,pv,Q)) {
492  testAngles(p,pv);
493  T3.setParametersWithCovariance(&T1.associatedSurface(),p,pv);
494  return true;
495  }
496  return false;
497  }
498  if(q1) {T3=T1; Q=0; return true;}
499  if(q2) {T3=T2; Q=0; return true;}
500  return false;
501 }
502 
504 // Xi2 of Local position
506 
507 Trk::FitQualityOnSurface
508 Trk::KalmanUpdator_xk::predictedStateFitQuality
509 (const Trk::TrackParameters& T,
510  const Amg::Vector2D & P,
511  const Amg::MatrixX & E) const
512 {
513  const AmgSymMatrix(5)* v = T.covariance();
514  if(!v) return {};
515 
516  double t[5] = {T.parameters()[0],T.parameters()[1],
517  (*v)(0,0),(*v)(1,0),(*v)(1,1)};
518 
519  int N; double x2;
520  if(predictedStateFitQuality(t,P,E,N,x2)) return {x2,N};
521  return {};
522 }
523 
525 
526 bool Trk::KalmanUpdator_xk::predictedStateFitQuality
527 (const Trk::PatternTrackParameters& T,
528  const Amg::Vector2D & P,
529  const Amg::MatrixX & E,
530  int & N,
531  double & X2) const
532 {
533  if(!T.iscovariance()) {N = 0; return false;}
534 
535  const AmgVector(5) & p = T.parameters();
536  const AmgSymMatrix(5) & cov = *T.covariance();
537 
538  double t[5] = {p[0],p[1],
539  cov(0, 0),cov(0, 1),cov(1, 1)};
540 
541  return predictedStateFitQuality(t,P,E,N,X2);
542 }
543 
545 // Xi2 of Local parameters
547 
548 Trk::FitQualityOnSurface
549 Trk::KalmanUpdator_xk::predictedStateFitQuality
550 (const Trk::TrackParameters& T,
551  const Trk::LocalParameters& P,
552  const Amg::MatrixX & E) const
553 {
554  // Conversion track parameters
555  //
556  double p[5];
557  double pv[15]; if(!trackParametersToUpdator(T,p,pv)) return {};
558 
559  // Conversion local parameters
560  //
561  int n;
562  int k;
563  double m[5];
564  double mv[15]; if(!localParametersToUpdator(P,E,n,k,m,mv)) return {};
565 
566  // Xi2 calculation
567  //
568  double r[5];
569  double w[15]={1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.};
570  int ib = m_key[k];
571  int ie=m_key[k+1];
572  for(int i=ib; i!=ie; ++i) {w[i-ib] = mv[i-ib]+pv[m_map[i]];}
573 
574  bool q=true; if(n!=1) q=invert(n,w,w); else w[0]=1./w[0];
575 
576  if(q) {
577  differenceParLoc(k,m,p,r);
578  return {Xi2(n,r,w),n};
579  }
580  return {};
581 }
582 
584 
585 bool Trk::KalmanUpdator_xk::predictedStateFitQuality
586 (const Trk::PatternTrackParameters& T,
587  const Trk::LocalParameters & P,
588  const Amg::MatrixX & E,
589  int & N,
590  double & X2) const
591 {
592  // Conversion track parameters
593  //
594  double p[5];
595  double pv[15]; if(!trackParametersToUpdator(T,p,pv)) return false;
596 
597  // Conversion local parameters
598  //
599  int n;
600  int k;
601  double m[5];
602  double mv[15]; if(!localParametersToUpdator(P,E,n,k,m,mv)) return false;
603 
604  // Xi2 calculation
605  //
606  double r[5];
607  double w[15]={1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.};
608  int ib = m_key[k];
609  int ie=m_key[k+1];
610  for(int i=ib; i!=ie; ++i) {w[i-ib] = mv[i-ib]+pv[m_map[i]];}
611 
612  bool q=true; if(n!=1) q=invert(n,w,w); else w[0]=1./w[0];
613  N = n;
614  if(q) {differenceParLoc(k,m,p,r); X2 = Xi2(n,r,w); return true ;}
615  return false;
616 }
617 
618 
620 // Xi2 of Local position
622 
623 Trk::FitQualityOnSurface
624 Trk::KalmanUpdator_xk::fullStateFitQuality
625 (const Trk::TrackParameters& T,
626  const Amg::Vector2D & P,
627  const Amg::MatrixX & E) const
628 {
629  const AmgSymMatrix(5)* v = T.covariance();
630  if(!v) return {};
631  double t[5] = {T.parameters()[0],T.parameters()[1],
632  (*v)(0,0),(*v)(1,0),(*v)(1,1)};
633 
634  int N; double x2;
635  if(fullStateFitQuality(t,P,E,N,x2)) return {x2,N};
636  return {};
637 }
638 
640 
641 bool Trk::KalmanUpdator_xk::fullStateFitQuality
642 (const Trk::PatternTrackParameters& T,
643  const Amg::Vector2D & P,
644  const Amg::MatrixX & E,
645  int & N,
646  double & X2) const
647 {
648  if(!T.iscovariance()) {N = 0; return false;}
649 
650  const AmgVector(5) & p = T.parameters();
651  const AmgSymMatrix(5) & cov = *T.covariance();
652 
653  double t[5] = {p[0],p[1],
654  cov(0, 0),cov(0, 1),cov(1, 1)};
655 
656  return fullStateFitQuality(t,P,E,N,X2);
657 }
658 
660 // Xi2 of Local parameters
662 
663 Trk::FitQualityOnSurface Trk::KalmanUpdator_xk::fullStateFitQuality
664 (const Trk::TrackParameters& T,
665  const Trk::LocalParameters& P,
666  const Amg::MatrixX & E) const
667 {
668  // Conversion track parameters
669  //
670  double p[5];
671  double pv[15]; if(!trackParametersToUpdator(T,p,pv)) return {};
672  // Conversion local parameters
673  //
674  int n;
675  int k;
676  double m[5];
677  double mv[15]; if(!localParametersToUpdator(P,E,n,k,m,mv)) return {};
678 
679  // Xi2 calculation
680  //
681  double r[5];
682  double w[15]={1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.};
683  int ib = m_key[k];
684  int ie=m_key[k+1];
685  for(int i=ib; i!=ie; ++i) {w[i-ib] = mv[i-ib]-pv[m_map[i]];}
686 
687  bool q=true; if(n!=1) q=invert(n,w,w); else w[0]=1./w[0];
688 
689  if(q) {
690  differenceParLoc(k,m,p,r);
691  return {Xi2(n,r,w),n};
692  }
693  return {0.,n};
694 }
695 
697 
698 bool Trk::KalmanUpdator_xk::fullStateFitQuality
699 (const Trk::PatternTrackParameters& T,
700  const Trk::LocalParameters & P,
701  const Amg::MatrixX & E,
702  int & N,
703  double & X2) const
704 {
705  // Conversion track parameters
706  //
707  N = 0;
708  double p[5];
709  double pv[15]; if(!trackParametersToUpdator(T,p,pv)) return false;
710 
711  // Conversion local parameters
712  //
713  int k;
714  double m[5];
715  double mv[15]; if(!localParametersToUpdator(P,E,N,k,m,mv)) return false;
716 
717  // Xi2 calculation
718  //
719  double r[5];
720  double w[15];
721  int ib = m_key[k];
722  int ie=m_key[k+1];
723  for(int i=ib; i!=ie; ++i) {w[i-ib] = mv[i-ib]-pv[m_map[i]];}
724 
725  bool q=true; if(N!=1) q=invert(N,w,w); else w[0]=1./w[0];
726 
727  if(q) {differenceParLoc(k,m,p,r); X2 = Xi2(N,r,w); return true ;}
728  return false;
729 }
730 
732 // Xi2 of two track parameters
734 
735 Trk::FitQualityOnSurface Trk::KalmanUpdator_xk::predictedStateFitQuality
736 (const Trk::TrackParameters& T1,
737  const Trk::TrackParameters& T2) const
738 {
739  const double pi2 = 2.*M_PI ;
740  const double pi = M_PI;
741  double m[5];
742  double mv[15]; if(!trackParametersToUpdator(T1,m,mv)) return {};
743  double p[5];
744  double pv[15]; if(!trackParametersToUpdator(T2,p,pv)) return {};
745  double r[5] = {m [ 0]-p [ 0],
746  m [ 1]-p [ 1],
747  m [ 2]-p [ 2],
748  m [ 3]-p [ 3],
749  m [ 4]-p [ 4]};
750 
751  if (r[2] > pi) r[2] = fmod(r[2]+pi,pi2)-pi;
752  else if(r[2] <-pi) r[2] = fmod(r[2]-pi,pi2)+pi;
753  if (r[3] > pi) r[3] = fmod(r[3]+pi,pi2)-pi;
754  else if(r[3] <-pi) r[3] = fmod(r[3]-pi,pi2)+pi;
755 
756  double w[15]= {mv[ 0]+pv[ 0],
757  mv[ 1]+pv[ 1],
758  mv[ 2]+pv[ 2],
759  mv[ 3]+pv[ 3],
760  mv[ 4]+pv[ 4],
761  mv[ 5]+pv[ 5],
762  mv[ 6]+pv[ 6],
763  mv[ 7]+pv[ 7],
764  mv[ 8]+pv[ 8],
765  mv[ 9]+pv[ 9],
766  mv[10]+pv[10],
767  mv[11]+pv[11],
768  mv[12]+pv[12],
769  mv[13]+pv[13],
770  mv[14]+pv[14]};
771 
772  if(invert(5,w,w)) return {Xi2(5,r,w),5};
773  return {};
774 }
775 
777 
778 bool Trk::KalmanUpdator_xk::predictedStateFitQuality
779 (const Trk::PatternTrackParameters& T1,
780  const Trk::PatternTrackParameters& T2,
781  double & X2) const
782 {
783  const double pi2 = 2.*M_PI;
784  const double pi = M_PI;
785  double m[5];
786  double mv[15]; if(!trackParametersToUpdator(T1,m,mv)) return 0;
787  double p[5];
788  double pv[15]; if(!trackParametersToUpdator(T2,p,pv)) return 0;
789  double r[5] = {m [ 0]-p [ 0],
790  m [ 1]-p [ 1],
791  m [ 2]-p [ 2],
792  m [ 3]-p [ 3],
793  m [ 4]-p [ 4]};
794 
795  if (r[2] > pi) r[2] = fmod(r[2]+pi,pi2)-pi;
796  else if(r[2] <-pi) r[2] = fmod(r[2]-pi,pi2)+pi;
797  if (r[3] > pi) r[3] = fmod(r[3]+pi,pi2)-pi;
798  else if(r[3] <-pi) r[3] = fmod(r[3]-pi,pi2)+pi;
799 
800  double w[15]= {mv[ 0]+pv[ 0],
801  mv[ 1]+pv[ 1],
802  mv[ 2]+pv[ 2],
803  mv[ 3]+pv[ 3],
804  mv[ 4]+pv[ 4],
805  mv[ 5]+pv[ 5],
806  mv[ 6]+pv[ 6],
807  mv[ 7]+pv[ 7],
808  mv[ 8]+pv[ 8],
809  mv[ 9]+pv[ 9],
810  mv[10]+pv[10],
811  mv[11]+pv[11],
812  mv[12]+pv[12],
813  mv[13]+pv[13],
814  mv[14]+pv[14]};
815 
816  if(invert(5,w,w)) {X2 = Xi2(5,r,w); return true;}
817  return false;
818 }
819 
821 // let the client tools know how the assumptions on the initial
822 // precision for non-measured track parameters are configured
824 
825 std::vector<double> Trk::KalmanUpdator_xk::initialErrors() const
826 {
827  std::vector<double> errors(5);
828 
829  errors[0] = sqrt(m_cov0[0]);
830  errors[1] = sqrt(m_cov0[1]);
831  errors[2] = sqrt(m_cov0[2]);
832  errors[3] = sqrt(m_cov0[3]);
833  errors[4] = sqrt(m_cov0[4]);
834  return errors;
835 }
836 
838 // Protected methods
840 
842 // Add or remove local position together with error matrix
843 // with or witout Xi2 calculation
845 
846 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::update
847 (const Trk::TrackParameters& T,
848  const Amg::Vector2D & P,
849  const Amg::MatrixX & E,
850  Trk::FitQualityOnSurface* & Q,
851  int O,
852  bool X) const
853 {
854  // Measurement information preparation
855  //
856  double m[2];
857  double mv[3];
858  int n = E.rows(); if(n<=0) return nullptr;
859  m [0] = P[0];
860  mv[0] = E(0,0);
861  if(n==2) {
862  m [1] = P[1];
863  mv[1] = E(1,0);
864  mv[2] = E(1,1);
865  }
866 
867  // Conversion track parameters to updator presentation
868  //
869  double p[5];
870  double pv[15]; bool measured = trackParametersToUpdator(T,p,pv);
871  bool update = false;
872 
873  if(measured) {
874  double x2;
875  if (n==2) {
876  update = updateWithTwoDim(O,X,m,mv,p,pv,x2);
877  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(x2,2);
878  }
879  else if(n==1) {
880  update = updateWithOneDim(O,X,m,mv,p,pv,x2);
881  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(x2,1);
882  }
883  if(update) {
884  testAngles(p,pv); return updatorToTrackParameters(T,p,pv);
885  }
886  return nullptr;
887  }
888 
889  // For no measured track parameters
890  //
891  if(O<0) return nullptr;
892 
893  if (n==1) {
894  update = updateNoMeasuredWithOneDim(m,mv,p,pv);
895  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(0.,1);
896  }
897  else if(n==2) {
898  update = updateNoMeasuredWithTwoDim(m,mv,p,pv);
899  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(0.,2);
900  }
901 
902  if(update) return updatorToTrackParameters(T,p,pv);
903  return nullptr;
904 }
905 
907 
908 bool Trk::KalmanUpdator_xk::update
909 (Trk::PatternTrackParameters& T ,
910  const Amg::Vector2D & P ,
911  const Amg::MatrixX & E ,
912  int O ,
913  bool X ,
914  Trk::PatternTrackParameters& Ta,
915  double & Q ,
916  int & N ) const
917 {
918  // Measurement information preparation
919  //
920  double m[2];
921  double mv[3];
922  N = E.rows(); if(N<=0) return false;
923  m [0] = P[0];
924  mv[0] = E(0,0);
925  if(N==2) {
926  m [1] = P[1];
927  mv[1] = E(1,0);
928  mv[2] = E(1,1);
929  }
930 
931  // Conversion track parameters to updator presentation
932  //
933  double p[5];
934  double pv[15]; bool measured = trackParametersToUpdator(T,p,pv);
935  bool update = false;
936 
937  if(measured) {
938 
939  if (N==2) update = updateWithTwoDim(O,X,m,mv,p,pv,Q);
940  else if(N==1) update = updateWithOneDim(O,X,m,mv,p,pv,Q);
941  if(update) {
942  testAngles(p,pv);
943  Ta.setParametersWithCovariance(&T.associatedSurface(),p,pv);
944  }
945  return update;
946  }
947 
948  // For no measured track parameters
949  //
950  if(O<0) return false;
951  Q = 0.;
952  if (N==1) update = updateNoMeasuredWithOneDim(m,mv,p,pv);
953  else if(N==2) update = updateNoMeasuredWithTwoDim(m,mv,p,pv);
954 
955 
956  if(update) Ta.setParametersWithCovariance(&T.associatedSurface(),p,pv);
957  return update;
958 }
959 
961 
962 bool Trk::KalmanUpdator_xk::updateOneDimension
963 (Trk::PatternTrackParameters& T ,
964  const Amg::Vector2D & P ,
965  const Amg::MatrixX & E ,
966  int O ,
967  bool X ,
968  Trk::PatternTrackParameters& Ta,
969  double & Q ) const
970 {
971  // Measurement information preparation
972  //
973  double m[2];
974  double mv[3];
975  int N = E.rows(); if(N!=2 ) return false;
976  m [0] = P[0];
977  m [1] = P[1];
978  mv[0] = E(0,0);
979  mv[1] = E(1,0);
980  mv[2] = E(1,1);
981 
982  // Conversion track parameters to updator presentation
983  //
984  double p[5];
985  double pv[15]; bool measured = trackParametersToUpdator(T,p,pv);
986  bool update = false;
987 
988  if(measured) {
989 
990  if(fabs(mv[1]) < 1.e-30) {
991  update = updateWithOneDimWithBoundary(O,X,m,mv,p,pv,Q);
992  }
993  else {
994  update = updateWithTwoDimWithBoundary(O,X,m,mv,p,pv,Q);
995  }
996  if(update) {
997  testAngles(p,pv);
998  Ta.setParametersWithCovariance(&T.associatedSurface(),p,pv);
999  }
1000  return update;
1001  }
1002 
1003  // For no measured track parameters
1004  //
1005  if(O<0) return false;
1006  Q = 0.;
1007  update = updateNoMeasuredWithTwoDim(m,mv,p,pv);
1008  if(update) Ta.setParametersWithCovariance(&T.associatedSurface(),p,pv);
1009  return update;
1010 }
1011 
1012 
1014 // Add or remove local parameters together with error matrix
1015 // with or witout Xi2 calculation
1017 
1018 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::update
1019 (const Trk::TrackParameters& T,
1020  const Trk::LocalParameters& P,
1021  const Amg::MatrixX & E,
1022  Trk::FitQualityOnSurface* & Q,
1023  int O,
1024  bool X) const
1025 {
1026  // Conversion local parameters
1027  //
1028  int n;
1029  int k;
1030  double m[5];
1031  double mv[15]; if(!localParametersToUpdator(P,E,n,k,m,mv)) return nullptr;
1032 
1033  // Conversion track parameters to updator presentation
1034  //
1035  double p[5];
1036  double pv[15]; bool measured = trackParametersToUpdator(T,p,pv);
1037  bool update = false;
1038 
1039  if(measured) {
1040 
1041  double x2;
1042  if (n==2 && k==3) {
1043  update = updateWithTwoDim(O,X,m,mv,p,pv,x2);
1044  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(x2,2);
1045  }
1046  else if(n==1 && k==1) {
1047  update = updateWithOneDim(O,X,m,mv,p,pv,x2);
1048  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(x2,1);
1049  }
1050  else {
1051  update = updateWithAnyDim(O,X,m,mv,p,pv,x2,n,k);
1052  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(x2,n);
1053  }
1054  if(update) {testAngles(p,pv); return updatorToTrackParameters(T,p,pv);}
1055  return nullptr;
1056  }
1057 
1058  // For no measured track parameters
1059  //
1060  if(O<0) return nullptr;
1061  if (n==1 && k==1) {
1062  update = updateNoMeasuredWithOneDim(m,mv,p,pv);
1063  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(0.,1);
1064  }
1065  else if(n==2 && k==3) {
1066  update = updateNoMeasuredWithTwoDim(m,mv,p,pv);
1067  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(0.,2);
1068  }
1069  else {
1070  update = updateNoMeasuredWithAnyDim(m,mv,p,pv,k);
1071  if(update && X && !Q) Q = new Trk::FitQualityOnSurface(0.,n);
1072  }
1073  if(update) return updatorToTrackParameters(T,p,pv);
1074  return nullptr;
1075 }
1076 
1078 
1079 bool Trk::KalmanUpdator_xk::update
1080 (Trk::PatternTrackParameters& T ,
1081  const Trk::LocalParameters & P ,
1082  const Amg::MatrixX & E ,
1083  int O ,
1084  bool X ,
1085  Trk::PatternTrackParameters& Ta,
1086  double & Q ,
1087  int & N ) const
1088 {
1089  // Conversion local parameters
1090  //
1091  int k;
1092  double m[5];
1093  double mv[15]; if(!localParametersToUpdator(P,E,N,k,m,mv)) return false;
1094 
1095  // Conversion track parameters to updator presentation
1096  //
1097  double p[5];
1098  double pv[15]; bool measured = trackParametersToUpdator(T,p,pv);
1099  bool update = false;
1100 
1101  if(measured) {
1102 
1103  if (N==2 && k==3) update = updateWithTwoDim(O,X,m,mv,p,pv,Q);
1104  else if(N==1 && k==1) update = updateWithOneDim(O,X,m,mv,p,pv,Q);
1105  else update = updateWithAnyDim(O,X,m,mv,p,pv,Q,N,k);
1106  if(update) {
1107  testAngles(p,pv);
1108  Ta.setParametersWithCovariance(&T.associatedSurface(),p,pv);
1109  }
1110  return update;
1111  }
1112 
1113  // For no measured track parameters
1114  //
1115  if(O<0) return false;
1116  Q = 0.;
1117  if (N==1 && k==1) update = updateNoMeasuredWithOneDim(m,mv,p,pv);
1118  else if(N==2 && k==3) update = updateNoMeasuredWithTwoDim(m,mv,p,pv);
1119  else update = updateNoMeasuredWithAnyDim(m,mv,p,pv,k);
1120  if(update) Ta.setParametersWithCovariance(&T.associatedSurface(),p,pv);
1121  return update;
1122 }
1123 
1125 // Xi2 of Local position
1127 
1128 bool Trk::KalmanUpdator_xk::predictedStateFitQuality
1129 (const double* T,
1130  const Amg::Vector2D & P,
1131  const Amg::MatrixX & E,
1132  int& N,
1133  double& X2)
1134 {
1135  // Measurement information preparation
1136  //
1137  double m[2];
1138  double mv[3]; X2 = 0.;
1139  N = E.rows(); if(N<=0) return false;
1140  m [0] = P[0]-T[0];
1141  mv[0] = E(0,0)+T[2];
1142  if(N==1) {
1143 
1144  if(mv[0]>0.) X2 = m[0]*m[0]/mv[0];
1145  return true;
1146  }
1147  if(N==2) {
1148  m [1] = P[1]-T[1];
1149  mv[1] = E(1,0)+T[3];
1150  mv[2] = E(1,1)+T[4];
1151  double d = mv[0]*mv[2]-mv[1]*mv[1];
1152  if(d>0.) X2 = (m[0]*m[0]*mv[2]+m[1]*m[1]*mv[0]-2.*m[0]*m[1]*mv[1])/d;
1153  return true;
1154  }
1155  return false;
1156 }
1157 
1159 // Xi2 of Local position
1161 
1162 bool Trk::KalmanUpdator_xk::fullStateFitQuality
1163 (const double* T,
1164  const Amg::Vector2D & P,
1165  const Amg::MatrixX & E,
1166  int& N,
1167  double& X2)
1168 {
1169  // Measurement information preparation
1170  //
1171  double m[2];
1172  double mv[3]; X2 = 0.;
1173  N = E.rows(); if(N<=0) return false;
1174  m [0] = P[0]-T[0];
1175  mv[0] = E(0,0)-T[2];
1176  if(N==1) {
1177 
1178  if(mv[0]>0.) X2 = m[0]*m[0]/mv[0];
1179  return true;
1180  }
1181  if(N==2) {
1182  m [1] = P[1]-T[1];
1183  mv[1] = E(1,0)-T[3];
1184  mv[2] = E(1,1)-T[4];
1185  double d = mv[0]*mv[2]-mv[1]*mv[1];
1186  if(d>0.) X2 = (m[0]*m[0]*mv[2]+m[1]*m[1]*mv[0]-2.*m[0]*m[1]*mv[1])/d;
1187  return true;
1188  }
1189  return false;
1190 }
1191 
1193 // Add one dimension information to no measured track parameters
1194 // M and MV is measuremet together with covariance
1195 // P and PV is new track parameters together with covarianace
1197 
1198 bool Trk::KalmanUpdator_xk::updateNoMeasuredWithOneDim
1199 (const double* M,const double* MV,double* P, double* PV) const
1200 {
1201  P [ 0] = M [ 0];
1202  PV[ 0] = MV[ 0];
1203  PV[ 1] = 0.;
1204  PV[ 2] = m_cov0[1];
1205  PV[ 3] = 0.;
1206  PV[ 4] = 0.;
1207  PV[ 5] = m_cov0[2];
1208  PV[ 6] = 0.;
1209  PV[ 7] = 0.;
1210  PV[ 8] = 0.;
1211  PV[ 9] = m_cov0[3];
1212  PV[10] = 0.;
1213  PV[11] = 0.;
1214  PV[12] = 0.;
1215  PV[13] = 0.;
1216  PV[14] = m_cov0[4];
1217  return true;
1218 }
1219 
1221 // Add two dimension information to no measured track parameters
1222 // M and MV is measuremet together with covariance
1223 // P and PV is new track parameters together with covarianace
1225 
1226 bool Trk::KalmanUpdator_xk::updateNoMeasuredWithTwoDim
1227 (const double* M,const double* MV,double* P, double* PV) const
1228 {
1229  P [ 0] = M [ 0];
1230  P [ 1] = M [ 1];
1231  PV[ 0] = MV[ 0];
1232  PV[ 1] = MV[ 1];
1233  PV[ 2] = MV[ 2];
1234  PV[ 3] = 0.;
1235  PV[ 4] = 0.;
1236  PV[ 5] = m_cov0[2];
1237  PV[ 6] = 0.;
1238  PV[ 7] = 0.;
1239  PV[ 8] = 0.;
1240  PV[ 9] = m_cov0[3];
1241  PV[10] = 0.;
1242  PV[11] = 0.;
1243  PV[12] = 0.;
1244  PV[13] = 0.;
1245  PV[14] = m_cov0[4];
1246  return true;
1247 }
1248 
1250 // Add any dimension information (>=1 and <=5) to no measured
1251 // track parameters
1252 // M and MV is measuremet together with covariance
1253 // P and PV is new track parameters together with covarianace
1254 // K is key word
1256 
1257 bool Trk::KalmanUpdator_xk::updateNoMeasuredWithAnyDim
1258 (const double* M,const double* MV,double* P,double* PV,int K) const
1259 {
1260 
1261  int i=0;
1262  int j=0;
1263  while(K) {if(K&1) P[i]=M[j++]; K>>=1; ++i;} if(i==0) return false;
1264 
1265  PV[ 0] = m_cov0[0];
1266  PV[ 1] = 0.;
1267  PV[ 2] = m_cov0[1];
1268  PV[ 3] = 0.;
1269  PV[ 4] = 0.;
1270  PV[ 5] = m_cov0[2];
1271  PV[ 6] = 0.;
1272  PV[ 7] = 0.;
1273  PV[ 8] = 0.;
1274  PV[ 9] = m_cov0[3];
1275  PV[10] = 0.;
1276  PV[11] = 0.;
1277  PV[12] = 0.;
1278  PV[13] = 0.;
1279  PV[14] = m_cov0[4];
1280 
1281  int ne = m_key[K+1]; i=0;
1282  for(int n = m_key[K]; n!=ne; ++n) {PV[m_map[n]] = MV[i++];}
1283  return true;
1284 }
1285 
1287 // Add or remove one dimension information to measured track parameters
1288 // M and MV is measuremet together with covariance
1289 // P and PV is new track parameters together with covarianace
1291 
1292 bool Trk::KalmanUpdator_xk::updateWithOneDim
1293 (int O,bool X,const double* M,const double* MV,double* P, double* PV,double& xi2)
1294 {
1295  double v0;
1296  if(O>0) {v0 = MV[0]+PV[0];}
1297  else {v0 = MV[0]-PV[0];}
1298 
1299  if(v0<=0.) return false;
1300  double w0 = 1./v0;
1301  double r0 = M[0]-P[0];
1302 
1303  // K matrix with (5x1) size
1304  //
1305  double k0 = PV[ 0]*w0;
1306  double k1 = PV[ 1]*w0;
1307  double k2 = PV[ 3]*w0;
1308  double k3 = PV[ 6]*w0;
1309  double k4 = PV[10]*w0;
1310 
1311  if(O<0) {k0=-k0; k1=-k1; k2=-k2; k3=-k3; k4=-k4;}
1312 
1313  // New parameters
1314  //
1315  P[0]+=(k0*r0);
1316  P[1]+=(k1*r0);
1317  P[2]+=(k2*r0);
1318  P[3]+=(k3*r0);
1319  P[4]+=(k4*r0);
1320 
1321  // New covariance matrix
1322  //
1323  if((PV[14]-= (k4*PV[10]))<=0.) return false;
1324  PV[13]-= (k4*PV[ 6]);
1325  PV[12]-= (k4*PV[ 3]);
1326  PV[11]-= (k4*PV[ 1]);
1327  PV[10]-= (k4*PV[ 0]);
1328  if((PV[ 9]-= (k3*PV[ 6]))<=0.) return false;
1329  PV[ 8]-= (k3*PV[ 3]);
1330  PV[ 7]-= (k3*PV[ 1]);
1331  PV[ 6]-= (k3*PV[ 0]);
1332  if((PV[ 5]-= (k2*PV[ 3]))<=0.) return false;
1333  PV[ 4]-= (k2*PV[ 1]);
1334  PV[ 3]-= (k2*PV[ 0]);
1335  if((PV[ 2]-= (k1*PV[ 1]))<=0.) return false;
1336  PV[ 1]-= (k1*PV[ 0]);
1337  if((PV[ 0]-= (k0*PV[ 0]))<=0.) return false;
1338 
1339  if(X) xi2 = r0*r0*w0;
1340  return true;
1341 }
1342 
1344 // Add or remove one dimension information to measured track parameters
1345 // and check boundary for second parameters
1346 // M and MV is measuremet together with covariance
1347 // P and PV is new track parameters together with covarianace
1349 
1350 bool Trk::KalmanUpdator_xk::updateWithOneDimWithBoundary
1351 (int O,bool X,double* M,double* MV,double* P, double* PV,double& xi2) const
1352 {
1353  double v0;
1354  if(O>0) {v0 = MV[0]+PV[0];}
1355  else {v0 = MV[0]-PV[0];}
1356 
1357  if(v0<=0.) return false;
1358  double w0 = 1./v0;
1359  double r0 = M[0]-P[0];
1360 
1361  // K matrix with (5x1) size
1362  //
1363  double k0 = PV[ 0]*w0;
1364  double k1 = PV[ 1]*w0;
1365  double k2 = PV[ 3]*w0;
1366  double k3 = PV[ 6]*w0;
1367  double k4 = PV[10]*w0;
1368 
1369  if(O<0) {k0=-k0; k1=-k1; k2=-k2; k3=-k3; k4=-k4;}
1370 
1371  // Boundary check
1372  //
1373  double P1 = P[1]+k1*r0 ;
1374  double dP = P1-M[1] ;
1375  double W = sqrt(MV[2])*1.732051;
1376 
1377  if(fabs(dP) <= W) {
1378 
1379  P[0]+=(k0*r0);
1380  P[1] = P1 ;
1381  P[2]+=(k2*r0);
1382  P[3]+=(k3*r0);
1383  P[4]+=(k4*r0);
1384  if(X) xi2 = r0*r0*w0;
1385  }
1386  else {
1387 
1388  dP > W ? M[1]+=W : M[1]-=W; MV[2] = m_covBoundary;
1389  if(!updateWithTwoDimParameters(O,true,M,MV,P,PV,xi2)) return false;
1390  }
1391 
1392  // New covariance matrix
1393  //
1394  if((PV[14]-= (k4*PV[10]))<=0.) return false;
1395  PV[13]-= (k4*PV[ 6]);
1396  PV[12]-= (k4*PV[ 3]);
1397  PV[11]-= (k4*PV[ 1]);
1398  PV[10]-= (k4*PV[ 0]);
1399  if((PV[ 9]-= (k3*PV[ 6]))<=0.) return false;
1400  PV[ 8]-= (k3*PV[ 3]);
1401  PV[ 7]-= (k3*PV[ 1]);
1402  PV[ 6]-= (k3*PV[ 0]);
1403  if((PV[ 5]-= (k2*PV[ 3]))<=0.) return false;
1404  PV[ 4]-= (k2*PV[ 1]);
1405  PV[ 3]-= (k2*PV[ 0]);
1406  if((PV[ 2]-= (k1*PV[ 1]))<=0.) return false;
1407  PV[ 1]-= (k1*PV[ 0]);
1408  return (PV[ 0]-= (k0*PV[ 0])) > 0.;
1409 }
1410 
1412 // Add or remove two dimension information to measured track parameters
1413 // M and MV is measuremet together with covariance
1414 // P and PV is new track parameters together with covarianace
1416 
1417 bool Trk::KalmanUpdator_xk::updateWithTwoDim
1418 (int O,bool X,const double* M,const double* MV,double* P, double* PV,double& xi2)
1419 {
1420  double v0;
1421  double v1;
1422  double v2;
1423  if(O>0) {v0 = MV[0]+PV[0]; v1 = MV[1]+PV[1]; v2 = MV[2]+PV[2];}
1424  else {v0 = MV[0]-PV[0]; v1 = MV[1]-PV[1]; v2 = MV[2]-PV[2];}
1425 
1426  double d = v0*v2-v1*v1; if(d<=0.) return false; d=1./d;
1427  double w0 = v2*d;
1428  double w1 =-v1*d;
1429  double w2 = v0*d;
1430  double r0 = M[0]-P[0];
1431  double r1 = M[1]-P[1];
1432 
1433  // K matrix with (5x2) size
1434  //
1435  double k0 = PV[ 0]*w0+PV[ 1]*w1;
1436  double k1 = PV[ 0]*w1+PV[ 1]*w2;
1437  double k2 = PV[ 1]*w0+PV[ 2]*w1;
1438  double k3 = PV[ 1]*w1+PV[ 2]*w2;
1439  double k4 = PV[ 3]*w0+PV[ 4]*w1;
1440  double k5 = PV[ 3]*w1+PV[ 4]*w2;
1441  double k6 = PV[ 6]*w0+PV[ 7]*w1;
1442  double k7 = PV[ 6]*w1+PV[ 7]*w2;
1443  double k8 = PV[10]*w0+PV[11]*w1;
1444  double k9 = PV[10]*w1+PV[11]*w2;
1445 
1446  if(O<0) {
1447  k0=-k0; k1=-k1; k2=-k2; k3=-k3; k4=-k4;
1448  k5=-k5; k6=-k6; k7=-k7; k8=-k8; k9=-k9;
1449  }
1450 
1451  // New parameters
1452  //
1453  P[0]+=(k0*r0+k1*r1);
1454  P[1]+=(k2*r0+k3*r1);
1455  P[2]+=(k4*r0+k5*r1);
1456  P[3]+=(k6*r0+k7*r1);
1457  P[4]+=(k8*r0+k9*r1);
1458 
1459  // New covariance matrix
1460  //
1461  if((PV[14]-= (k8*PV[10]+k9*PV[11]))<=0.) return false;
1462  PV[13]-= (k8*PV[ 6]+k9*PV[ 7]);
1463  PV[12]-= (k8*PV[ 3]+k9*PV[ 4]);
1464  PV[11]-= (k8*PV[ 1]+k9*PV[ 2]);
1465  PV[10]-= (k8*PV[ 0]+k9*PV[ 1]);
1466  if((PV[ 9]-= (k6*PV[ 6]+k7*PV[ 7]))<=0.) return false;
1467  PV[ 8]-= (k6*PV[ 3]+k7*PV[ 4]);
1468  PV[ 7]-= (k6*PV[ 1]+k7*PV[ 2]);
1469  PV[ 6]-= (k6*PV[ 0]+k7*PV[ 1]);
1470  if((PV[ 5]-= (k4*PV[ 3]+k5*PV[ 4]))<=0.) return false;
1471  PV[ 4]-= (k4*PV[ 1]+k5*PV[ 2]);
1472  PV[ 3]-= (k4*PV[ 0]+k5*PV[ 1]);
1473  if((PV[ 2]-= (k3*PV[ 2]+k2*PV[ 1]))<=0.) return false;
1474  double c1 = (1.-k3)*PV[ 1]-k2*PV[ 0];
1475  if((PV[ 0]-= (k0*PV[ 0]+k1*PV[ 1]))<=0.) return false;
1476  PV[ 1] = c1;
1477 
1478  if(X) xi2 = (r0*r0*w0+r1*r1*w2+2.*r0*r1*w1);
1479  return true;
1480 }
1481 
1483 // Add or remove two dimension information to measured track parameters
1484 // without calculation new covariance matrix
1485 // M and MV is measuremet together with covariance
1486 // P and PV is new track parameters together with covarianace
1488 
1489 bool Trk::KalmanUpdator_xk::updateWithTwoDimParameters
1490 (int O,bool X,const double* M,const double* MV,double* P, const double* PV,double& xi2)
1491 {
1492  double v0;
1493  double v1;
1494  double v2;
1495  if(O>0) {v0 = MV[0]+PV[0]; v1 = MV[1]+PV[1]; v2 = MV[2]+PV[2];}
1496  else {v0 = MV[0]-PV[0]; v1 = MV[1]-PV[1]; v2 = MV[2]-PV[2];}
1497 
1498  double d = v0*v2-v1*v1; if(d<=0.) return false; d=1./d;
1499  double w0 = v2*d;
1500  double w1 =-v1*d;
1501  double w2 = v0*d;
1502  double r0 = M[0]-P[0];
1503  double r1 = M[1]-P[1];
1504 
1505  // K matrix with (5x2) size
1506  //
1507  double k0 = PV[ 0]*w0+PV[ 1]*w1;
1508  double k1 = PV[ 0]*w1+PV[ 1]*w2;
1509  double k2 = PV[ 1]*w0+PV[ 2]*w1;
1510  double k3 = PV[ 1]*w1+PV[ 2]*w2;
1511  double k4 = PV[ 3]*w0+PV[ 4]*w1;
1512  double k5 = PV[ 3]*w1+PV[ 4]*w2;
1513  double k6 = PV[ 6]*w0+PV[ 7]*w1;
1514  double k7 = PV[ 6]*w1+PV[ 7]*w2;
1515  double k8 = PV[10]*w0+PV[11]*w1;
1516  double k9 = PV[10]*w1+PV[11]*w2;
1517 
1518  if(O<0) {
1519  k0=-k0; k1=-k1; k2=-k2; k3=-k3; k4=-k4;
1520  k5=-k5; k6=-k6; k7=-k7; k8=-k8; k9=-k9;
1521  }
1522 
1523  // New parameters
1524  //
1525  P[0]+=(k0*r0+k1*r1);
1526  P[1]+=(k2*r0+k3*r1);
1527  P[2]+=(k4*r0+k5*r1);
1528  P[3]+=(k6*r0+k7*r1);
1529  P[4]+=(k8*r0+k9*r1);
1530  if(X) xi2 = (r0*r0*w0+r1*r1*w2+2.*r0*r1*w1);
1531  return true;
1532 }
1533 
1535 // Add or remove two dimension information to measured track parameters
1536 // as one dimension and check boundary for second parameter
1537 // M and MV is measuremet together with covariance
1538 // P and PV is new track parameters together with covarianace
1540 
1541 bool Trk::KalmanUpdator_xk::updateWithTwoDimWithBoundary
1542 (int O,bool X,double* M,double* MV,double* P, double* PV,double& xi2) const
1543 {
1544  // Increase second covariance
1545  //
1546  double sa = MV[0]+MV[2] ;
1547  double d = MV[0]*MV[2]-MV[1]*MV[1];
1548  double ds = d/sa ;
1549  double V0 = ds*(1.+ds/sa) ;
1550  double V1 = sa-V0 ;
1551  double dV = 1./(V0-V1) ;
1552  double sc = MV[1]*dV ;
1553  double al = (MV[0]-MV[2])*dV ;
1554  double s2 = .5*(1.-al) ;
1555  double c2 = s2+al ;
1556  double c = sqrt(c2) ;
1557  double s = sc/c ;
1558 
1559  // New measurement
1560  //
1561  double M0 = c*M[0]+s*M[1];
1562  M [ 1] = c*M[1]-s*M[0];
1563  M [ 0] = M0 ;
1564  MV[ 0] = V0 ;
1565  MV[ 1] = 0. ;
1566  MV[ 2] = V1 ;
1567 
1568  // Rotate track parameters and covariance matrix
1569  //
1570  double P0 = P[0] ;
1571  P [ 0] = c*P0 +s*P[1] ;
1572  P [ 1] = c*P[1]-s*P0 ;
1573  double B = 2.*sc*PV[ 1] ;
1574  double PV0 = PV[ 0] ;
1575  double PV3 = PV[ 3] ;
1576  double PV6 = PV[ 6] ;
1577  double PV10= PV[10] ;
1578  PV[ 0] = c2*PV0+s2*PV[ 2]+B ;
1579  PV[ 1] = sc*(PV[ 2]-PV0)+PV[ 1]*al;
1580  PV[ 2] = s2*PV0+c2*PV[ 2]-B ;
1581  PV[ 3] = c*PV3 +s*PV[ 4] ;
1582  PV[ 4] = c*PV[ 4]-s*PV3 ;
1583  PV[ 6] = c*PV6 +s*PV[ 7] ;
1584  PV[ 7] = c*PV[ 7]-s*PV6 ;
1585  PV[10] = c*PV10 +s*PV[11] ;
1586  PV[11] = c*PV[11]-s*PV10 ;
1587 
1588  if(!updateWithOneDimWithBoundary(O,X,M,MV,P,PV,xi2)) return false;
1589 
1590  // Back rotation new track parameters and covariance matrix
1591  //
1592  s = -s; sc = -sc;
1593 
1594  P0 = P[0] ;
1595  P [ 0] = c*P0 +s*P[1] ;
1596  P [ 1] = c*P[1]-s*P0 ;
1597  B = 2.*sc*PV[ 1] ;
1598  PV0 = PV[ 0] ;
1599  PV3 = PV[ 3] ;
1600  PV6 = PV[ 6] ;
1601  PV10 = PV[10] ;
1602  PV[ 0] = c2*PV0+s2*PV[ 2]+B ;
1603  PV[ 1] = sc*(PV[ 2]-PV0)+PV[ 1]*al;
1604  PV[ 2] = s2*PV0+c2*PV[ 2]-B ;
1605  PV[ 3] = c*PV3 +s*PV[ 4] ;
1606  PV[ 4] = c*PV[ 4]-s*PV3 ;
1607  PV[ 6] = c*PV6 +s*PV[ 7] ;
1608  PV[ 7] = c*PV[ 7]-s*PV6 ;
1609  PV[10] = c*PV10 +s*PV[11] ;
1610  PV[11] = c*PV[11]-s*PV10 ;
1611  return true;
1612 }
1613 
1615 // Add five dimension information to measured track parameters
1616 // M and MV is measuremet together with covariance
1617 // P and PV is new track parameters together with covarianace
1619 
1620 bool Trk::KalmanUpdator_xk::updateWithFiveDim
1621 (bool X,double* M,double* MV,double* P, double* PV,double& xi2)
1622 {
1623  const double pi2 = 2.*M_PI;
1624  const double pi = M_PI;
1625 
1626  double w[15]={MV[ 0]+PV[ 0],MV[ 1]+PV[ 1],MV[ 2]+PV[ 2],
1627  MV[ 3]+PV[ 3],MV[ 4]+PV[ 4],MV[ 5]+PV[ 5],
1628  MV[ 6]+PV[ 6],MV[ 7]+PV[ 7],MV[ 8]+PV[ 8],
1629  MV[ 9]+PV[ 9],MV[10]+PV[10],MV[11]+PV[11],
1630  MV[12]+PV[12],MV[13]+PV[13],MV[14]+PV[14]};
1631 
1632  if(!invert5(w,w)) return false;
1633 
1634  double k00 =(PV[ 0]*w[ 0]+PV[ 1]*w[ 1]+PV[ 3]*w[ 3])+(PV[ 6]*w[ 6]+PV[10]*w[10]);
1635  double k01 =(PV[ 0]*w[ 1]+PV[ 1]*w[ 2]+PV[ 3]*w[ 4])+(PV[ 6]*w[ 7]+PV[10]*w[11]);
1636  double k02 =(PV[ 0]*w[ 3]+PV[ 1]*w[ 4]+PV[ 3]*w[ 5])+(PV[ 6]*w[ 8]+PV[10]*w[12]);
1637  double k03 =(PV[ 0]*w[ 6]+PV[ 1]*w[ 7]+PV[ 3]*w[ 8])+(PV[ 6]*w[ 9]+PV[10]*w[13]);
1638  double k04 =(PV[ 0]*w[10]+PV[ 1]*w[11]+PV[ 3]*w[12])+(PV[ 6]*w[13]+PV[10]*w[14]);
1639  double k10 =(PV[ 1]*w[ 0]+PV[ 2]*w[ 1]+PV[ 4]*w[ 3])+(PV[ 7]*w[ 6]+PV[11]*w[10]);
1640  double k11 =(PV[ 1]*w[ 1]+PV[ 2]*w[ 2]+PV[ 4]*w[ 4])+(PV[ 7]*w[ 7]+PV[11]*w[11]);
1641  double k12 =(PV[ 1]*w[ 3]+PV[ 2]*w[ 4]+PV[ 4]*w[ 5])+(PV[ 7]*w[ 8]+PV[11]*w[12]);
1642  double k13 =(PV[ 1]*w[ 6]+PV[ 2]*w[ 7]+PV[ 4]*w[ 8])+(PV[ 7]*w[ 9]+PV[11]*w[13]);
1643  double k14 =(PV[ 1]*w[10]+PV[ 2]*w[11]+PV[ 4]*w[12])+(PV[ 7]*w[13]+PV[11]*w[14]);
1644  double k20 =(PV[ 3]*w[ 0]+PV[ 4]*w[ 1]+PV[ 5]*w[ 3])+(PV[ 8]*w[ 6]+PV[12]*w[10]);
1645  double k21 =(PV[ 3]*w[ 1]+PV[ 4]*w[ 2]+PV[ 5]*w[ 4])+(PV[ 8]*w[ 7]+PV[12]*w[11]);
1646  double k22 =(PV[ 3]*w[ 3]+PV[ 4]*w[ 4]+PV[ 5]*w[ 5])+(PV[ 8]*w[ 8]+PV[12]*w[12]);
1647  double k23 =(PV[ 3]*w[ 6]+PV[ 4]*w[ 7]+PV[ 5]*w[ 8])+(PV[ 8]*w[ 9]+PV[12]*w[13]);
1648  double k24 =(PV[ 3]*w[10]+PV[ 4]*w[11]+PV[ 5]*w[12])+(PV[ 8]*w[13]+PV[12]*w[14]);
1649  double k30 =(PV[ 6]*w[ 0]+PV[ 7]*w[ 1]+PV[ 8]*w[ 3])+(PV[ 9]*w[ 6]+PV[13]*w[10]);
1650  double k31 =(PV[ 6]*w[ 1]+PV[ 7]*w[ 2]+PV[ 8]*w[ 4])+(PV[ 9]*w[ 7]+PV[13]*w[11]);
1651  double k32 =(PV[ 6]*w[ 3]+PV[ 7]*w[ 4]+PV[ 8]*w[ 5])+(PV[ 9]*w[ 8]+PV[13]*w[12]);
1652  double k33 =(PV[ 6]*w[ 6]+PV[ 7]*w[ 7]+PV[ 8]*w[ 8])+(PV[ 9]*w[ 9]+PV[13]*w[13]);
1653  double k34 =(PV[ 6]*w[10]+PV[ 7]*w[11]+PV[ 8]*w[12])+(PV[ 9]*w[13]+PV[13]*w[14]);
1654  double k40 =(PV[10]*w[ 0]+PV[11]*w[ 1]+PV[12]*w[ 3])+(PV[13]*w[ 6]+PV[14]*w[10]);
1655  double k41 =(PV[10]*w[ 1]+PV[11]*w[ 2]+PV[12]*w[ 4])+(PV[13]*w[ 7]+PV[14]*w[11]);
1656  double k42 =(PV[10]*w[ 3]+PV[11]*w[ 4]+PV[12]*w[ 5])+(PV[13]*w[ 8]+PV[14]*w[12]);
1657  double k43 =(PV[10]*w[ 6]+PV[11]*w[ 7]+PV[12]*w[ 8])+(PV[13]*w[ 9]+PV[14]*w[13]);
1658  double k44 =(PV[10]*w[10]+PV[11]*w[11]+PV[12]*w[12])+(PV[13]*w[13]+PV[14]*w[14]);
1659 
1660  // Residial production
1661  //
1662  double r[5]={M[0]-P[0],M[1]-P[1],M[2]-P[2],M[3]-P[3],M[4]-P[4]};
1663 
1664  // Test for angles differences
1665  //
1666  if (r[2] > pi) r[2] = fmod(r[2]+pi,pi2)-pi;
1667  else if(r[2] <-pi) r[2] = fmod(r[2]-pi,pi2)+pi;
1668  if (r[3] > pi) r[3] = fmod(r[3]+pi,pi2)-pi;
1669  else if(r[3] <-pi) r[3] = fmod(r[3]-pi,pi2)+pi;
1670 
1671  // New parameters
1672  //
1673  double p0 =(k00*r[0]+k01*r[1]+k02*r[2])+(k03*r[3]+k04*r[4]); P[0]+=p0;
1674  double p1 =(k10*r[0]+k11*r[1]+k12*r[2])+(k13*r[3]+k14*r[4]); P[1]+=p1;
1675  double p2 =(k20*r[0]+k21*r[1]+k22*r[2])+(k23*r[3]+k24*r[4]); P[2]+=p2;
1676  double p3 =(k30*r[0]+k31*r[1]+k32*r[2])+(k33*r[3]+k34*r[4]); P[3]+=p3;
1677  double p4 =(k40*r[0]+k41*r[1]+k42*r[2])+(k43*r[3]+k44*r[4]); P[4]+=p4;
1678 
1679  // New covariance matrix
1680  //
1681  double v0 =(k00*PV[ 0]+k01*PV[ 1]+k02*PV[ 3])+(k03*PV[ 6]+k04*PV[10]);
1682  double v1 =(k10*PV[ 0]+k11*PV[ 1]+k12*PV[ 3])+(k13*PV[ 6]+k14*PV[10]);
1683  double v2 =(k10*PV[ 1]+k11*PV[ 2]+k12*PV[ 4])+(k13*PV[ 7]+k14*PV[11]);
1684  double v3 =(k20*PV[ 0]+k21*PV[ 1]+k22*PV[ 3])+(k23*PV[ 6]+k24*PV[10]);
1685  double v4 =(k20*PV[ 1]+k21*PV[ 2]+k22*PV[ 4])+(k23*PV[ 7]+k24*PV[11]);
1686  double v5 =(k20*PV[ 3]+k21*PV[ 4]+k22*PV[ 5])+(k23*PV[ 8]+k24*PV[12]);
1687  double v6 =(k30*PV[ 0]+k31*PV[ 1]+k32*PV[ 3])+(k33*PV[ 6]+k34*PV[10]);
1688  double v7 =(k30*PV[ 1]+k31*PV[ 2]+k32*PV[ 4])+(k33*PV[ 7]+k34*PV[11]);
1689  double v8 =(k30*PV[ 3]+k31*PV[ 4]+k32*PV[ 5])+(k33*PV[ 8]+k34*PV[12]);
1690  double v9 =(k30*PV[ 6]+k31*PV[ 7]+k32*PV[ 8])+(k33*PV[ 9]+k34*PV[13]);
1691  double v10 =(k40*PV[ 0]+k41*PV[ 1]+k42*PV[ 3])+(k43*PV[ 6]+k44*PV[10]);
1692  double v11 =(k40*PV[ 1]+k41*PV[ 2]+k42*PV[ 4])+(k43*PV[ 7]+k44*PV[11]);
1693  double v12 =(k40*PV[ 3]+k41*PV[ 4]+k42*PV[ 5])+(k43*PV[ 8]+k44*PV[12]);
1694  double v13 =(k40*PV[ 6]+k41*PV[ 7]+k42*PV[ 8])+(k43*PV[ 9]+k44*PV[13]);
1695  double v14 =(k40*PV[10]+k41*PV[11]+k42*PV[12])+(k43*PV[13]+k44*PV[14]);
1696 
1697  if((PV[ 0]-=v0 )<=0.) return false;
1698  PV[ 1]-=v1 ;
1699  if((PV[ 2]-=v2 )<=0.) return false;
1700  PV[ 3]-=v3 ;
1701  PV[ 4]-=v4 ;
1702  if((PV[ 5]-=v5 )<=0.) return false;
1703  PV[ 6]-=v6 ;
1704  PV[ 7]-=v7 ;
1705  PV[ 8]-=v8 ;
1706  if((PV[ 9]-=v9 )<=0.) return false;
1707  PV[10]-=v10;
1708  PV[11]-=v11;
1709  PV[12]-=v12;
1710  PV[13]-=v13;
1711  if((PV[14]-=v14)<=0.) return false;
1712 
1713  if(X) xi2 = Xi2(5,r,w);
1714  return true;
1715 }
1716 
1718 // Add or remove any dimension information to measured track parameters
1719 // M and MV is measuremet together with covariance
1720 // P and PV is new track parameters together with covarianace
1722 
1723 bool Trk::KalmanUpdator_xk::updateWithAnyDim
1724 (int O,bool X,double* M,const double* MV,double* P, double* PV,double& xi2,
1725  int N,int K) const
1726 {
1727  double s;
1728  double w[15]={1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1.};
1729  int ib = m_key[K];
1730  int ie=m_key[K+1];
1731 
1732  if(O>0) {s= 1.; for(int i=ib; i!=ie; ++i) {w[i-ib] = MV[i-ib]+PV[m_map[i]];}}
1733  else {s=-1.; for(int i=ib; i!=ie; ++i) {w[i-ib] = MV[i-ib]-PV[m_map[i]];}}
1734 
1735  if(N==1) w[0] = 1./w[0]; else if(!invert(N,w,w)) return false;
1736  double v[15]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
1737  for(int i=ib; i!=ie; ++i) {v[m_map[i]] =w[i-ib];}
1738 
1739  double k00 =((PV[ 0]*v[ 0]+PV[ 1]*v[ 1]+PV[ 3]*v[ 3])+(PV[ 6]*v[ 6]+PV[10]*v[10]))*s;
1740  double k01 =((PV[ 0]*v[ 1]+PV[ 1]*v[ 2]+PV[ 3]*v[ 4])+(PV[ 6]*v[ 7]+PV[10]*v[11]))*s;
1741  double k02 =((PV[ 0]*v[ 3]+PV[ 1]*v[ 4]+PV[ 3]*v[ 5])+(PV[ 6]*v[ 8]+PV[10]*v[12]))*s;
1742  double k03 =((PV[ 0]*v[ 6]+PV[ 1]*v[ 7]+PV[ 3]*v[ 8])+(PV[ 6]*v[ 9]+PV[10]*v[13]))*s;
1743  double k04 =((PV[ 0]*v[10]+PV[ 1]*v[11]+PV[ 3]*v[12])+(PV[ 6]*v[13]+PV[10]*v[14]))*s;
1744  double k10 =((PV[ 1]*v[ 0]+PV[ 2]*v[ 1]+PV[ 4]*v[ 3])+(PV[ 7]*v[ 6]+PV[11]*v[10]))*s;
1745  double k11 =((PV[ 1]*v[ 1]+PV[ 2]*v[ 2]+PV[ 4]*v[ 4])+(PV[ 7]*v[ 7]+PV[11]*v[11]))*s;
1746  double k12 =((PV[ 1]*v[ 3]+PV[ 2]*v[ 4]+PV[ 4]*v[ 5])+(PV[ 7]*v[ 8]+PV[11]*v[12]))*s;
1747  double k13 =((PV[ 1]*v[ 6]+PV[ 2]*v[ 7]+PV[ 4]*v[ 8])+(PV[ 7]*v[ 9]+PV[11]*v[13]))*s;
1748  double k14 =((PV[ 1]*v[10]+PV[ 2]*v[11]+PV[ 4]*v[12])+(PV[ 7]*v[13]+PV[11]*v[14]))*s;
1749  double k20 =((PV[ 3]*v[ 0]+PV[ 4]*v[ 1]+PV[ 5]*v[ 3])+(PV[ 8]*v[ 6]+PV[12]*v[10]))*s;
1750  double k21 =((PV[ 3]*v[ 1]+PV[ 4]*v[ 2]+PV[ 5]*v[ 4])+(PV[ 8]*v[ 7]+PV[12]*v[11]))*s;
1751  double k22 =((PV[ 3]*v[ 3]+PV[ 4]*v[ 4]+PV[ 5]*v[ 5])+(PV[ 8]*v[ 8]+PV[12]*v[12]))*s;
1752  double k23 =((PV[ 3]*v[ 6]+PV[ 4]*v[ 7]+PV[ 5]*v[ 8])+(PV[ 8]*v[ 9]+PV[12]*v[13]))*s;
1753  double k24 =((PV[ 3]*v[10]+PV[ 4]*v[11]+PV[ 5]*v[12])+(PV[ 8]*v[13]+PV[12]*v[14]))*s;
1754  double k30 =((PV[ 6]*v[ 0]+PV[ 7]*v[ 1]+PV[ 8]*v[ 3])+(PV[ 9]*v[ 6]+PV[13]*v[10]))*s;
1755  double k31 =((PV[ 6]*v[ 1]+PV[ 7]*v[ 2]+PV[ 8]*v[ 4])+(PV[ 9]*v[ 7]+PV[13]*v[11]))*s;
1756  double k32 =((PV[ 6]*v[ 3]+PV[ 7]*v[ 4]+PV[ 8]*v[ 5])+(PV[ 9]*v[ 8]+PV[13]*v[12]))*s;
1757  double k33 =((PV[ 6]*v[ 6]+PV[ 7]*v[ 7]+PV[ 8]*v[ 8])+(PV[ 9]*v[ 9]+PV[13]*v[13]))*s;
1758  double k34 =((PV[ 6]*v[10]+PV[ 7]*v[11]+PV[ 8]*v[12])+(PV[ 9]*v[13]+PV[13]*v[14]))*s;
1759  double k40 =((PV[10]*v[ 0]+PV[11]*v[ 1]+PV[12]*v[ 3])+(PV[13]*v[ 6]+PV[14]*v[10]))*s;
1760  double k41 =((PV[10]*v[ 1]+PV[11]*v[ 2]+PV[12]*v[ 4])+(PV[13]*v[ 7]+PV[14]*v[11]))*s;
1761  double k42 =((PV[10]*v[ 3]+PV[11]*v[ 4]+PV[12]*v[ 5])+(PV[13]*v[ 8]+PV[14]*v[12]))*s;
1762  double k43 =((PV[10]*v[ 6]+PV[11]*v[ 7]+PV[12]*v[ 8])+(PV[13]*v[ 9]+PV[14]*v[13]))*s;
1763  double k44 =((PV[10]*v[10]+PV[11]*v[11]+PV[12]*v[12])+(PV[13]*v[13]+PV[14]*v[14]))*s;
1764 
1765  // Residial production
1766  //
1767  double r[5]; differenceLocPar(K,M,P,r);
1768 
1769  // New parameters
1770  //
1771  double p0 =(k00*r[0]+k01*r[1]+k02*r[2])+(k03*r[3]+k04*r[4]); P[0]+=p0;
1772  double p1 =(k10*r[0]+k11*r[1]+k12*r[2])+(k13*r[3]+k14*r[4]); P[1]+=p1;
1773  double p2 =(k20*r[0]+k21*r[1]+k22*r[2])+(k23*r[3]+k24*r[4]); P[2]+=p2;
1774  double p3 =(k30*r[0]+k31*r[1]+k32*r[2])+(k33*r[3]+k34*r[4]); P[3]+=p3;
1775  double p4 =(k40*r[0]+k41*r[1]+k42*r[2])+(k43*r[3]+k44*r[4]); P[4]+=p4;
1776 
1777  // New covariance matrix
1778  //
1779  double v0 =(k00*PV[ 0]+k01*PV[ 1]+k02*PV[ 3])+(k03*PV[ 6]+k04*PV[10]);
1780  double v1 =(k10*PV[ 0]+k11*PV[ 1]+k12*PV[ 3])+(k13*PV[ 6]+k14*PV[10]);
1781  double v2 =(k10*PV[ 1]+k11*PV[ 2]+k12*PV[ 4])+(k13*PV[ 7]+k14*PV[11]);
1782  double v3 =(k20*PV[ 0]+k21*PV[ 1]+k22*PV[ 3])+(k23*PV[ 6]+k24*PV[10]);
1783  double v4 =(k20*PV[ 1]+k21*PV[ 2]+k22*PV[ 4])+(k23*PV[ 7]+k24*PV[11]);
1784  double v5 =(k20*PV[ 3]+k21*PV[ 4]+k22*PV[ 5])+(k23*PV[ 8]+k24*PV[12]);
1785  double v6 =(k30*PV[ 0]+k31*PV[ 1]+k32*PV[ 3])+(k33*PV[ 6]+k34*PV[10]);
1786  double v7 =(k30*PV[ 1]+k31*PV[ 2]+k32*PV[ 4])+(k33*PV[ 7]+k34*PV[11]);
1787  double v8 =(k30*PV[ 3]+k31*PV[ 4]+k32*PV[ 5])+(k33*PV[ 8]+k34*PV[12]);
1788  double v9 =(k30*PV[ 6]+k31*PV[ 7]+k32*PV[ 8])+(k33*PV[ 9]+k34*PV[13]);
1789  double v10 =(k40*PV[ 0]+k41*PV[ 1]+k42*PV[ 3])+(k43*PV[ 6]+k44*PV[10]);
1790  double v11 =(k40*PV[ 1]+k41*PV[ 2]+k42*PV[ 4])+(k43*PV[ 7]+k44*PV[11]);
1791  double v12 =(k40*PV[ 3]+k41*PV[ 4]+k42*PV[ 5])+(k43*PV[ 8]+k44*PV[12]);
1792  double v13 =(k40*PV[ 6]+k41*PV[ 7]+k42*PV[ 8])+(k43*PV[ 9]+k44*PV[13]);
1793  double v14 =(k40*PV[10]+k41*PV[11]+k42*PV[12])+(k43*PV[13]+k44*PV[14]);
1794  PV[ 0]-=v0 ;
1795  PV[ 1]-=v1 ; PV[ 2]-=v2 ;
1796  PV[ 3]-=v3 ; PV[ 4]-=v4 ; PV[ 5]-=v5 ;
1797  PV[ 6]-=v6 ; PV[ 7]-=v7 ; PV[ 8]-=v8 ; PV[ 9]-=v9 ;
1798  PV[10]-=v10; PV[11]-=v11; PV[12]-=v12; PV[13]-=v13; PV[14]-=v14;
1799 
1800  if(PV[0]<=0.||PV[2]<=0.||PV[5]<=0.||PV[9]<=0.||PV[14]<=0.) return false;
1801 
1802  if(X) xi2 = Xi2(5,r,v);
1803  return true;
1804 }
1805 
1807 // Transformation track parameters to updator presentation
1808 // true if it is measured track parameters
1810 
1811 bool Trk::KalmanUpdator_xk::trackParametersToUpdator
1812 (const Trk::TrackParameters& T,double* P,double* V)
1813 {
1814  P[0] = T.parameters()[0];
1815  P[1] = T.parameters()[1];
1816  P[2] = T.parameters()[2];
1817  P[3] = T.parameters()[3];
1818  P[4] = T.parameters()[4];
1819 
1820 
1821  const AmgSymMatrix(5)* v = T.covariance();
1822  if(!v) return false;
1823 
1824  const AmgSymMatrix(5)& c = *v;
1825  V[ 0] = c(0,0);
1826  V[ 1] = c(1,0);
1827  V[ 2] = c(1,1);
1828  V[ 3] = c(2,0);
1829  V[ 4] = c(2,1);
1830  V[ 5] = c(2,2);
1831  V[ 6] = c(3,0);
1832  V[ 7] = c(3,1);
1833  V[ 8] = c(3,2);
1834  V[ 9] = c(3,3);
1835  V[10] = c(4,0);
1836  V[11] = c(4,1);
1837  V[12] = c(4,2);
1838  V[13] = c(4,3);
1839  V[14] = c(4,4);
1840  return true;
1841 }
1842 
1844 // Transformation track parameters to updator presentation
1845 // true if it is measured track parameters
1847 
1848 bool Trk::KalmanUpdator_xk::trackParametersToUpdator
1849 (const Trk::PatternTrackParameters& T,double* P,double* V)
1850 {
1851  const AmgVector(5) & par = T.parameters();
1852 
1853  P[0] = par[0];
1854  P[1] = par[1];
1855  P[2] = par[2];
1856  P[3] = par[3];
1857  P[4] = par[4];
1858 
1859  if(!T.iscovariance()) return false;
1860 
1861  const AmgSymMatrix(5) & cov = *T.covariance();
1862 
1863  V[ 0] = cov(0, 0);
1864  V[ 1] = cov(0, 1);
1865  V[ 2] = cov(1, 1);
1866  V[ 3] = cov(0, 2);
1867  V[ 4] = cov(1, 2);
1868  V[ 5] = cov(2, 2);
1869  V[ 6] = cov(0, 3);
1870  V[ 7] = cov(1, 3);
1871  V[ 8] = cov(2, 3);
1872  V[ 9] = cov(3, 3);
1873  V[10] = cov(0, 4);
1874  V[11] = cov(1, 4);
1875  V[12] = cov(2, 4);
1876  V[13] = cov(3, 4);
1877  V[14] = cov(4, 4);
1878  return true;
1879 }
1880 
1882 // Transformation local parameters to updator presentation
1883 // N - dimension of local parameters,K - key word
1884 // P - parameters, V -covariance
1886 
1887 bool Trk::KalmanUpdator_xk::localParametersToUpdator
1888 (const Trk::LocalParameters& L,const Amg::MatrixX& C,
1889  int& N,int& K,double* P,double* V)
1890 {
1891 
1892  N = C.rows(); if(N==0 || N>5 || L.dimension()!=N) return false;
1893  K = L.parameterKey();
1894 
1895  //const CLHEP::HepVector& H = L;
1896 
1897  P[ 0]=L(0);
1898  V[ 0]=C(0,0);
1899 
1900  if(N>1) {
1901  P[ 1]=L(1);
1902  V[ 1]=C(1,0); V[ 2]=C(1,1);
1903  }
1904  if(N>2) {
1905  P[ 2]=L(2);
1906  V[ 3]=C(2,0); V[ 4]=C(2,1); V[ 5]=C(2,2);
1907  }
1908  if(N>3) {
1909  P[ 3]=L(3);
1910  V[ 6]=C(3,0); V[ 7]=C(3,1); V[ 8]=C(3,2); V[ 9]=C(3,3);
1911  }
1912  if(N>4) {
1913  P[ 4]=L(4);
1914  V[10]=C(4,0); V[11]=C(4,1); V[12]=C(4,2); V[13]=C(4,3); V[14]=C(4,4);
1915  }
1916  return true;
1917 }
1918 
1920 // Track parameters production from updator presentation
1922 
1923 std::unique_ptr<Trk::TrackParameters> Trk::KalmanUpdator_xk::updatorToTrackParameters
1924 (const Trk::TrackParameters& T,double* P,double* V)
1925 {
1926  AmgSymMatrix(5) e;
1927  e<< V[ 0],V[ 1],V[ 3],V[ 6],V[10],
1928  V[ 1],V[ 2],V[ 4],V[ 7],V[11],
1929  V[ 3],V[ 4],V[ 5],V[ 8],V[12],
1930  V[ 6],V[ 7],V[ 8],V[ 9],V[13],
1931  V[10],V[11],V[12],V[13],V[14];
1932  return T.associatedSurface().createUniqueTrackParameters(P[0],P[1],P[2],P[3],P[4],std::move(e));
1933 }
1934 
1936 // Inversion of a positive definite symmetric matrix
1937 // with size (2x2), (3x3), (4x4) and (5x5)
1938 // Input parameters : n - size of matrix
1939 // a - the elements of the lower triangle of
1940 // the matrix to be inverted
1941 // Output parameters : b - inverted matrix
1943 
1944 bool Trk::KalmanUpdator_xk::invert(int n,double* a,double* b)
1945 {
1946  if(n==2) return invert2(a,b);
1947  if(n==3) return invert3(a,b);
1948  if(n==4) return invert4(a,b);
1949  if(n==5) return invert5(a,b);
1950  return false;
1951 }
1952 
1954 // Inversion of a positive definite symmetric matrix (2x2)
1955 // using Kramer's rule
1956 // Input parameters : a(0/2) - the elements of the lower triangle of
1957 // the matrix to be inverted
1958 // 0
1959 // 1 2
1960 // Output parameters : b(0/2) - inverted matrix
1962 
1963 bool Trk::KalmanUpdator_xk::invert2 (const double* a,double* b)
1964 {
1965  double d = a[0]*a[2]-a[1]*a[1]; if(d<=0.) return false; d=1./d;
1966  double b0 = a[2]*d;
1967  b[1] =-a[1]*d;
1968  b[2] = a[0]*d;
1969  b[0] = b0;
1970  return true;
1971 }
1972 
1974 // Inversion of a positive definite symmetric matrix (3x3)
1975 // using Kramer's rule
1976 // Input parameters : a(0/5) - the elements of the lower triangle of
1977 // the matrix to be inverted
1978 // 0
1979 // 1 2
1980 // 3 4 5
1981 // Output parameters : b(0/5) - inverted matrix
1983 
1984 bool Trk::KalmanUpdator_xk::invert3(const double* a,double* b)
1985 {
1986  double b0 = (a[2]*a[5]-a[4]*a[4]);
1987  double b1 =-(a[1]*a[5]-a[3]*a[4]);
1988  double b2 = (a[0]*a[5]-a[3]*a[3]);
1989  double b3 = (a[1]*a[4]-a[2]*a[3]);
1990  double b4 =-(a[0]*a[4]-a[1]*a[3]);
1991  double b5 = (a[0]*a[2]-a[1]*a[1]);
1992  double d = a[0]*b0+a[1]*b1+a[3]*b3; if(d<=0.) return false;
1993  b[0] = b0*(d=1./d);
1994  b[1] = b1*d;
1995  b[2] = b2*d;
1996  b[3] = b3*d;
1997  b[4] = b4*d;
1998  b[5] = b5*d;
1999  return true;
2000 }
2001 
2003 // Inversion of a positive definite symmetric matrix (4x4)
2004 // using Kramer's rule
2005 // Input parameters : a(0/9) - the elements of the lower triangle of
2006 // the matrix to be inverted
2007 // 0
2008 // 1 2
2009 // 3 4 5
2010 // 6 7 8 9
2011 // Output parameters : b(0/9) - inverted matrix
2013 
2014 bool Trk::KalmanUpdator_xk::invert4(const double* a,double* b)
2015 {
2016  double d00 = a[1]*a[4]-a[2]*a[3];
2017  double d01 = a[1]*a[5]-a[4]*a[3];
2018  double d02 = a[2]*a[5]-a[4]*a[4];
2019  double d03 = a[1]*a[7]-a[2]*a[6];
2020  double d04 = a[1]*a[8]-a[4]*a[6];
2021  double d05 = a[1]*a[9]-a[7]*a[6];
2022  double d06 = a[2]*a[8]-a[4]*a[7];
2023  double d07 = a[2]*a[9]-a[7]*a[7];
2024  double d08 = a[3]*a[7]-a[4]*a[6];
2025  double d09 = a[3]*a[8]-a[5]*a[6];
2026  double d10 = a[3]*a[9]-a[8]*a[6];
2027  double d11 = a[4]*a[8]-a[5]*a[7];
2028  double d12 = a[4]*a[9]-a[8]*a[7];
2029  double d13 = a[5]*a[9]-a[8]*a[8];
2030 
2031  // Determinant calculation
2032  //
2033  double c0 = a[2]*d13-a[4]*d12+a[7]*d11;
2034  double c1 = a[1]*d13-a[4]*d10+a[7]*d09;
2035  double c2 = a[1]*d12-a[2]*d10+a[7]*d08;
2036  double c3 = a[1]*d11-a[2]*d09+a[4]*d08;
2037  double det = a[0]*c0-a[1]*c1+a[3]*c2-a[6]*c3;
2038 
2039  if (det <= 0.) return false;
2040  det = 1./det;
2041 
2042  b[2] = (a[0]*d13-a[3]*d10+a[6]*d09)*det;
2043  b[4] = -(a[0]*d12-a[1]*d10+a[6]*d08)*det;
2044  b[5] = (a[0]*d07-a[1]*d05+a[6]*d03)*det;
2045  b[7] = (a[0]*d11-a[1]*d09+a[3]*d08)*det;
2046  b[8] = -(a[0]*d06-a[1]*d04+a[3]*d03)*det;
2047  b[9] = (a[0]*d02-a[1]*d01+a[3]*d00)*det;
2048  b[0] = c0 *det;
2049  b[1] = -c1 *det;
2050  b[3] = c2 *det;
2051  b[6] = -c3 *det;
2052  return true;
2053 }
2054 
2056 // Inversion of a positive definite symmetric matrix (5x5)
2057 // by a modification of the Gauss-Jordan method
2058 //
2059 // Input parameters : a(0/14) - the elements of the lower triangle of
2060 // the matrix to be inverted
2061 // 0
2062 // 1 2
2063 // 3 4 5
2064 // 6 7 8 9
2065 // 10 11 12 13 14
2066 // Output parameters : b(0/14) - inverted matrix
2068 
2069 bool Trk::KalmanUpdator_xk::invert5(const double* a,double* b)
2070 {
2071  if(a[ 0] <=0.) return false;
2072  double x1 = 1./a[ 0];
2073  double x2 =-a[ 1]*x1;
2074  double x3 =-a[ 3]*x1;
2075  double x4 =-a[ 6]*x1;
2076  double x5 =-a[10]*x1;
2077  double b0 = a[ 2]+a[ 1]*x2; if(b0<=0.) return false;
2078  double b1 = a[ 4]+a[ 3]*x2;
2079  double b2 = a[ 5]+a[ 3]*x3;
2080  double b3 = a[ 7]+a[ 6]*x2;
2081  double b4 = a[ 8]+a[ 6]*x3;
2082  double b5 = a[ 9]+a[ 6]*x4;
2083  double b6 = a[11]+a[10]*x2;
2084  double b7 = a[12]+a[10]*x3;
2085  double b8 = a[13]+a[10]*x4;
2086  double b9 = a[14]+a[10]*x5;
2087  double y1 = 1./b0;
2088  double y2 =-b1*y1;
2089  double y3 =-b3*y1;
2090  double y4 =-b6*y1;
2091  double y5 = x2*y1;
2092 
2093  if((b0 = b2+b1*y2) <=0.) return false;
2094  b1 = b4+b3*y2;
2095  b2 = b5+b3*y3;
2096  b3 = b7+b6*y2;
2097  b4 = b8+b6*y3;
2098  b5 = b9+b6*y4;
2099  b6 = x3+x2*y2;
2100  b7 = x4+x2*y3;
2101  b8 = x5+x2*y4;
2102  b9 = x1+x2*y5;
2103  x1 = 1./b0;
2104  x2 =-b1*x1;
2105  x3 =-b3*x1;
2106  x4 = b6*x1;
2107  x5 = y2*x1;
2108  if((b0 = b2+b1*x2) <=0.) return false;
2109  b1 = b4+b3*x2;
2110  b2 = b5+b3*x3;
2111  b3 = b7+b6*x2;
2112  b4 = b8+b6*x3;
2113  b5 = b9+b6*x4;
2114  b6 = y3+y2*x2;
2115  b7 = y4+y2*x3;
2116  b8 = y5+y2*x4;
2117  b9 = y1+y2*x5;
2118  y1 = 1./b0;
2119  y2 =-b1*y1;
2120  y3 = b3*y1;
2121  y4 = b6*y1;
2122  y5 = x2*y1;
2123  if((b0 = b2+b1*y2) <=0.) return false;
2124  b1 = b4+b3*y2;
2125  b2 = b5+b3*y3;
2126  b3 = b7+b6*y2;
2127  b4 = b8+b6*y3;
2128  b5 = b9+b6*y4;
2129  b6 = x3+x2*y2;
2130  b7 = x4+x2*y3;
2131  b8 = x5+x2*y4;
2132  b9 = x1+x2*y5;
2133  b[14] = 1./b0;
2134  b[10] = b1*b[14];
2135  b[11] = b3*b[14];
2136  b[12] = b6*b[14];
2137  b[13] = y2*b[14];
2138  b[ 0] = b2+b1*b[10];
2139  b[ 1] = b4+b3*b[10];
2140  b[ 2] = b5+b3*b[11];
2141  b[ 3] = b7+b6*b[10];
2142  b[ 4] = b8+b6*b[11];
2143  b[ 5] = b9+b6*b[12];
2144  b[ 6] = y3+y2*b[10];
2145  b[ 7] = y4+y2*b[11];
2146  b[ 8] = y5+y2*b[12];
2147  b[ 9] = y1+y2*b[13];
2148  return true;
2149 }
2150 
2152 // Xi2 for 2x2, 3x3, 4x4 and 5x5 matrix calculation
2153 // R - residial and W -weight matrix
2155 
2156 double Trk::KalmanUpdator_xk::Xi2(int N,double* R,double* W)
2157 {
2158  if(N==1) return Xi2for1(R,W);
2159  if(N==2) return Xi2for2(R,W);
2160  if(N==3) return Xi2for3(R,W);
2161  if(N==4) return Xi2for4(R,W);
2162  if(N==5) return Xi2for5(R,W);
2163  return 0.;
2164 }
2165 
2167 // Xi2 of 1x1 matrix and 1 parameters
2168 // R - residial and W -weight matrix
2170 
2171 double Trk::KalmanUpdator_xk::Xi2for1(const double* R,const double* W)
2172 {
2173  double Xi2 = R[0]*W[0]*R[0];
2174  return Xi2;
2175 }
2176 
2178 // Xi2 of 2x2 matrix and 2 parameters
2179 // R - residial and W -weight matrix
2181 
2182 double Trk::KalmanUpdator_xk::Xi2for2(const double* R,const double* W)
2183 {
2184  double Xi2 =
2185  (R[0]*W[ 0]+R[1]*W[ 1])*R[0]+
2186  (R[0]*W[ 1]+R[1]*W[ 2])*R[1];
2187  return Xi2;
2188 }
2189 
2191 // Xi2 of 3x3 matrix and 3 parameters
2192 // R - residial and W -weight matrix
2194 
2195 double Trk::KalmanUpdator_xk::Xi2for3(const double* R,const double* W)
2196 {
2197  double Xi2 =
2198  (R[0]*W[ 0]+R[1]*W[ 1]+R[2]*W[ 3])*R[0]+
2199  (R[0]*W[ 1]+R[1]*W[ 2]+R[2]*W[ 4])*R[1]+
2200  (R[0]*W[ 3]+R[1]*W[ 4]+R[2]*W[ 5])*R[2];
2201  return Xi2;
2202 }
2203 
2205 // Xi2 of 4x4 matrix and 4 parameters
2206 // R - residial and W -weight matrix
2208 
2209 double Trk::KalmanUpdator_xk::Xi2for4(const double* R,const double* W)
2210 {
2211  double Xi2 =
2212  ((R[0]*W[ 0]+R[1]*W[ 1])+(R[2]*W[ 3]+R[3]*W[ 6]))*R[0]+
2213  ((R[0]*W[ 1]+R[1]*W[ 2])+(R[2]*W[ 4]+R[3]*W[ 7]))*R[1]+
2214  ((R[0]*W[ 3]+R[1]*W[ 4])+(R[2]*W[ 5]+R[3]*W[ 8]))*R[2]+
2215  ((R[0]*W[ 6]+R[1]*W[ 7])+(R[2]*W[ 8]+R[3]*W[ 9]))*R[3];
2216  return Xi2;
2217 }
2218 
2220 // Xi2 of 5x5 matrix and 5 parameters
2221 // R - residial and W -weight matrix
2223 
2224 double Trk::KalmanUpdator_xk::Xi2for5(const double* R,const double* W)
2225 {
2226  double Xi2 =
2227  ((R[0]*W[ 0]+R[1]*W[ 1]+R[2]*W[ 3])+(R[3]*W[ 6]+R[4]*W[10]))*R[0]+
2228  ((R[0]*W[ 1]+R[1]*W[ 2]+R[2]*W[ 4])+(R[3]*W[ 7]+R[4]*W[11]))*R[1]+
2229  ((R[0]*W[ 3]+R[1]*W[ 4]+R[2]*W[ 5])+(R[3]*W[ 8]+R[4]*W[12]))*R[2]+
2230  ((R[0]*W[ 6]+R[1]*W[ 7]+R[2]*W[ 8])+(R[3]*W[ 9]+R[4]*W[13]))*R[3]+
2231  ((R[0]*W[10]+R[1]*W[11]+R[2]*W[12])+(R[3]*W[13]+R[4]*W[14]))*R[4];
2232  return Xi2;
2233 }
2234 
2236 // Residial between local and track parameters calculation
2237 // L - local parameters, T - track parameters, R-residial
2239 
2240 int Trk::KalmanUpdator_xk::differenceParLoc
2241 (int K,const double* L,const double* T, double* R)
2242 {
2243  const double pi2 = 2.*M_PI;
2244  const double pi = M_PI;
2245 
2246  int i = 0;
2247  if(K & 1) {R[i]=L[i]-T[0]; ++i;}
2248  if(K & 2) {R[i]=L[i]-T[1]; ++i;}
2249  if(K & 4) {
2250  R[i]=L[i]-T[2];
2251  if (R[i] > pi) R[i] = fmod(R[i]+pi,pi2)-pi;
2252  else if(R[i] <-pi) R[i] = fmod(R[i]-pi,pi2)+pi;
2253  ++i;
2254  }
2255  if(K & 8) {
2256  R[i]=L[i]-T[3];
2257  if (R[i] > pi) R[i] = fmod(R[i]+pi,pi2)-pi;
2258  else if(R[i] <-pi) R[i] = fmod(R[i]-pi,pi2)+pi;
2259  ++i;
2260  }
2261  if(K & 16) {R[i]=L[i]-T[4]; ++i;}
2262  return i;
2263 }
2264 
2266 // Residial between track parameters and local parameters calculation
2267 // L - local parameters, T - track parameters, R-residial
2269 
2270 void Trk::KalmanUpdator_xk::differenceLocPar
2271 (int K,const double* L,const double* T, double* R)
2272 {
2273  const double pi2 = 2.*M_PI;
2274  const double pi = M_PI;
2275  int i = 0;
2276  R[0]=0.; if(K & 1) R[0]=L[i++]-T[0];
2277  R[1]=0.; if(K & 2) R[1]=L[i++]-T[1];
2278  R[2]=0.; if(K & 4) {
2279  R[2]=L[i++]-T[2];
2280  if (R[2] > pi) R[2] = fmod(R[2]+pi,pi2)-pi;
2281  else if(R[2] <-pi) R[2] = fmod(R[2]-pi,pi2)+pi;
2282  }
2283  R[3]=0.; if(K & 8){
2284  R[3]=L[i++]-T[3];
2285  if (R[3] > pi) R[3] = fmod(R[3]+pi,pi2)-pi;
2286  else if(R[3] <-pi) R[3] = fmod(R[3]-pi,pi2)+pi;
2287  }
2288  R[4]=0.; if(K & 16) R[4]=L[i++]-T[4];
2289 }
2290 
2292 // Map for different combination of Local parameters preparation
2294 
2295 void Trk::KalmanUpdator_xk::mapKeyProduction()
2296 {
2297  int n=0; m_key[0]=m_key[1]=0;
2298 
2299  for(int K=1; K!= 32; ++K) {
2300 
2301  unsigned int I[5]={0,0,0,0,0};
2302  unsigned int m=0;
2303  for(int i=0; i!=5; ++i) {if((K>>i)&1) I[i]=1;}
2304 
2305  for(int i=0; i!=5; ++i) {
2306  for(int j=0; j<=i; ++j) {if(I[i] && I[j]) {m_map[n++] = m;} ++m;}
2307  }
2308  m_key[K+1] = n;
2309  }
2310 }
2311 
2313 // Test angles inside boundaries
2314 // Azimuthal angle p[2] shoud be > -pi and < +pi
2315 // Polar angle p[3] shoud be > 0 and < +pi
2317 
2318 void Trk::KalmanUpdator_xk::testAngles(double* p,double* v)
2319 {
2320  const double pi2 = 2.*M_PI;
2321  const double pi = M_PI;
2322 
2323  // Polar angle check
2324  //
2325  if (p[3] > pi) p[3] = fmod(p[3]+pi,pi2)-pi;
2326  else if(p[3] <-pi) p[3] = fmod(p[3]-pi,pi2)+pi;
2327 
2328  if (p[3] < 0.) {
2329 
2330  p[ 3] = -p[ 3];
2331  p[ 2]+= pi;
2332  v[ 6] = -v[ 6];
2333  v[ 7] = -v[ 7];
2334  v[ 8] = -v[ 8];
2335  v[13] = -v[13];
2336  }
2337 
2338  // Azimuthal angle check
2339  //
2340  if (p[2] > pi) p[2] = fmod(p[2]+pi,pi2)-pi;
2341  else if(p[2] <-pi) p[2] = fmod(p[2]-pi,pi2)+pi;
2342 }
2343 
beamspotman.r
def r
Definition: beamspotman.py:676
Trk::LocalParameters
Definition: LocalParameters.h:98
plotBeamSpotCompare.x1
x1
Definition: plotBeamSpotCompare.py:216
checkxAOD.ds
ds
Definition: Tools/PyUtils/bin/checkxAOD.py:257
Trk::KalmanUpdator_xk::differenceParLoc
static int differenceParLoc(int, const double *, const double *, double *)
Definition: KalmanUpdator_xk.cxx:2241
Trk::PatternTrackParameters::setParametersWithCovariance
void setParametersWithCovariance(const Surface *, const double *, const double *)
python.SystemOfUnits.s
int s
Definition: SystemOfUnits.py:131
Trk::k0
@ k0
Definition: ParticleHypothesis.h:35
Amg::VectorX
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorX
Dynamic Vector - dynamic allocation.
Definition: EventPrimitives.h:32
Amg::MatrixX
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Dynamic Matrix - dynamic allocation.
Definition: EventPrimitives.h:29
Trk::KalmanUpdator_xk::updateWithTwoDim
static bool updateWithTwoDim(int, bool, const double *, const double *, double *, double *, double &)
Definition: KalmanUpdator_xk.cxx:1418
python.SystemOfUnits.m
int m
Definition: SystemOfUnits.py:91
python.PerfMonSerializer.p
def p
Definition: PerfMonSerializer.py:743
TrackParameters.h
Trk::PatternTrackParameters::associatedSurface
virtual const Surface & associatedSurface() const override final
Trk::KalmanUpdator_xk::updateWithTwoDimWithBoundary
bool updateWithTwoDimWithBoundary(int, bool, double *, double *, double *, double *, double &) const
Definition: KalmanUpdator_xk.cxx:1542
Trk::KalmanUpdator_xk::initialErrors
virtual std::vector< double > initialErrors() const override final
let the client tools know how the assumptions on the initial precision for non-measured track paramet...
Definition: KalmanUpdator_xk.cxx:825
Trk::LocalParameters::parameterKey
int parameterKey() const
Identifier key for matrix expansion/reduction.
Trk::KalmanUpdator_xk::Xi2
static double Xi2(int, double *, double *)
Definition: KalmanUpdator_xk.cxx:2156
Amg::Vector2D
Eigen::Matrix< double, 2, 1 > Vector2D
Definition: GeoPrimitives.h:48
JetTiledMap::W
@ W
Definition: TiledEtaPhiMap.h:44
AthCommonDataStore< AthCommonMsg< AlgTool > >::declareProperty
Gaudi::Details::PropertyBase & declareProperty(Gaudi::Property< T > &t)
Definition: AthCommonDataStore.h:145
hist_file_dump.d
d
Definition: hist_file_dump.py:137
LArSC2NtupleDumper.nEt
nEt
Definition: LArSC2NtupleDumper.py:89
Trk::KalmanUpdator_xk::invert3
static bool invert3(const double *, double *)
Definition: KalmanUpdator_xk.cxx:1984
plotBeamSpotCompare.x2
x2
Definition: plotBeamSpotCompare.py:218
extractSporadic.c1
c1
Definition: extractSporadic.py:134
Trk::KalmanUpdator_xk::fullStateFitQuality
virtual FitQualityOnSurface fullStateFitQuality(const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &) const override final
estimator for FitQuality on Surface from a full track state, that is a state which contains the curre...
Definition: KalmanUpdator_xk.cxx:625
Trk::KalmanUpdator_xk::testAngles
static void testAngles(double *, double *)
Definition: KalmanUpdator_xk.cxx:2318
plotBeamSpotVxVal.cov
cov
Definition: plotBeamSpotVxVal.py:201
python.PhysicalConstants.pi2
float pi2
Definition: PhysicalConstants.py:52
M_PI
#define M_PI
Definition: ActiveFraction.h:11
Trk::KalmanUpdator_xk::m_cov0
std::vector< double > m_cov0
Definition: KalmanUpdator_xk.h:331
PlotCalibFromCool.ib
ib
Definition: PlotCalibFromCool.py:419
JetTiledMap::N
@ N
Definition: TiledEtaPhiMap.h:44
Trk::KalmanUpdator_xk::invert
static bool invert(int, double *, double *)
Definition: KalmanUpdator_xk.cxx:1944
read_hist_ntuple.t
t
Definition: read_hist_ntuple.py:5
Trk::KalmanUpdator_xk::updateWithOneDimWithBoundary
bool updateWithOneDimWithBoundary(int, bool, double *, double *, double *, double *, double &) const
Definition: KalmanUpdator_xk.cxx:1351
Trk::KalmanUpdator_xk::updateWithAnyDim
bool updateWithAnyDim(int, bool, double *, const double *, double *, double *, double &, int, int) const
Definition: KalmanUpdator_xk.cxx:1724
Trk::KalmanUpdator_xk::m_covBoundary
double m_covBoundary
Definition: KalmanUpdator_xk.h:334
Trk::KalmanUpdator_xk::updateWithTwoDimParameters
static bool updateWithTwoDimParameters(int, bool, const double *, const double *, double *, const double *, double &)
Definition: KalmanUpdator_xk.cxx:1490
makeTRTBarrelCans.y1
tuple y1
Definition: makeTRTBarrelCans.py:15
pi
#define pi
Definition: TileMuonFitter.cxx:65
Trk::KalmanUpdator_xk::updateNoMeasuredWithTwoDim
bool updateNoMeasuredWithTwoDim(const double *, const double *, double *, double *) const
Definition: KalmanUpdator_xk.cxx:1227
AthenaPoolTestRead.sc
sc
Definition: AthenaPoolTestRead.py:27
Monitored::X
@ X
Definition: HistogramFillerUtils.h:24
PlotCalibFromCool.ie
ie
Definition: PlotCalibFromCool.py:420
ParticleJetTools::p3
Amg::Vector3D p3(const xAOD::TruthVertex *p)
Definition: ParticleJetLabelCommon.cxx:55
Trk::KalmanUpdator_xk::combineStates
virtual std::unique_ptr< TrackParameters > combineStates(const TrackParameters &, const TrackParameters &) const override final
adds to a track state the parameters from another state using a statistical combination - use with ca...
Definition: KalmanUpdator_xk.cxx:399
Trk::KalmanUpdator_xk::update
std::unique_ptr< TrackParameters > update(const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &, FitQualityOnSurface *&, int, bool) const
Definition: KalmanUpdator_xk.cxx:847
Trk::AmgSymMatrix
AmgSymMatrix(5) &GXFTrackState
Definition: GXFTrackState.h:156
Trk::FitQualityOnSurface
Definition: FitQualityOnSurface.h:19
compileRPVLLRates_emergingFilterTest.c3
c3
Definition: compileRPVLLRates_emergingFilterTest.py:559
hotSpotInTAG.c0
c0
Definition: hotSpotInTAG.py:192
Trk::KalmanUpdator_xk::Xi2for1
static double Xi2for1(const double *, const double *)
Definition: KalmanUpdator_xk.cxx:2171
Trk::KalmanUpdator_xk::finalize
virtual StatusCode finalize() override final
Definition: KalmanUpdator_xk.cxx:75
Trk::KalmanUpdator_xk::updatorToTrackParameters
static std::unique_ptr< TrackParameters > updatorToTrackParameters(const TrackParameters &, double *, double *)
Definition: KalmanUpdator_xk.cxx:1924
KalmanUpdator_xk.h
Trk::KalmanUpdator_xk::updateNoMeasuredWithOneDim
bool updateNoMeasuredWithOneDim(const double *, const double *, double *, double *) const
Definition: KalmanUpdator_xk.cxx:1199
parseMapping.v0
def v0
Definition: parseMapping.py:149
lumiFormat.i
int i
Definition: lumiFormat.py:92
Trk::KalmanUpdator_xk::trackParametersToUpdator
static bool trackParametersToUpdator(const TrackParameters &, double *, double *)
Definition: KalmanUpdator_xk.cxx:1812
Trk::KalmanUpdator_xk::KalmanUpdator_xk
KalmanUpdator_xk(const std::string &, const std::string &, const IInterface *)
Definition: KalmanUpdator_xk.cxx:24
beamspotman.n
n
Definition: beamspotman.py:731
endmsg
#define endmsg
Definition: AnalysisConfig_Ntuple.cxx:63
EL::StatusCode
::StatusCode StatusCode
StatusCode definition for legacy code.
Definition: PhysicsAnalysis/D3PDTools/EventLoop/EventLoop/StatusCode.h:22
AmgVector
AmgVector(4) T2BSTrackFilterTool
Definition: T2BSTrackFilterTool.cxx:114
makeTRTBarrelCans.y2
tuple y2
Definition: makeTRTBarrelCans.py:18
WritePulseShapeToCool.det
det
Definition: WritePulseShapeToCool.py:204
Trk::KalmanUpdator_xk::addToStateOneDimension
virtual bool addToStateOneDimension(PatternTrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &, PatternTrackParameters &) const override final
add without chi2 calculation, PRD-level, pattern track parameters, specifically 1D
Definition: KalmanUpdator_xk.cxx:109
Trk::KalmanUpdator_xk::updateWithFiveDim
static bool updateWithFiveDim(bool, double *, double *, double *, double *, double &)
Definition: KalmanUpdator_xk.cxx:1621
Trk::KalmanUpdator_xk::Xi2for4
static double Xi2for4(const double *, const double *)
Definition: KalmanUpdator_xk.cxx:2209
Trk::ParametersBase
Definition: ParametersBase.h:55
Trk::KalmanUpdator_xk::invert5
static bool invert5(const double *, double *)
Definition: KalmanUpdator_xk.cxx:2069
Trk::KalmanUpdator_xk::Xi2for3
static double Xi2for3(const double *, const double *)
Definition: KalmanUpdator_xk.cxx:2195
mergePhysValFiles.errors
list errors
Definition: DataQuality/DataQualityUtils/scripts/mergePhysValFiles.py:43
Trk::KalmanUpdator_xk::~KalmanUpdator_xk
virtual ~KalmanUpdator_xk()
Trk::KalmanUpdator_xk::differenceLocPar
static void differenceLocPar(int, const double *, const double *, double *)
Definition: KalmanUpdator_xk.cxx:2271
Trk::KalmanUpdator_xk::addToState
virtual std::unique_ptr< TrackParameters > addToState(const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &) const override final
add without chi2 calculation, PRD-level, EDM track parameters
Definition: KalmanUpdator_xk.cxx:85
Trk::KalmanUpdator_xk::predictedStateFitQuality
virtual FitQualityOnSurface predictedStateFitQuality(const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &) const override final
estimator for FitQuality on Surface from a predicted track state, that is a state which does not cont...
Definition: KalmanUpdator_xk.cxx:509
Trk::KalmanUpdator_xk::updateWithOneDim
static bool updateWithOneDim(int, bool, const double *, const double *, double *, double *, double &)
Definition: KalmanUpdator_xk.cxx:1293
name
std::string name
Definition: Control/AthContainers/Root/debug.cxx:195
createCoolChannelIdFile.par
par
Definition: createCoolChannelIdFile.py:29
Trk::KalmanUpdator_xk::mapKeyProduction
void mapKeyProduction()
Definition: KalmanUpdator_xk.cxx:2295
VP1PartSpect::E
@ E
Definition: VP1PartSpectFlags.h:21
plotBeamSpotMon.b
b
Definition: plotBeamSpotMon.py:77
compileRPVLLRates.c2
c2
Definition: compileRPVLLRates.py:361
Trk::KalmanUpdator_xk::updateNoMeasuredWithAnyDim
bool updateNoMeasuredWithAnyDim(const double *, const double *, double *, double *, int) const
Definition: KalmanUpdator_xk.cxx:1258
Trk::KalmanUpdator_xk::updateParameterDifference
virtual std::pair< AmgVector(5), AmgSymMatrix(5)> * updateParameterDifference(const AmgVector(5) &, const AmgSymMatrix(5) &, const Amg::VectorX &, const Amg::MatrixX &, int, Trk::FitQualityOnSurface *&, bool) const override final
pure AMG interface for reference-track KF, allowing update of parameter differences
Definition: KalmanUpdator_xk.cxx:281
LocalParameters.h
Trk::LocalParameters::dimension
int dimension() const
Dimension of this localParameters() vector.
ReadCellNoiseFromCoolCompare.v2
v2
Definition: ReadCellNoiseFromCoolCompare.py:364
Trk::KalmanUpdator_xk::Xi2for2
static double Xi2for2(const double *, const double *)
Definition: KalmanUpdator_xk.cxx:2182
Trk::KalmanUpdator_xk::invert4
static bool invert4(const double *, double *)
Definition: KalmanUpdator_xk.cxx:2014
Trk::KalmanUpdator_xk::localParametersToUpdator
static bool localParametersToUpdator(const LocalParameters &, const Amg::MatrixX &, int &, int &, double *, double *)
Definition: KalmanUpdator_xk.cxx:1888
dqt_zlumi_pandas.update
update
Definition: dqt_zlumi_pandas.py:42
DiTauMassTools::MaxHistStrategyV2::e
e
Definition: PhysicsAnalysis/TauID/DiTauMassTools/DiTauMassTools/HelperFunctions.h:26
a
TList * a
Definition: liststreamerinfos.cxx:10
Trk::KalmanUpdator_xk::Xi2for5
static double Xi2for5(const double *, const double *)
Definition: KalmanUpdator_xk.cxx:2224
Trk::KalmanUpdator_xk::initialize
virtual StatusCode initialize() override final
Definition: KalmanUpdator_xk.cxx:53
Trk::PatternTrackParameters
Definition: PatternTrackParameters.h:38
ReadCellNoiseFromCoolCompare.s2
s2
Definition: ReadCellNoiseFromCoolCompare.py:379
python.changerun.pv
pv
Definition: changerun.py:81
extractSporadic.q
list q
Definition: extractSporadic.py:98
PatternTrackParameters.h
Trk::KalmanUpdator_xk::updateOneDimension
bool updateOneDimension(PatternTrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &, int, bool, PatternTrackParameters &, double &) const
Definition: KalmanUpdator_xk.cxx:963
I
#define I(x, y, z)
Definition: MD5.cxx:116
AthAlgTool
Definition: AthAlgTool.h:26
python.IoTestsLib.w
def w
Definition: IoTestsLib.py:200
Trk::KalmanUpdator_xk::invert2
static bool invert2(const double *, double *)
Definition: KalmanUpdator_xk.cxx:1963
python.compressB64.c
def c
Definition: compressB64.py:93
python.AutoConfigFlags.msg
msg
Definition: AutoConfigFlags.py:7
Trk::v
@ v
Definition: ParamDefs.h:84
fitman.k
k
Definition: fitman.py:528
Trk::KalmanUpdator_xk::removeFromState
virtual std::unique_ptr< TrackParameters > removeFromState(const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &) const override final
the reverse updating or inverse KalmanFilter removes a measurement from the track state,...
Definition: KalmanUpdator_xk.cxx:123
Generated on Thu Jul 4 2024 21:19:19 for ATLAS Offline Software by   doxygen 1.8.18