[u/mrichter/AliRoot.git] / STEER / AliTrackFitterKalman.cxx

/**************************************************************************
 * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
 *                                                                        *
 * Author: The ALICE Off-line Project.                                    *
 * Contributors are mentioned in the code where appropriate.              *
 *                                                                        *
 * Permission to use, copy, modify and distribute this software and its   *
 * documentation strictly for non-commercial purposes is hereby granted   *
 * without fee, provided that the above copyright notice appears in all   *
 * copies and that both the copyright notice and this permission notice   *
 * appear in the supporting documentation. The authors make no claims     *
 * about the suitability of this software for any purpose. It is          *
 * provided "as is" without express or implied warranty.                  *
 **************************************************************************/

///////////////////////////////////////////////////////////////////////////////
//
//                      Kalman-Filter-like fit 
//   to a straight-line crossing a set of arbitrarily oriented planes.
//   The resulting line is given by the equation:
//                  (x-x0)/vx = (y-y0)/1 = (z-z0)/vz
//   Parameters of the fit are:
//        x0,y0,z0 (a point on the line) and
//        vx,1,vz  (a vector collinear with the line)
//
//   LIMITATION:  The line must not be perpendicular to the Y axis. 
//
//          Origin: Iouri Belikov, CERN, Jouri.Belikov@cern.ch
//
//////////////////////////////////////////////////////////////////////////////

#include <TMatrixD.h>
#include <TMatrixDSym.h>

//#include "AliLog.h"
#include "AliTrackFitterKalman.h"

ClassImp(AliTrackFitterKalman)

const Double_t AliTrackFitterKalman::fgkMaxChi2=33.;

AliTrackFitterKalman::
AliTrackFitterKalman(AliTrackPointArray *array, Bool_t owner):
  AliTrackFitter(array,owner),
  fMaxChi2(fgkMaxChi2),
  fSeed(kFALSE)
{
  //
  // Constructor
  //
}

Bool_t AliTrackFitterKalman::Begin(Int_t first, Int_t last) {
  //
  // Make a seed out of the track points with the indices "first" and "last". 
  // This is the "default" seed.
  //
  if (fSeed) return kTRUE;
  AliTrackPoint fp,lp;
  fPoints->GetPoint(fp,first); fPoints->GetPoint(lp,last);
  return MakeSeed(&fp,&lp);
}


Bool_t AliTrackFitterKalman::
MakeSeed(const AliTrackPoint *p0, const AliTrackPoint *p1) {
  //
  // Make a seed out of the two track points 
  //
  Double_t x0=p0->GetX(), y0=p0->GetY(), z0=p0->GetZ();
  Double_t dx=p1->GetX()-x0, dy=p1->GetY()-y0, dz=p1->GetZ()-z0;
  Double_t vx=dx/dy;
  Double_t vz=dz/dy;
  Double_t par[5]={x0,y0,z0,vx,vz};

  const Float_t *cv0=p0->GetCov();
  const Float_t *cv1=p1->GetCov();
  Double_t rdx2=(cv0[0]+cv1[0])/dx/dx;
  Double_t rdy2=(cv0[3]+cv1[3])/dy/dy;
  Double_t rdz2=(cv0[5]+cv1[5])/dz/dz;
  Double_t svx2=vx*vx*(rdx2+rdy2);     
  Double_t svz2=vz*vz*(rdz2+rdy2);     
  Double_t cov[15]={
     cv0[0],
     cv0[1],cv0[3],
     cv0[2],cv0[4],cv0[5],
     0.,0.,0.,svx2,
     0.,0.,0.,0.,svz2
  };

  SetSeed(par,cov);

  return kTRUE;
}

Bool_t AliTrackFitterKalman::AddPoint(const AliTrackPoint *p)
{
  //
  // Add a point to the fit
  //

  if (!Propagate(p))   return kFALSE;
  Double_t chi2=GetPredictedChi2(p);
  if (chi2>fMaxChi2)   return kFALSE;
  if (!Update(p,chi2)) return kFALSE;
  
  return kTRUE;
}


