CbmRoot
CbmLitKalmanFilter.cxx
Go to the documentation of this file.
1 
6 #include "fitter/CbmLitKalmanFilter.h"
7 
8 #include "data/CbmLitPixelHit.h"
9 #include "data/CbmLitStripHit.h"
10 #include "data/CbmLitTrackParam.h"
11 #include "utils/CbmLitMatrixMath.h"
12 
13 #include <cmath>
14 #include <iostream>
15 
16 #include "TMath.h"
17 #include "TMatrixD.h"
18 
19 litfloat CbmLitTrackParam::fSpeedOfLight = 1.e-7 * TMath::C();
20 
21 CbmLitKalmanFilter::CbmLitKalmanFilter() {}
22 
23 CbmLitKalmanFilter::~CbmLitKalmanFilter() {}
24 
25 LitStatus CbmLitKalmanFilter::Update(const CbmLitTrackParam* parIn,
26  CbmLitTrackParam* parOut,
27  const CbmLitHit* hit,
28  litfloat& chiSq) {
29  *parOut = *parIn;
30  return Update(parOut, hit, chiSq);
31 }
32 
33 LitStatus CbmLitKalmanFilter::Update(CbmLitTrackParam* par,
34  const CbmLitHit* hit,
35  litfloat& chiSq) {
36  LitStatus result = kLITSUCCESS;
37  if (hit->GetType() == kLITSTRIPHIT) {
38  result = Update(par, static_cast<const CbmLitStripHit*>(hit), chiSq);
39  } else if (hit->GetType() == kLITPIXELHIT) {
40  result = Update(par, static_cast<const CbmLitPixelHit*>(hit), chiSq);
41  }
42  return result;
43 }
44 
45 LitStatus CbmLitKalmanFilter::Update(CbmLitTrackParam* par,
46  const CbmLitPixelHit* hit,
47  litfloat& chiSq) {
48  std::vector<litfloat> cIn = par->GetCovMatrix();
49 
50  static const litfloat ONE = 1., TWO = 2.;
51 
52  litfloat dxx = hit->GetDx() * hit->GetDx();
53  litfloat dxy = hit->GetDxy();
54  litfloat dyy = hit->GetDy() * hit->GetDy();
55  litfloat dtt = hit->GetDt() * hit->GetDt();
56 
57  // calculate residuals
58  litfloat dx = hit->GetX() - par->GetX();
59  litfloat dy = hit->GetY() - par->GetY();
60  litfloat dt = hit->GetT() - par->GetTime();
61 
62  // Calculate and inverse residual covariance matrix
63  //litfloat t = ONE / (dxx * dyy + dxx * cIn[5] + dyy * cIn[0] + cIn[0] * cIn[5] -
64  //dxy * dxy - TWO * dxy * cIn[1] - cIn[1] * cIn[1]);
65  litfloat t =
66  ONE
67  / ((cIn[0] + dxx) * ((cIn[6] + dyy) * (cIn[20] + dtt) - cIn[10] * cIn[10])
68  - (cIn[1] + dxy) * ((cIn[1] + dxy) * (cIn[20] + dtt) - cIn[5] * cIn[10])
69  + cIn[5] * ((cIn[1] + dxy) * cIn[10] - (cIn[6] + dyy) * cIn[5]));
70  //litfloat R00 = (dyy + cIn[5]) * t;
71  //litfloat R01 = -(dxy + cIn[1]) * t;
72  //litfloat R11 = (dxx + cIn[0]) * t;
73  litfloat R00 = ((cIn[6] + dyy) * (cIn[20] + dtt) - cIn[10] * cIn[10]) * t;
74  litfloat R01 = (cIn[5] * cIn[10] - (cIn[1] + dxy) * (cIn[20] + dtt)) * t;
75  litfloat R02 = ((cIn[1] + dxy) * cIn[10] - (cIn[6] + dyy) * cIn[5]) * t;
76  litfloat R11 = ((cIn[0] + dxx) * (cIn[20] + dtt) - cIn[5] * cIn[5]) * t;
77  litfloat R12 = ((cIn[1] + dxy) * cIn[5] - (cIn[0] + dxx) * cIn[10]) * t;
78  litfloat R22 =
79  ((cIn[0] + dxx) * (cIn[6] + dyy) - (cIn[1] + dxy) * (cIn[1] + dxy)) * t;
80 
81  /*TMatrixD ResM(3, 3);
82  ResM(0, 0) = dxx + cIn[0];
83  ResM(0, 1) = dxy + cIn[1];
84  ResM(0, 2) = cIn[5];
85  ResM(1, 0) = dxy + cIn[1];
86  ResM(1, 1) = dyy + cIn[6];
87  ResM(1, 2) = cIn[10];
88  ResM(2, 0) = cIn[5];
89  ResM(2, 1) = cIn[10];
90  ResM(2, 2) = dtt + cIn[20];
91  ResM.Invert();
92 
93  litfloat R00 = ResM(0, 0);
94  litfloat R01 = ResM(0, 1);
95  litfloat R02 = ResM(0, 2);
96  litfloat R11 = ResM(1, 1);
97  litfloat R12 = ResM(1, 2);
98  litfloat R22 = ResM(2, 2);*/
99 
100  // Calculate Kalman gain matrix
101  litfloat K00 = cIn[0] * R00 + cIn[1] * R01 + cIn[5] * R02;
102  litfloat K01 = cIn[0] * R01 + cIn[1] * R11 + cIn[5] * R12;
103  litfloat K02 = cIn[0] * R02 + cIn[1] * R12 + cIn[5] * R22;
104  litfloat K10 = cIn[1] * R00 + cIn[6] * R01 + cIn[10] * R02;
105  litfloat K11 = cIn[1] * R01 + cIn[6] * R11 + cIn[10] * R12;
106  litfloat K12 = cIn[1] * R02 + cIn[6] * R12 + cIn[10] * R22;
107  litfloat K20 = cIn[2] * R00 + cIn[7] * R01 + cIn[14] * R02;
108  litfloat K21 = cIn[2] * R01 + cIn[7] * R11 + cIn[14] * R12;
109  litfloat K22 = cIn[2] * R02 + cIn[7] * R12 + cIn[14] * R22;
110  litfloat K30 = cIn[3] * R00 + cIn[8] * R01 + cIn[17] * R02;
111  litfloat K31 = cIn[3] * R01 + cIn[8] * R11 + cIn[17] * R12;
112  litfloat K32 = cIn[3] * R02 + cIn[8] * R12 + cIn[17] * R22;
113  litfloat K40 = cIn[4] * R00 + cIn[9] * R01 + cIn[19] * R02;
114  litfloat K41 = cIn[4] * R01 + cIn[9] * R11 + cIn[19] * R12;
115  litfloat K42 = cIn[4] * R02 + cIn[9] * R12 + cIn[19] * R22;
116  litfloat K50 = cIn[5] * R00 + cIn[10] * R01 + cIn[20] * R02;
117  litfloat K51 = cIn[5] * R01 + cIn[10] * R11 + cIn[20] * R12;
118  litfloat K52 = cIn[5] * R02 + cIn[10] * R12 + cIn[20] * R22;
119 
120  // Calculate filtered state vector
121  litfloat xOut[6] = {par->GetX(),
122  par->GetY(),
123  par->GetTx(),
124  par->GetTy(),
125  par->GetQp(),
126  par->GetTime()};
127  xOut[0] += K00 * dx + K01 * dy + K02 * dt;
128  xOut[1] += K10 * dx + K11 * dy + K12 * dt;
129  xOut[2] += K20 * dx + K21 * dy + K22 * dt;
130  xOut[3] += K30 * dx + K31 * dy + K32 * dt;
131  xOut[4] += K40 * dx + K41 * dy + K42 * dt;
132  xOut[5] += K50 * dx + K51 * dy + K52 * dt;
133 
134  // Calculate filtered covariance matrix
135  std::vector<litfloat> cOut = cIn;
136 
137  cOut[0] -= K00 * cIn[0] + K01 * cIn[1] + K02 * cIn[5];
138  cOut[1] -= K00 * cIn[1] + K01 * cIn[6] + K02 * cIn[10];
139  cOut[2] -= K00 * cIn[2] + K01 * cIn[7] + K02 * cIn[14];
140  cOut[3] -= K00 * cIn[3] + K01 * cIn[8] + K02 * cIn[17];
141  cOut[4] -= K00 * cIn[4] + K01 * cIn[9] + K02 * cIn[19];
142  cOut[5] -= K00 * cIn[5] + K01 * cIn[10] + K02 * cIn[20];
143 
144  cOut[6] -= K10 * cIn[1] + K11 * cIn[6] + K12 * cIn[10];
145  cOut[7] -= K10 * cIn[2] + K11 * cIn[7] + K12 * cIn[14];
146  cOut[8] -= K10 * cIn[3] + K11 * cIn[8] + K12 * cIn[17];
147  cOut[9] -= K10 * cIn[4] + K11 * cIn[9] + K12 * cIn[19];
148  cOut[10] -= K10 * cIn[5] + K11 * cIn[10] + K12 * cIn[20];
149 
150  cOut[11] -= K20 * cIn[2] + K21 * cIn[7] + K22 * cIn[14];
151  cOut[12] -= K20 * cIn[3] + K21 * cIn[8] + K22 * cIn[17];
152  cOut[13] -= K20 * cIn[4] + K21 * cIn[9] + K22 * cIn[19];
153  cOut[14] -= K20 * cIn[5] + K21 * cIn[10] + K22 * cIn[20];
154 
155  cOut[15] -= K30 * cIn[3] + K31 * cIn[8] + K32 * cIn[17];
156  cOut[16] -= K30 * cIn[4] + K31 * cIn[9] + K32 * cIn[19];
157  cOut[17] -= K30 * cIn[5] + K31 * cIn[10] + K32 * cIn[20];
158 
159  cOut[18] -= K40 * cIn[4] + K41 * cIn[9] + K42 * cIn[19];
160  cOut[19] -= K40 * cIn[5] + K41 * cIn[10] + K42 * cIn[20];
161 
162  cOut[20] -= K50 * cIn[5] + K51 * cIn[10] + K52 * cIn[20];
163 
164  // Copy filtered state to output
165  par->SetX(xOut[0]);
166  par->SetY(xOut[1]);
167  par->SetTx(xOut[2]);
168  par->SetTy(xOut[3]);
169  par->SetQp(xOut[4]);
170  par->SetTime(xOut[5]);
171  par->SetCovMatrix(cOut);
172 
173  // Calculate chi-square
174  litfloat xmx = hit->GetX() - par->GetX();
175  litfloat ymy = hit->GetY() - par->GetY();
176  litfloat tmt = hit->GetT() - par->GetTime();
177  litfloat C0 = cOut[0];
178  litfloat C1 = cOut[1];
179  litfloat C5 = cOut[6];
180 
181  /*litfloat norm = dxx * dyy - dxx * C5 - dyy * C0 + C0 * C5
182  - dxy * dxy + 2 * dxy * C1 - C1 * C1;
183 
184  chiSq = ((xmx * (dyy - C5) - ymy * (dxy - C1)) * xmx
185  +(-xmx * (dxy - C1) + ymy * (dxx - C0)) * ymy) / norm +
186  (hit->GetT() - par->GetTime()) * (hit->GetT() - par->GetTime()) / (hit->GetDt() * hit->GetDt() + cOut[20]);*/
187  litfloat norm =
188  (dxx - cOut[0]) * ((dyy - cOut[6]) * (dtt - cOut[20]) - cOut[10] * cOut[10])
189  + (dxy - cOut[1])
190  * (cOut[5] * cOut[10] - (dxy - cOut[1]) * (dtt - cOut[20]))
191  + cOut[5] * ((dxy - cOut[1]) * cOut[10] - (dyy - cOut[6]) * cOut[5]);
192 
193  if (norm == 0.) { norm = 1e-10; }
194 
195  // Mij is the (symmetric) inverse of the residual matrix
196  litfloat M00 =
197  ((dyy - cOut[6]) * (dtt - cOut[20]) - cOut[10] * cOut[10]) / norm;
198  litfloat M01 =
199  ((dxy - cOut[1]) * (dtt - cOut[20]) - cOut[5] * cOut[10]) / norm;
200  litfloat M02 =
201  ((dxy - cOut[1]) * cOut[10] - (dyy - cOut[6]) * cOut[5]) / norm;
202  litfloat M11 =
203  ((dxx - cOut[0]) * (dtt - cOut[20]) - cOut[5] * cOut[5]) / norm;
204  litfloat M12 =
205  ((dxx - cOut[0]) * cOut[10] - (dxy - cOut[1]) * cOut[5]) / norm;
206  litfloat M22 =
207  ((dxx - cOut[0]) * (dyy - cOut[6]) - (dxy - cOut[1]) * (dxy - cOut[1]))
208  / norm;
209  /*TMatrixD Chi2M(3, 3);
210  Chi2M(0, 0) = dxx - cOut[0];
211  Chi2M(0, 1) = dxy - cOut[1];
212  Chi2M(0, 2) = -cOut[5];
213  Chi2M(1, 0) = dxy - cOut[1];
214  Chi2M(1, 1) = dyy - cOut[6];
215  Chi2M(1, 2) = -cOut[10];
216  Chi2M(2, 0) = -cOut[5];
217  Chi2M(2, 1) = -cOut[10];
218  Chi2M(2, 2) = dtt - cOut[20];
219  Chi2M.Invert();
220 
221  litfloat M00 = Chi2M(0, 0);
222  litfloat M01 = Chi2M(0, 1);
223  litfloat M02 = Chi2M(0, 2);
224  litfloat M11 = Chi2M(1, 1);
225  litfloat M12 = Chi2M(1, 2);
226  litfloat M22 = Chi2M(2, 2);*/
227 
228  chiSq = xmx * (xmx * M00 + ymy * M01 + tmt * M02)
229  + ymy * (xmx * M01 + ymy * M11 + tmt * M12)
230  + tmt * (xmx * M02 + ymy * M12 + tmt * M22);
231 
232  return kLITSUCCESS;
233 }
234 
235 LitStatus CbmLitKalmanFilter::UpdateWMF(CbmLitTrackParam* par,
236  const CbmLitPixelHit* hit,
237  litfloat& chiSq) {
238  litfloat xIn[5] = {
239  par->GetX(), par->GetY(), par->GetTx(), par->GetTy(), par->GetQp()};
240 
241  std::vector<litfloat> cIn = par->GetCovMatrix();
242  std::vector<litfloat> cInInv = par->GetCovMatrix();
243 
244  litfloat dxx = hit->GetDx() * hit->GetDx();
245  litfloat dxy = hit->GetDxy();
246  litfloat dyy = hit->GetDy() * hit->GetDy();
247 
248  // Inverse predicted cov matrix
249  InvSym15(cInInv);
250  // Calculate C1
251  std::vector<litfloat> C1 = cInInv;
252  litfloat det = dxx * dyy - dxy * dxy;
253  C1[0] += dyy / det;
254  C1[1] += -dxy / det;
255  C1[5] += dxx / det;
256  // Inverse C1 -> output updated covariance matrix
257  InvSym15(C1);
258 
259  std::vector<litfloat> t(5);
260  t[0] = cInInv[0] * par->GetX() + cInInv[1] * par->GetY()
261  + cInInv[2] * par->GetTx() + cInInv[3] * par->GetTy()
262  + cInInv[4] * par->GetQp()
263  + (dyy * hit->GetX() - dxy * hit->GetY()) / det;
264  t[1] = cInInv[1] * par->GetX() + cInInv[5] * par->GetY()
265  + cInInv[6] * par->GetTx() + cInInv[7] * par->GetTy()
266  + cInInv[8] * par->GetQp()
267  + (-dxy * hit->GetX() + dxx * hit->GetY()) / det;
268  t[2] = cInInv[2] * par->GetX() + cInInv[6] * par->GetY()
269  + cInInv[9] * par->GetTx() + cInInv[10] * par->GetTy()
270  + cInInv[11] * par->GetQp();
271  t[3] = cInInv[3] * par->GetX() + cInInv[7] * par->GetY()
272  + cInInv[10] * par->GetTx() + cInInv[12] * par->GetTy()
273  + cInInv[13] * par->GetQp();
274  t[4] = cInInv[4] * par->GetX() + cInInv[8] * par->GetY()
275  + cInInv[11] * par->GetTx() + cInInv[13] * par->GetTy()
276  + cInInv[14] * par->GetQp();
277 
278  std::vector<litfloat> xOut(5);
279  Mult15On5(C1, t, xOut);
280 
281  // Copy filtered state to output
282  par->SetX(xOut[0]);
283  par->SetY(xOut[1]);
284  par->SetTx(xOut[2]);
285  par->SetTy(xOut[3]);
286  par->SetQp(xOut[4]);
287  par->SetCovMatrix(C1);
288 
289  // Calculate chi square
290  litfloat dx0 = xOut[0] - xIn[0];
291  litfloat dx1 = xOut[1] - xIn[1];
292  litfloat dx2 = xOut[2] - xIn[2];
293  litfloat dx3 = xOut[3] - xIn[3];
294  litfloat dx4 = xOut[4] - xIn[4];
295  litfloat xmx = hit->GetX() - par->GetX();
296  litfloat ymy = hit->GetY() - par->GetY();
297  chiSq = ((xmx * dyy - ymy * dxy) * xmx + (-xmx * dxy + ymy * dxx) * ymy) / det
298  + (dx0 * cInInv[0] + dx1 * cInInv[1] + dx2 * cInInv[2]
299  + dx3 * cInInv[3] + dx4 * cInInv[4])
300  * dx0
301  + (dx0 * cInInv[1] + dx1 * cInInv[5] + dx2 * cInInv[6]
302  + dx3 * cInInv[7] + dx4 * cInInv[8])
303  * dx1
304  + (dx0 * cInInv[2] + dx1 * cInInv[6] + dx2 * cInInv[9]
305  + dx3 * cInInv[10] + dx4 * cInInv[11])
306  * dx2
307  + (dx0 * cInInv[3] + dx1 * cInInv[7] + dx2 * cInInv[10]
308  + dx3 * cInInv[12] + dx4 * cInInv[13])
309  * dx3
310  + (dx0 * cInInv[4] + dx1 * cInInv[8] + dx2 * cInInv[11]
311  + dx3 * cInInv[13] + dx4 * cInInv[14])
312  * dx4;
313 
314  return kLITSUCCESS;
315 }
316 
317 LitStatus CbmLitKalmanFilter::Update(CbmLitTrackParam* par,
318  const CbmLitStripHit* hit,
319  litfloat& chiSq) {
320  litfloat xIn[5] = {
321  par->GetX(), par->GetY(), par->GetTx(), par->GetTy(), par->GetQp()};
322  std::vector<litfloat> cIn = par->GetCovMatrix();
323 
324  litfloat u = hit->GetU();
325  litfloat duu = hit->GetDu() * hit->GetDu();
326  litfloat phiCos = hit->GetCosPhi();
327  litfloat phiSin = hit->GetSinPhi();
328  litfloat phiCosSq = phiCos * phiCos;
329  litfloat phiSinSq = phiSin * phiSin;
330  litfloat phi2SinCos = 2 * phiCos * phiSin;
331 
332  // Inverted covariance matrix of predicted residual
333  litfloat R =
334  1. / (duu + cIn[0] * phiCosSq + phi2SinCos * cIn[1] + cIn[5] * phiSinSq);
335 
336  // Calculate Kalman gain matrix
337  litfloat K0 = cIn[0] * phiCos + cIn[1] * phiSin;
338  litfloat K1 = cIn[1] * phiCos + cIn[5] * phiSin;
339  litfloat K2 = cIn[2] * phiCos + cIn[6] * phiSin;
340  litfloat K3 = cIn[3] * phiCos + cIn[7] * phiSin;
341  litfloat K4 = cIn[4] * phiCos + cIn[8] * phiSin;
342 
343  litfloat KR0 = K0 * R;
344  litfloat KR1 = K1 * R;
345  litfloat KR2 = K2 * R;
346  litfloat KR3 = K3 * R;
347  litfloat KR4 = K4 * R;
348 
349  // Residual of predictions
350  litfloat r = u - xIn[0] * phiCos - xIn[1] * phiSin;
351 
352  // Calculate filtered state vector
353  std::vector<litfloat> xOut(5);
354  xOut[0] = xIn[0] + KR0 * r;
355  xOut[1] = xIn[1] + KR1 * r;
356  xOut[2] = xIn[2] + KR2 * r;
357  xOut[3] = xIn[3] + KR3 * r;
358  xOut[4] = xIn[4] + KR4 * r;
359 
360  // Calculate filtered covariance matrix
361  std::vector<litfloat> cOut(15);
362  cOut[0] = cIn[0] - KR0 * K0;
363  cOut[1] = cIn[1] - KR0 * K1;
364  cOut[2] = cIn[2] - KR0 * K2;
365  cOut[3] = cIn[3] - KR0 * K3;
366  cOut[4] = cIn[4] - KR0 * K4;
367 
368  cOut[5] = cIn[5] - KR1 * K1;
369  cOut[6] = cIn[6] - KR1 * K2;
370  cOut[7] = cIn[7] - KR1 * K3;
371  cOut[8] = cIn[8] - KR1 * K4;
372 
373  cOut[9] = cIn[9] - KR2 * K2;
374  cOut[10] = cIn[10] - KR2 * K3;
375  cOut[11] = cIn[11] - KR2 * K4;
376 
377  cOut[12] = cIn[12] - KR3 * K3;
378  cOut[13] = cIn[13] - KR3 * K4;
379 
380  cOut[14] = cIn[14] - KR4 * K4;
381 
382  // Copy filtered state to output
383  par->SetX(xOut[0]);
384  par->SetY(xOut[1]);
385  par->SetTx(xOut[2]);
386  par->SetTy(xOut[3]);
387  par->SetQp(xOut[4]);
388  par->SetCovMatrix(cOut);
389 
390  // Filtered resuduals
391  litfloat ru = u - xOut[0] * phiCos - xOut[1] * phiSin;
392 
393  // Calculate chi-square
394  chiSq =
395  (ru * ru)
396  / (duu - phiCosSq * cOut[0] - phi2SinCos * cOut[1] - phiSinSq * cOut[5]);
397 
398  return kLITSUCCESS;
399 }
400 
401 LitStatus CbmLitKalmanFilter::UpdateWMF(CbmLitTrackParam* par,
402  const CbmLitStripHit* hit,
403  litfloat& chiSq) {
404  litfloat xIn[5] = {
405  par->GetX(), par->GetY(), par->GetTx(), par->GetTy(), par->GetQp()};
406 
407  std::vector<litfloat> cIn = par->GetCovMatrix();
408  std::vector<litfloat> cInInv = par->GetCovMatrix();
409 
410  litfloat duu = hit->GetDu() * hit->GetDu();
411  litfloat phiCos = hit->GetCosPhi();
412  litfloat phiSin = hit->GetSinPhi();
413 
414  // Inverse predicted cov matrix
415  InvSym15(cInInv);
416  // Calculate C1
417  std::vector<litfloat> C1 = cInInv;
418  C1[0] += phiCos * phiCos / duu;
419  C1[1] += phiCos * phiSin / duu;
420  C1[5] += phiSin * phiSin / duu;
421  // Inverse C1 -> output updated covariance matrix
422  InvSym15(C1);
423 
424  std::vector<litfloat> t(5);
425  t[0] = cInInv[0] * par->GetX() + cInInv[1] * par->GetY()
426  + cInInv[2] * par->GetTx() + cInInv[3] * par->GetTy()
427  + cInInv[4] * par->GetQp() + phiCos * hit->GetU() / duu;
428  t[1] = cInInv[1] * par->GetX() + cInInv[5] * par->GetY()
429  + cInInv[6] * par->GetTx() + cInInv[7] * par->GetTy()
430  + cInInv[8] * par->GetQp() + phiSin * hit->GetU() / duu;
431  t[2] = cInInv[2] * par->GetX() + cInInv[6] * par->GetY()
432  + cInInv[9] * par->GetTx() + cInInv[10] * par->GetTy()
433  + cInInv[11] * par->GetQp();
434  t[3] = cInInv[3] * par->GetX() + cInInv[7] * par->GetY()
435  + cInInv[10] * par->GetTx() + cInInv[12] * par->GetTy()
436  + cInInv[13] * par->GetQp();
437  t[4] = cInInv[4] * par->GetX() + cInInv[8] * par->GetY()
438  + cInInv[11] * par->GetTx() + cInInv[13] * par->GetTy()
439  + cInInv[14] * par->GetQp();
440 
441  std::vector<litfloat> xOut(5);
442  Mult15On5(C1, t, xOut);
443 
444  // Copy filtered state to output
445  par->SetX(xOut[0]);
446  par->SetY(xOut[1]);
447  par->SetTx(xOut[2]);
448  par->SetTy(xOut[3]);
449  par->SetQp(xOut[4]);
450  par->SetCovMatrix(C1);
451 
452  // Calculate chi square
453  litfloat zeta = hit->GetU() - phiCos * xOut[0] - phiSin * xOut[1];
454  litfloat dx0 = xOut[0] - xIn[0];
455  litfloat dx1 = xOut[1] - xIn[1];
456  litfloat dx2 = xOut[2] - xIn[2];
457  litfloat dx3 = xOut[3] - xIn[3];
458  litfloat dx4 = xOut[4] - xIn[4];
459  chiSq = zeta * zeta / duu
460  + (dx0 * cInInv[0] + dx1 * cInInv[1] + dx2 * cInInv[2]
461  + dx3 * cInInv[3] + dx4 * cInInv[4])
462  * dx0
463  + (dx0 * cInInv[1] + dx1 * cInInv[5] + dx2 * cInInv[6]
464  + dx3 * cInInv[7] + dx4 * cInInv[8])
465  * dx1
466  + (dx0 * cInInv[2] + dx1 * cInInv[6] + dx2 * cInInv[9]
467  + dx3 * cInInv[10] + dx4 * cInInv[11])
468  * dx2
469  + (dx0 * cInInv[3] + dx1 * cInInv[7] + dx2 * cInInv[10]
470  + dx3 * cInInv[12] + dx4 * cInInv[13])
471  * dx3
472  + (dx0 * cInInv[4] + dx1 * cInInv[8] + dx2 * cInInv[11]
473  + dx3 * cInInv[13] + dx4 * cInInv[14])
474  * dx4;
475 
476  return kLITSUCCESS;
477 }
CbmLitHit::GetT
litfloat GetT() const
Definition: CbmLitHit.h:50
CbmLitTrackParam.h
Data class for track parameters.
CbmLitTrackParam::SetTy
void SetTy(litfloat ty)
Definition: CbmLitTrackParam.h:68
litfloat
double litfloat
Definition: CbmLitFloat.h:15
CbmLitPixelHit::GetDy
litfloat GetDy() const
Definition: CbmLitPixelHit.h:40
CbmLitStripHit.h
Base data class for strip hits.
CbmLitStripHit::GetCosPhi
litfloat GetCosPhi() const
Definition: CbmLitStripHit.h:41
CbmLitTrackParam::GetX
litfloat GetX() const
Definition: CbmLitTrackParam.h:53
CbmLitKalmanFilter::Update
virtual LitStatus Update(const CbmLitTrackParam *parIn, CbmLitTrackParam *parOut, const CbmLitHit *hit, litfloat &chiSq)
Main function to be implemented for concrete track update algorithm.
Definition: CbmLitKalmanFilter.cxx:25
CbmLitTrackParam
Data class for track parameters.
Definition: CbmLitTrackParam.h:29
CbmLitTrackParam::GetY
litfloat GetY() const
Definition: CbmLitTrackParam.h:54
CbmLitTrackParam::GetQp
litfloat GetQp() const
Definition: CbmLitTrackParam.h:58
CbmLitTrackParam::SetQp
void SetQp(litfloat qp)
Definition: CbmLitTrackParam.h:69
CbmLitPixelHit.h
Base data class for pixel hits.
CbmLitStripHit::GetU
litfloat GetU() const
Definition: CbmLitStripHit.h:38
kLITSUCCESS
@ kLITSUCCESS
Definition: CbmLitEnums.h:24
CbmLitKalmanFilter::~CbmLitKalmanFilter
virtual ~CbmLitKalmanFilter()
Definition: CbmLitKalmanFilter.cxx:23
CbmLitTrackParam::SetTime
void SetTime(litfloat t)
Definition: CbmLitTrackParam.h:70
CbmLitKalmanFilter::UpdateWMF
LitStatus UpdateWMF(CbmLitTrackParam *par, const CbmLitPixelHit *hit, litfloat &chiSq)
Definition: CbmLitKalmanFilter.cxx:235
CbmLitPixelHit::GetY
litfloat GetY() const
Definition: CbmLitPixelHit.h:38
CbmLitHit
Base data class for hits.
Definition: CbmLitHit.h:26
kLITSTRIPHIT
@ kLITSTRIPHIT
Definition: CbmLitEnums.h:15
CbmLitTrackParam::SetY
void SetY(litfloat y)
Definition: CbmLitTrackParam.h:65
InvSym15
bool InvSym15(std::vector< litfloat > &a)
Definition: CbmLitMatrixMath.cxx:41
CbmLitPixelHit
Base data class for pixel hits.
Definition: CbmLitPixelHit.h:22
kLITPIXELHIT
@ kLITPIXELHIT
Definition: CbmLitEnums.h:16
CbmLitStripHit::GetDu
litfloat GetDu() const
Definition: CbmLitStripHit.h:39
CbmLitStripHit::GetSinPhi
litfloat GetSinPhi() const
Definition: CbmLitStripHit.h:42
CbmLitPixelHit::GetDxy
litfloat GetDxy() const
Definition: CbmLitPixelHit.h:41
Mult15On5
bool Mult15On5(const std::vector< litfloat > &a, const std::vector< litfloat > &b, std::vector< litfloat > &c)
Definition: CbmLitMatrixMath.cxx:351
CbmLitPixelHit::GetX
litfloat GetX() const
Definition: CbmLitPixelHit.h:37
CbmLitHit::GetDt
litfloat GetDt() const
Definition: CbmLitHit.h:51
CbmLitTrackParam::SetCovMatrix
void SetCovMatrix(const vector< litfloat > &C)
Definition: CbmLitTrackParam.h:71
CbmLitMatrixMath.h
CbmLitTrackParam::GetCovMatrix
const vector< litfloat > & GetCovMatrix() const
Definition: CbmLitTrackParam.h:61
CbmLitTrackParam::GetTy
litfloat GetTy() const
Definition: CbmLitTrackParam.h:57
CbmLitKalmanFilter.h
CbmLitTrackParam::SetX
void SetX(litfloat x)
Definition: CbmLitTrackParam.h:64
LitStatus
LitStatus
Definition: CbmLitEnums.h:23
CbmLitTrackParam::GetTime
litfloat GetTime() const
Definition: CbmLitTrackParam.h:59
CbmLitStripHit
Base data class for strip hits.
Definition: CbmLitStripHit.h:22
CbmLitHit::GetType
LitHitType GetType() const
Definition: CbmLitHit.h:47
CbmLitTrackParam::GetTx
litfloat GetTx() const
Definition: CbmLitTrackParam.h:56
CbmLitPixelHit::GetDx
litfloat GetDx() const
Definition: CbmLitPixelHit.h:39
CbmLitTrackParam::SetTx
void SetTx(litfloat tx)
Definition: CbmLitTrackParam.h:67
NS_L1TrackFitter::ONE
const fvec ONE
Definition: L1TrackFitter.cxx:33
CbmLitTrackParam::fSpeedOfLight
static litfloat fSpeedOfLight
Definition: CbmLitTrackParam.h:31
CbmLitKalmanFilter::CbmLitKalmanFilter
CbmLitKalmanFilter()
Definition: CbmLitKalmanFilter.cxx:21