ATLAS Offline Software
Loading...
Searching...
No Matches
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
23Trk::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
46Trk::KalmanUpdator_xk::~KalmanUpdator_xk()
47= default;
48
50// Initialisation
52
53StatusCode 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
75StatusCode Trk::KalmanUpdator_xk::finalize()
76{
77 return StatusCode::SUCCESS;
78}
79
81// Add local position together with erro matrix without Xi2 calculation
83
84std::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
95bool 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
108bool 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
122std::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
133bool 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
148std::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
159bool 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
174std::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
185bool 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
200std::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
211bool 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
224bool 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
241std::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
252bool 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
267std::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
280std::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
357bool 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
372std::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
383bool 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
398std::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
418bool 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
451std::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
473bool 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
507Trk::FitQualityOnSurface
508Trk::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
526bool 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
548Trk::FitQualityOnSurface
549Trk::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
585bool 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
623Trk::FitQualityOnSurface
624Trk::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
641bool 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
663Trk::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
698bool 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
735Trk::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
778bool 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
825std::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
846std::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
908bool 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
962bool 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
1018std::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
1079bool 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
1128bool 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
1162bool 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
1198bool 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
1226bool 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
1257bool 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
1292bool 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
1350bool 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
1417bool 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
1489bool 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
1541bool 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
1620bool 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
1723bool 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
1811bool 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
1848bool 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
1887bool 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
1923std::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
1944bool 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
1963bool 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
1984bool 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
2014bool 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
2069bool 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
2156double 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
2171double 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
2182double 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
2195double 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
2209double 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
2224double 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
2240int 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
2270void 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
2295void 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
2318void 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
M_PI
#define M_PI
Definition ActiveFraction.h:11
endmsg
#define endmsg
Definition AnalysisConfig_Ntuple.cxx:63
AmgSymMatrix
#define AmgSymMatrix(dim)
Definition EventPrimitives.h:50
AmgVector
#define AmgVector(rows)
Definition EventPrimitives.h:55
KalmanUpdator_xk.h
a
static Double_t a
Definition LArPhysWaveHECTool.cxx:38
al
static Double_t al
Definition LArPhysWaveHECTool.cxx:38
P
static Double_t P(Double_t *tt, Double_t *par)
Definition LArPhysWaveHECTool.cxx:280
sc
static Double_t sc
Definition LArPhysWaveHECTool.cxx:37
I
#define I(x, y, z)
Definition MD5.cxx:116
pi
#define pi
Definition TileMuonFitter.cxx:65
AthAlgTool::AthAlgTool
AthAlgTool(const std::string &type, const std::string &name, const IInterface *parent)
Constructor with parameters:
Definition AthAlgTool.cxx:16
AthCommonDataStore< AthCommonMsg< AlgTool > >::declareProperty
Gaudi::Details::PropertyBase & declareProperty(Gaudi::Property< T, V, H > &t)
Definition AthCommonDataStore.h:145
Trk::KalmanUpdator_xk::trackParametersToUpdator
static bool trackParametersToUpdator(const TrackParameters &, double *, double *)
Definition KalmanUpdator_xk.cxx:1812
Trk::KalmanUpdator_xk::invert5
static bool invert5(const double *, double *)
Definition KalmanUpdator_xk.cxx:2069
Trk::KalmanUpdator_xk::initialize
virtual StatusCode initialize() override final
Definition KalmanUpdator_xk.cxx:53
Trk::KalmanUpdator_xk::m_cov0
std::vector< double > m_cov0
Definition KalmanUpdator_xk.h:331
Trk::KalmanUpdator_xk::updateNoMeasuredWithOneDim
bool updateNoMeasuredWithOneDim(const double *, const double *, double *, double *) const
Definition KalmanUpdator_xk.cxx:1199
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
Trk::KalmanUpdator_xk::localParametersToUpdator
static bool localParametersToUpdator(const LocalParameters &, const Amg::MatrixX &, int &, int &, double *, double *)
Definition KalmanUpdator_xk.cxx:1888
Trk::KalmanUpdator_xk::updateWithTwoDim
static bool updateWithTwoDim(int, bool, const double *, const double *, double *, double *, double &)
Definition KalmanUpdator_xk.cxx:1418
Trk::KalmanUpdator_xk::~KalmanUpdator_xk
virtual ~KalmanUpdator_xk()
Trk::KalmanUpdator_xk::updatorToTrackParameters
static std::unique_ptr< TrackParameters > updatorToTrackParameters(const TrackParameters &, double *, double *)
Definition KalmanUpdator_xk.cxx:1924
Trk::KalmanUpdator_xk::updateNoMeasuredWithTwoDim
bool updateNoMeasuredWithTwoDim(const double *, const double *, double *, double *) const
Definition KalmanUpdator_xk.cxx:1227
Trk::KalmanUpdator_xk::invert3
static bool invert3(const double *, double *)
Definition KalmanUpdator_xk.cxx:1984
Trk::KalmanUpdator_xk::updateWithOneDim
static bool updateWithOneDim(int, bool, const double *, const double *, double *, double *, double &)
Definition KalmanUpdator_xk.cxx:1293
Trk::KalmanUpdator_xk::differenceParLoc
static int differenceParLoc(int, const double *, const double *, double *)
Definition KalmanUpdator_xk.cxx:2241
Trk::KalmanUpdator_xk::updateNoMeasuredWithAnyDim
bool updateNoMeasuredWithAnyDim(const double *, const double *, double *, double *, int) const
Definition KalmanUpdator_xk.cxx:1258
Trk::KalmanUpdator_xk::Xi2
static double Xi2(int, double *, double *)
Definition KalmanUpdator_xk.cxx:2156
Trk::KalmanUpdator_xk::Xi2for4
static double Xi2for4(const double *, const double *)
Definition KalmanUpdator_xk.cxx:2209
Trk::KalmanUpdator_xk::mapKeyProduction
void mapKeyProduction()
Definition KalmanUpdator_xk.cxx:2295
Trk::KalmanUpdator_xk::KalmanUpdator_xk
KalmanUpdator_xk(const std::string &, const std::string &, const IInterface *)
Definition KalmanUpdator_xk.cxx:24
Trk::KalmanUpdator_xk::Xi2for2
static double Xi2for2(const double *, const double *)
Definition KalmanUpdator_xk.cxx:2182
Trk::KalmanUpdator_xk::m_covBoundary
double m_covBoundary
Definition KalmanUpdator_xk.h:334
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::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::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
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::KalmanUpdator_xk::updateWithFiveDim
static bool updateWithFiveDim(bool, double *, double *, double *, double *, double &)
Definition KalmanUpdator_xk.cxx:1621
Trk::KalmanUpdator_xk::invert4
static bool invert4(const double *, double *)
Definition KalmanUpdator_xk.cxx:2014
Trk::KalmanUpdator_xk::updateOneDimension
bool updateOneDimension(PatternTrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &, int, bool, PatternTrackParameters &, double &) const
Definition KalmanUpdator_xk.cxx:963
Trk::KalmanUpdator_xk::updateWithOneDimWithBoundary
bool updateWithOneDimWithBoundary(int, bool, double *, double *, double *, double *, double &) const
Definition KalmanUpdator_xk.cxx:1351
Trk::KalmanUpdator_xk::differenceLocPar
static void differenceLocPar(int, const double *, const double *, double *)
Definition KalmanUpdator_xk.cxx:2271
Trk::KalmanUpdator_xk::updateWithTwoDimWithBoundary
bool updateWithTwoDimWithBoundary(int, bool, double *, double *, double *, double *, double &) const
Definition KalmanUpdator_xk.cxx:1542
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::Xi2for3
static double Xi2for3(const double *, const double *)
Definition KalmanUpdator_xk.cxx:2195
Trk::KalmanUpdator_xk::Xi2for1
static double Xi2for1(const double *, const double *)
Definition KalmanUpdator_xk.cxx:2171
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::updateWithTwoDimParameters
static bool updateWithTwoDimParameters(int, bool, const double *, const double *, double *, const double *, double &)
Definition KalmanUpdator_xk.cxx:1490
Trk::KalmanUpdator_xk::invert
static bool invert(int, double *, double *)
Definition KalmanUpdator_xk.cxx:1944
Trk::KalmanUpdator_xk::m_key
unsigned int m_key[33]
Definition KalmanUpdator_xk.h:332
Trk::KalmanUpdator_xk::invert2
static bool invert2(const double *, double *)
Definition KalmanUpdator_xk.cxx:1963
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::update
std::unique_ptr< TrackParameters > update(const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &, FitQualityOnSurface *&, int, bool) const
Definition KalmanUpdator_xk.cxx:847
Trk::KalmanUpdator_xk::Xi2for5
static double Xi2for5(const double *, const double *)
Definition KalmanUpdator_xk.cxx:2224
Trk::KalmanUpdator_xk::finalize
virtual StatusCode finalize() override final
Definition KalmanUpdator_xk.cxx:75
Trk::KalmanUpdator_xk::m_map
unsigned int m_map[160]
Definition KalmanUpdator_xk.h:333
Trk::KalmanUpdator_xk::testAngles
static void testAngles(double *, double *)
Definition KalmanUpdator_xk.cxx:2318
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::PatternTrackParameters::setParametersWithCovariance
void setParametersWithCovariance(const Surface *, const double *, const double *)
r
int r
Definition globals.cxx:22
C
struct color C
Amg::MatrixX
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Dynamic Matrix - dynamic allocation.
Definition EventPrimitives.h:27
Amg::Vector2D
Eigen::Matrix< double, 2, 1 > Vector2D
Definition GeoPrimitives.h:48
Amg::VectorX
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorX
Dynamic Vector - dynamic allocation.
Definition EventPrimitives.h:30
Trk::v
@ v
Definition ParamDefs.h:78
Trk::TrackParameters
ParametersBase< TrackParametersDim, Charged > TrackParameters
Definition Tracking/TrkEvent/TrkParameters/TrkParameters/TrackParameters.h:27
msg
MsgStream & msg
Definition testRead.cxx:32
Generated on for ATLAS Offline Software by doxygen 1.14.0