Double_t AliTrackFitterKalman::GetPredictedChi2(const AliTrackPoint *p) const {
  //
  // Calculate the predicted chi2 increment.
  //

  Float_t txyz[3]={GetParam()[0],GetParam()[1],GetParam()[2]};
  TMatrixDSym &cv=*fCov;
  Float_t tcov[6]={
    cv(0,0),cv(1,0),cv(2,0),
            cv(1,1),cv(2,1),
                    cv(2,2)
  };
  AliTrackPoint tp(txyz,tcov,p->GetVolumeID());

  Double_t chi2=p->GetResidual(tp,kTRUE);

  return chi2;
}


Bool_t AliTrackFitterKalman::Propagate(const AliTrackPoint *p) {
  //
  // Propagate the track towards the measured point "p"
  //

  TMatrixDSym &cv=*fCov;
  Double_t s=p->GetY() - fParams[1];
  Double_t sig2=s*s/12.; 

  Double_t vx=fParams[3], vz=fParams[4];
  fParams[0] += s*vx;
  fParams[1] += s;
  fParams[2] += s*vz;

  Double_t 
  c00 = cv(0,0) + 2*s*cv(3,0) + s*s*cv(3,3) + vx*vx*sig2,

  c10 = cv(1,0) + s*cv(1,3) + vx*sig2, c11=cv(1,1) + sig2,
  
  c20 = cv(2,0) + s*(cv(4,0) + cv(2,3)) + s*s*cv(4,3) + vx*vz*sig2,
  c21 = cv(2,1) + s*cv(4,1) + vz*sig2,
  c22 = cv(2,2) + 2*s*cv(4,2) + s*s*cv(4,4) + vz*vz*sig2,

  c30 = cv(3,0) + s*cv(3,3), c31 = cv(3,1),
  c32 = cv(3,2) + s*cv(3,4), c33 = cv(3,3),

  c40 = cv(4,0) + s*cv(4,3), c41 = cv(4,1),
  c42 = cv(4,2) + s*cv(4,4), c43 = cv(4,3), c44 = cv(4,4);


  cv(0,0)=c00; cv(0,1)=c10; cv(0,2)=c20; cv(0,3)=c30; cv(0,4)=c40;
  cv(1,0)=c10; cv(1,1)=c11; cv(1,2)=c21; cv(1,3)=c31; cv(1,4)=c41;
  cv(2,0)=c20; cv(2,1)=c21; cv(2,2)=c22; cv(2,3)=c32; cv(2,4)=c42;
  cv(3,0)=c30; cv(3,1)=c31; cv(3,2)=c32; cv(3,3)=c33; cv(3,4)=c43;
  cv(4,0)=c40; cv(4,1)=c41; cv(4,2)=c42; cv(4,3)=c43; cv(4,4)=c44;

  return kTRUE;
}

Bool_t AliTrackFitterKalman::Update(const AliTrackPoint *p, Double_t chi2) {
  //
  // Update the track params using the measured point "p"
  //

  TMatrixDSym &c=*fCov;
  const Float_t *cov=p->GetCov();

  TMatrixDSym v(3);
  v(0,0)=cov[0]+c(0,0); v(0,1)=cov[1]+c(0,1); v(0,2)=cov[2]+c(0,2);
  v(1,0)=cov[1]+c(1,0); v(1,1)=cov[3]+c(1,1); v(1,2)=cov[4]+c(1,2);
  v(2,0)=cov[2]+c(2,0); v(2,1)=cov[4]+c(2,1); v(2,2)=cov[5]+c(2,2);
  v.Invert();

  TMatrixD ch(5,3);
  ch(0,0)=c(0,0); ch(0,1)=c(0,1); ch(0,2)=c(0,2);
  ch(1,0)=c(1,0); ch(1,1)=c(1,1); ch(1,2)=c(1,2);
  ch(2,0)=c(2,0); ch(2,1)=c(2,1); ch(2,2)=c(2,2);
  ch(3,0)=c(3,0); ch(3,1)=c(3,1); ch(3,2)=c(3,2);
  ch(4,0)=c(4,0); ch(4,1)=c(4,1); ch(4,2)=c(4,2);

  TMatrixD k(ch,TMatrixD::kMult,v);

  TMatrixD d(3,1);
  d(0,0) = p->GetX() - fParams[0];
  d(1,0) = p->GetY() - fParams[1];
  d(2,0) = p->GetZ() - fParams[2];

  TMatrixD x(k,TMatrixD::kMult,d);

  fParams[0]+=x(0,0);
  fParams[1]+=x(1,0);
  fParams[2]+=x(2,0);
  fParams[3]+=x(3,0);
  fParams[4]+=x(4,0);

  TMatrixD hc(3,5);
  hc(0,0)=c(0,0);hc(0,1)=c(0,1);hc(0,2)=c(0,2);hc(0,3)=c(0,3);hc(0,4)=c(0,4);
  hc(1,0)=c(1,0);hc(1,1)=c(1,1);hc(1,2)=c(1,2);hc(1,3)=c(1,3);hc(1,4)=c(1,4);
  hc(2,0)=c(2,0);hc(2,1)=c(2,1);hc(2,2)=c(2,2);hc(2,3)=c(2,3);hc(2,4)=c(2,4);
  
  TMatrixD s(k,TMatrixD::kMult,hc);

  c(0,0)-=s(0,0);c(0,1)-=s(0,1);c(0,2)-=s(0,2);c(0,3)-=s(0,3);c(0,4)-=s(0,4);
  c(1,0)-=s(1,0);c(1,1)-=s(1,1);c(1,2)-=s(1,2);c(1,3)-=s(1,3);c(1,4)-=s(1,4);
  c(2,0)-=s(2,0);c(2,1)-=s(2,1);c(2,2)-=s(2,2);c(2,3)-=s(2,3);c(2,4)-=s(2,4);
  c(3,0)-=s(3,0);c(3,1)-=s(3,1);c(3,2)-=s(3,2);c(3,3)-=s(3,3);c(3,4)-=s(3,4);
  c(4,0)-=s(4,0);c(4,1)-=s(4,1);c(4,2)-=s(4,2);c(4,3)-=s(4,3);c(4,4)-=s(4,4);

  fChi2 += chi2;
  fNdf  += 2;

  return kTRUE;
}

//_____________________________________________________________________________
Bool_t AliTrackFitterKalman::GetPCA(const AliTrackPoint &p, AliTrackPoint &i) const
{
  //
  // Get the intersection point "i" between the track and the plane
  // the point "p" belongs to.
  //

  TMatrixD t(3,1);
  Double_t s=p.GetY() - fParams[1];
  Double_t vx=fParams[3], vz=fParams[4];
  t(0,0) = fParams[0] + s*vx;
  t(1,0) = fParams[1] + s;
  t(2,0) = fParams[2] + s*vz;
  
  TMatrixDSym tC(3);
  {
  Double_t sig2=s*s/12.;

  tC(0,0) = vx*vx*sig2;
  tC(1,0) = vx*sig2; 
  tC(1,1) = sig2;
  tC(2,0) = vx*vz*sig2;
  tC(2,1) = vz*sig2;
  tC(2,2) = vz*vz*sig2;

  tC(0,1) = tC(1,0); tC(0,2) = tC(2,0);
                     tC(1,2) = tC(2,1);
  }

  TMatrixD m(3,1);
  m(0,0)=p.GetX();
  m(1,0)=p.GetY();
  m(2,0)=p.GetZ();
 
  TMatrixDSym mC(3);
  {
  const Float_t *cv=p.GetCov();
  mC(0,0)=cv[0]; mC(0,1)=cv[1]; mC(0,2)=cv[2];
  mC(1,0)=cv[1]; mC(1,1)=cv[3]; mC(1,2)=cv[4];
  mC(2,0)=cv[2]; mC(2,1)=cv[4]; mC(2,2)=cv[5];
  }

  TMatrixDSym tmW(tC);
  tmW+=mC;
  tmW.Invert();

  TMatrixD mW(tC,TMatrixD::kMult,tmW);
  TMatrixD tW(mC,TMatrixD::kMult,tmW);

  TMatrixD mi(mW,TMatrixD::kMult,m);
  TMatrixD ti(tW,TMatrixD::kMult,t);
  ti+=mi;

  TMatrixD iC(tC,TMatrixD::kMult,tmW);
  iC*=mC;
  Double_t sig2=1.;  // Releasing the matrix by 1 cm along the track direction 
  iC(0,0) += vx*vx*sig2;
  iC(0,1) += vx*sig2; 
  iC(1,1) += sig2;
  iC(0,2) += vx*vz*sig2;
  iC(1,2) += vz*sig2;
  iC(2,2) += vz*vz*sig2;


  Float_t cov[6]={
    iC(0,0), iC(0,1), iC(0,2),
             iC(1,1), iC(1,2),
                      iC(2,2)
  };
  i.SetXYZ(ti(0,0),ti(1,0),ti(2,0),cov);
  UShort_t id=p.GetVolumeID();
  i.SetVolumeID(id);

  return kTRUE;
}


//_____________________________________________________________________________
void 
AliTrackFitterKalman::SetSeed(const Double_t par[5], const Double_t cov[15]) {
  //
  // Set the initial approximation for the track parameters
  //
  for (Int_t i=0; i<5; i++) fParams[i]=par[i];
  fParams[5]=0.;

  delete fCov;
  fCov=new TMatrixDSym(5);
  TMatrixDSym &cv=*fCov;

  cv(0,0)=cov[0 ];
  cv(1,0)=cov[1 ]; cv(1,1)=cov[2 ];
  cv(2,0)=cov[3 ]; cv(2,1)=cov[4 ]; cv(2,2)=cov[5 ];
  cv(3,0)=cov[6 ]; cv(3,1)=cov[7 ]; cv(3,2)=cov[8 ]; cv(3,3)=cov[9 ];
  cv(4,0)=cov[10]; cv(4,1)=cov[11]; cv(4,2)=cov[12]; cv(4,3)=cov[13]; cv(4,4)=cov[14];
 
  cv(0,1)=cv(1,0);
  cv(0,2)=cv(2,0); cv(1,2)=cv(2,1);
  cv(0,3)=cv(3,0); cv(1,3)=cv(3,1); cv(2,3)=cv(3,2);
  cv(0,4)=cv(4,0); cv(1,4)=cv(4,1); cv(2,4)=cv(4,2); cv(3,4)=cv(4,3);

  fSeed=kTRUE;
}
Commit	Line	Data
9c4c8863	1	/**************************************************************************
	2	* Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
	3	* *
	4	* Author: The ALICE Off-line Project. *
	5	* Contributors are mentioned in the code where appropriate. *
	6	* *
	7	* Permission to use, copy, modify and distribute this software and its *
	8	* documentation strictly for non-commercial purposes is hereby granted *
	9	* without fee, provided that the above copyright notice appears in all *
	10	* copies and that both the copyright notice and this permission notice *
	11	* appear in the supporting documentation. The authors make no claims *
	12	* about the suitability of this software for any purpose. It is *
	13	* provided "as is" without express or implied warranty. *
	14	**************************************************************************/
	15
	16	///////////////////////////////////////////////////////////////////////////////
	17	//
	18	// Kalman-Filter-like fit
	19	// to a straight-line crossing a set of arbitrarily oriented planes.
	20	// The resulting line is given by the equation:
	21	// (x-x0)/vx = (y-y0)/1 = (z-z0)/vz
	22	// Parameters of the fit are:
	23	// x0,y0,z0 (a point on the line) and
	24	// vx,1,vz (a vector collinear with the line)
	25	//
	26	// LIMITATION: The line must not be perpendicular to the Y axis.
	27	//
	28	// Origin: Iouri Belikov, CERN, Jouri.Belikov@cern.ch
	29	//
	30	//////////////////////////////////////////////////////////////////////////////
	31
	32	#include <TMatrixD.h>
	33	#include <TMatrixDSym.h>
	34
	35	//#include "AliLog.h"
	36	#include "AliTrackFitterKalman.h"
	37
	38	ClassImp(AliTrackFitterKalman)
	39
	40	const Double_t AliTrackFitterKalman::fgkMaxChi2=33.;
	41
	42	AliTrackFitterKalman::
	43	AliTrackFitterKalman(AliTrackPointArray *array, Bool_t owner):
	44	AliTrackFitter(array,owner),
57c8a1c2	45	fMaxChi2(fgkMaxChi2),
57c8a1c2	46	fSeed(kFALSE)
9c4c8863	47	{
	48	//
	49	// Constructor
	50	//
	51	}
	52
57c8a1c2	53	Bool_t AliTrackFitterKalman::Begin(Int_t first, Int_t last) {
	54	//
	55	// Make a seed out of the track points with the indices "first" and "last".
	56	// This is the "default" seed.
	57	//
	58	if (fSeed) return kTRUE;
	59	AliTrackPoint fp,lp;
	60	fPoints->GetPoint(fp,first); fPoints->GetPoint(lp,last);
	61	return MakeSeed(&fp,&lp);
	62	}
	63
	64
	65	Bool_t AliTrackFitterKalman::
9c4c8863	66	MakeSeed(const AliTrackPoint p0, const AliTrackPoint p1) {
	67	//
	68	// Make a seed out of the two track points
	69	//
	70	Double_t x0=p0->GetX(), y0=p0->GetY(), z0=p0->GetZ();
	71	Double_t dx=p1->GetX()-x0, dy=p1->GetY()-y0, dz=p1->GetZ()-z0;
	72	Double_t vx=dx/dy;
	73	Double_t vz=dz/dy;
	74	Double_t par[5]={x0,y0,z0,vx,vz};
	75
	76	const Float_t *cv0=p0->GetCov();
	77	const Float_t *cv1=p1->GetCov();
	78	Double_t rdx2=(cv0[0]+cv1[0])/dx/dx;
	79	Double_t rdy2=(cv0[3]+cv1[3])/dy/dy;
	80	Double_t rdz2=(cv0[5]+cv1[5])/dz/dz;
	81	Double_t svx2=vxvx(rdx2+rdy2);
	82	Double_t svz2=vzvz(rdz2+rdy2);
	83	Double_t cov[15]={
	84	cv0[0],
	85	cv0[1],cv0[3],
	86	cv0[2],cv0[4],cv0[5],
	87	0.,0.,0.,svx2,
	88	0.,0.,0.,0.,svz2
	89	};
	90
	91	SetSeed(par,cov);
57c8a1c2	92
57c8a1c2	93	return kTRUE;
9c4c8863	94	}
	95
	96	Bool_t AliTrackFitterKalman::AddPoint(const AliTrackPoint *p)
	97	{
	98	//
	99	// Add a point to the fit
	100	//
	101
	102	if (!Propagate(p)) return kFALSE;
	103	Double_t chi2=GetPredictedChi2(p);
	104	if (chi2>fMaxChi2) return kFALSE;
	105	if (!Update(p,chi2)) return kFALSE;
	106
	107	return kTRUE;
	108	}
	109
	110
	111	Double_t AliTrackFitterKalman::GetPredictedChi2(const AliTrackPoint *p) const {
	112	//
	113	// Calculate the predicted chi2 increment.
	114	//
	115
	116	Float_t txyz[3]={GetParam()[0],GetParam()[1],GetParam()[2]};
	117	TMatrixDSym &cv=*fCov;
	118	Float_t tcov[6]={
	119	cv(0,0),cv(1,0),cv(2,0),
	120	cv(1,1),cv(2,1),
	121	cv(2,2)
	122	};
	123	AliTrackPoint tp(txyz,tcov,p->GetVolumeID());
	124
	125	Double_t chi2=p->GetResidual(tp,kTRUE);
	126
	127	return chi2;
	128	}
	129
	130
	131	Bool_t AliTrackFitterKalman::Propagate(const AliTrackPoint *p) {
	132	//
	133	// Propagate the track towards the measured point "p"
	134	//
	135
	136	TMatrixDSym &cv=*fCov;
	137	Double_t s=p->GetY() - fParams[1];
	138	Double_t sig2=s*s/12.;
	139
	140	Double_t vx=fParams[3], vz=fParams[4];
	141	fParams[0] += s*vx;
	142	fParams[1] += s;
	143	fParams[2] += s*vz;
	144
	145	Double_t
	146	c00 = cv(0,0) + 2scv(3,0) + sscv(3,3) + vxvxsig2,
	147
	148	c10 = cv(1,0) + scv(1,3) + vxsig2, c11=cv(1,1) + sig2,
	149
	150	c20 = cv(2,0) + s(cv(4,0) + cv(2,3)) + sscv(4,3) + vxvz*sig2,
	151	c21 = cv(2,1) + scv(4,1) + vzsig2,
	152	c22 = cv(2,2) + 2scv(4,2) + sscv(4,4) + vzvzsig2,
	153
	154	c30 = cv(3,0) + s*cv(3,3), c31 = cv(3,1),
	155	c32 = cv(3,2) + s*cv(3,4), c33 = cv(3,3),
	156
	157	c40 = cv(4,0) + s*cv(4,3), c41 = cv(4,1),
158	c42 = cv(4,2) + s*cv(4,4), c43 = cv(4,3), c44 = cv(4,4);
159
160
161	cv(0,0)=c00; cv(0,1)=c10; cv(0,2)=c20; cv(0,3)=c30; cv(0,4)=c40;
162	cv(1,0)=c10; cv(1,1)=c11; cv(1,2)=c21; cv(1,3)=c31; cv(1,4)=c41;
163	cv(2,0)=c20; cv(2,1)=c21; cv(2,2)=c22; cv(2,3)=c32; cv(2,4)=c42;
164	cv(3,0)=c30; cv(3,1)=c31; cv(3,2)=c32; cv(3,3)=c33; cv(3,4)=c43;
165	cv(4,0)=c40; cv(4,1)=c41; cv(4,2)=c42; cv(4,3)=c43; cv(4,4)=c44;
166
167	return kTRUE;
168	}
169
170	Bool_t AliTrackFitterKalman::Update(const AliTrackPoint *p, Double_t chi2) {
171	//
172	// Update the track params using the measured point "p"
173	//
174
175	TMatrixDSym &c=*fCov;
176	const Float_t *cov=p->GetCov();
177
178	TMatrixDSym v(3);
179	v(0,0)=cov[0]+c(0,0); v(0,1)=cov[1]+c(0,1); v(0,2)=cov[2]+c(0,2);
180	v(1,0)=cov[1]+c(1,0); v(1,1)=cov[3]+c(1,1); v(1,2)=cov[4]+c(1,2);
181	v(2,0)=cov[2]+c(2,0); v(2,1)=cov[4]+c(2,1); v(2,2)=cov[5]+c(2,2);
182	v.Invert();
183
184	TMatrixD ch(5,3);
185	ch(0,0)=c(0,0); ch(0,1)=c(0,1); ch(0,2)=c(0,2);
186	ch(1,0)=c(1,0); ch(1,1)=c(1,1); ch(1,2)=c(1,2);
187	ch(2,0)=c(2,0); ch(2,1)=c(2,1); ch(2,2)=c(2,2);
188	ch(3,0)=c(3,0); ch(3,1)=c(3,1); ch(3,2)=c(3,2);
189	ch(4,0)=c(4,0); ch(4,1)=c(4,1); ch(4,2)=c(4,2);
190
191	TMatrixD k(ch,TMatrixD::kMult,v);
192
193	TMatrixD d(3,1);
194	d(0,0) = p->GetX() - fParams[0];
195	d(1,0) = p->GetY() - fParams[1];
196	d(2,0) = p->GetZ() - fParams[2];
197
198	TMatrixD x(k,TMatrixD::kMult,d);
199
200	fParams[0]+=x(0,0);
201	fParams[1]+=x(1,0);
202	fParams[2]+=x(2,0);
203	fParams[3]+=x(3,0);
204	fParams[4]+=x(4,0);
205
206	TMatrixD hc(3,5);
207	hc(0,0)=c(0,0);hc(0,1)=c(0,1);hc(0,2)=c(0,2);hc(0,3)=c(0,3);hc(0,4)=c(0,4);
208	hc(1,0)=c(1,0);hc(1,1)=c(1,1);hc(1,2)=c(1,2);hc(1,3)=c(1,3);hc(1,4)=c(1,4);
209	hc(2,0)=c(2,0);hc(2,1)=c(2,1);hc(2,2)=c(2,2);hc(2,3)=c(2,3);hc(2,4)=c(2,4);
210
211	TMatrixD s(k,TMatrixD::kMult,hc);
212
213	c(0,0)-=s(0,0);c(0,1)-=s(0,1);c(0,2)-=s(0,2);c(0,3)-=s(0,3);c(0,4)-=s(0,4);
214	c(1,0)-=s(1,0);c(1,1)-=s(1,1);c(1,2)-=s(1,2);c(1,3)-=s(1,3);c(1,4)-=s(1,4);
215	c(2,0)-=s(2,0);c(2,1)-=s(2,1);c(2,2)-=s(2,2);c(2,3)-=s(2,3);c(2,4)-=s(2,4);
216	c(3,0)-=s(3,0);c(3,1)-=s(3,1);c(3,2)-=s(3,2);c(3,3)-=s(3,3);c(3,4)-=s(3,4);
217	c(4,0)-=s(4,0);c(4,1)-=s(4,1);c(4,2)-=s(4,2);c(4,3)-=s(4,3);c(4,4)-=s(4,4);
218
219	fChi2 += chi2;
220	fNdf += 2;
221
222	return kTRUE;
223	}
224
225	//_____________________________________________________________________________
226	Bool_t AliTrackFitterKalman::GetPCA(const AliTrackPoint &p, AliTrackPoint &i) const
227	{
228	//
229	// Get the intersection point "i" between the track and the plane
230	// the point "p" belongs to.
231	//
232
233	TMatrixD t(3,1);
234	Double_t s=p.GetY() - fParams[1];
235	Double_t vx=fParams[3], vz=fParams[4];
236	t(0,0) = fParams[0] + s*vx;
237	t(1,0) = fParams[1] + s;
238	t(2,0) = fParams[2] + s*vz;
239
240	TMatrixDSym tC(3);
241	{
242	Double_t sig2=s*s/12.;
93a7cb17	243
	244	tC(0,0) = vxvxsig2;
	245	tC(1,0) = vx*sig2;
	246	tC(1,1) = sig2;
	247	tC(2,0) = vxvzsig2;
	248	tC(2,1) = vz*sig2;
	249	tC(2,2) = vzvzsig2;
9c4c8863	250
	251	tC(0,1) = tC(1,0); tC(0,2) = tC(2,0);
	252	tC(1,2) = tC(2,1);
	253	}
	254
	255	TMatrixD m(3,1);
	256	m(0,0)=p.GetX();
	257	m(1,0)=p.GetY();
	258	m(2,0)=p.GetZ();
	259
	260	TMatrixDSym mC(3);
	261	{
	262	const Float_t *cv=p.GetCov();
	263	mC(0,0)=cv[0]; mC(0,1)=cv[1]; mC(0,2)=cv[2];
	264	mC(1,0)=cv[1]; mC(1,1)=cv[3]; mC(1,2)=cv[4];
	265	mC(2,0)=cv[2]; mC(2,1)=cv[4]; mC(2,2)=cv[5];
	266	}
	267
	268	TMatrixDSym tmW(tC);
	269	tmW+=mC;
	270	tmW.Invert();
	271
11721007	272	TMatrixD mW(tC,TMatrixD::kMult,tmW);
11721007	273	TMatrixD tW(mC,TMatrixD::kMult,tmW);
9c4c8863	274
	275	TMatrixD mi(mW,TMatrixD::kMult,m);
	276	TMatrixD ti(tW,TMatrixD::kMult,t);
	277	ti+=mi;
	278
	279	TMatrixD iC(tC,TMatrixD::kMult,tmW);
	280	iC*=mC;
21404e08	281	Double_t sig2=1.; // Releasing the matrix by 1 cm along the track direction
	282	iC(0,0) += vxvxsig2;
	283	iC(0,1) += vx*sig2;
	284	iC(1,1) += sig2;
	285	iC(0,2) += vxvzsig2;
	286	iC(1,2) += vz*sig2;
	287	iC(2,2) += vzvzsig2;
	288
9c4c8863	289
	290	Float_t cov[6]={
	291	iC(0,0), iC(0,1), iC(0,2),
	292	iC(1,1), iC(1,2),
	293	iC(2,2)
	294	};
	295	i.SetXYZ(ti(0,0),ti(1,0),ti(2,0),cov);
	296	UShort_t id=p.GetVolumeID();
	297	i.SetVolumeID(id);
	298
	299	return kTRUE;
	300	}
	301
	302
	303	//_____________________________________________________________________________
	304	void
	305	AliTrackFitterKalman::SetSeed(const Double_t par[5], const Double_t cov[15]) {
	306	//
	307	// Set the initial approximation for the track parameters
	308	//
	309	for (Int_t i=0; i<5; i++) fParams[i]=par[i];
	310	fParams[5]=0.;
	311
	312	delete fCov;
	313	fCov=new TMatrixDSym(5);
	314	TMatrixDSym &cv=*fCov;
	315
	316	cv(0,0)=cov[0 ];
	317	cv(1,0)=cov[1 ]; cv(1,1)=cov[2 ];
	318	cv(2,0)=cov[3 ]; cv(2,1)=cov[4 ]; cv(2,2)=cov[5 ];
	319	cv(3,0)=cov[6 ]; cv(3,1)=cov[7 ]; cv(3,2)=cov[8 ]; cv(3,3)=cov[9 ];
	320	cv(4,0)=cov[10]; cv(4,1)=cov[11]; cv(4,2)=cov[12]; cv(4,3)=cov[13]; cv(4,4)=cov[14];
	321
	322	cv(0,1)=cv(1,0);
	323	cv(0,2)=cv(2,0); cv(1,2)=cv(2,1);
	324	cv(0,3)=cv(3,0); cv(1,3)=cv(3,1); cv(2,3)=cv(3,2);
	325	cv(0,4)=cv(4,0); cv(1,4)=cv(4,1); cv(2,4)=cv(4,2); cv(3,4)=cv(4,3);
	326
57c8a1c2	327	fSeed=kTRUE;
9c4c8863	328	}