[u/mrichter/AliRoot.git] / TPC / AliTPCkalmanTime.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.                  *
 **************************************************************************/

/*
  TPC Kalman filter implementation for time dependent variables:


The drift velocity and the gas gain are changing in time.
The drift velocity and gas gain is a function of many parameters, but not all of 
them are known. We assume that the most important parameters are pressure and temperature
and the influence of other parameters (gas composition, and electric field) are only 
slowly varying in time and can be expressed by smooth function $x_{off}(t)$:
\begin{equation}
x(t) = x_{off}(t)+k_N\frac{\Delta{P/T}}{P/T}	
\end{equation}
where x(t) is the parameter which we observe.
\begin{equation}
\begin{split}
x(t)=\frac{\Delta{G}}{G_0}	\\
x(t)=\frac{\Delta{v_d}}{v_{d0}}	
\end{split}
\end{equation}

Kalman filter parameters are following:
\begin{itemize}
\item State vector  ($x_{off}(t)$, $k_N$) at given time
\item Covariance matrix
\end{itemize}

Kalman filter implent following functions:
\begin{itemize}
\item Prediction - adding covariance element $\sigma_{xoff}$
\item Update state vector with new measurement vector ($x_t,\frac{\Delta{P/T}}{P/T}$)
\end{itemize}


*/

#include "AliTPCkalmanTime.h"
#include "TTreeStream.h"
#include "TRandom.h"


AliTPCkalmanTime::AliTPCkalmanTime():
  TNamed(),
  fState(0),
  fCovariance(0),
  fTime(0)
{
  //
  // Default constructor
  //
}

AliTPCkalmanTime::AliTPCkalmanTime(Double_t time, Double_t xoff, Double_t k, Double_t sigmaxoff, Double_t sigmak): 
  TNamed(),
  fState(0),
  fCovariance(0),
  fTime(0)
{
  //
  // Default constructor
  //
  Init(time,xoff,k,sigmaxoff,sigmak);
}


void AliTPCkalmanTime::Init(Double_t time, Double_t xoff, Double_t k, Double_t sigmaxoff, Double_t sigmak){
  //
  // Default constructor
  //
  fState = new TMatrixD(2,1);
  fCovariance = new TMatrixD(2,2);
  (*fState)(0,0)= xoff;  // offset
  (*fState)(1,0)= k;     // slope of the taylor
  fTime=time;
  (*fCovariance)(0,0)=sigmaxoff*sigmaxoff;
  (*fCovariance)(1,1)=sigmak*sigmak;
  (*fCovariance)(0,1)=0;
  (*fCovariance)(1,0)=0;
}


void AliTPCkalmanTime::Propagate(Double_t time, Double_t sigma, TTreeSRedirector *debug){
  //
  // Propagate the Kalman
  //
  if (!fCovariance) return;
  if (!fState) return;
  Double_t deltaT  =time-fTime;  //delta time - param2 is the current time
  Double_t sigmaT2 =(deltaT*deltaT)*sigma*sigma; 
  if (debug){
    (*debug)<<"matP"<<
      "time="<<time<<
      "fTime="<<fTime<<
      "sigmaT2="<<sigmaT2<<
      "cov.="<<fCovariance<<
      "\n";
  }
  (*fCovariance)(0,0)+=sigmaT2;
  fTime=time;  
}

void  AliTPCkalmanTime::Update(Double_t x, Double_t xerr, Double_t ptratio,TTreeSRedirector *debug){
  //
  //
  //
  static TMatrixD matHk(1,2);    // vector to mesurement
  static TMatrixD measR(1,1);    // measurement error 
  static TMatrixD matQk(2,2);    // prediction noise vector
  static TMatrixD vecZk(1,1);    // measurement
  //
  static TMatrixD vecYk(1,1);    // Innovation or measurement residual
  static TMatrixD matHkT(2,1);
  static TMatrixD matSk(1,1);    // Innovation (or residual) covariance
  static TMatrixD matKk(2,1);    // Optimal Kalman gain
  static TMatrixD mat1(2,2);     // update covariance matrix
  static TMatrixD covXk2(2,2);
  //
  //
  TMatrixD covXk(*fCovariance);    // X covariance 
  TMatrixD vecXk(*fState);         // X vector
  //
  vecZk(0,0) = x;                // current mesurement
  measR(0,0) = xerr*xerr;        // measurement error                      
  matHk(0,0)=1;  matHk(0,1)= ptratio; 
  vecYk = vecZk-matHk*vecXk;     // Innovation or measurement residual
  //
  //
  matHkT=matHk.T(); matHk.T();
  matSk = (matHk*(covXk*matHkT))+measR;    // Innovation (or residual) covariance
  matSk.Invert();
  matKk = (covXk*matHkT)*matSk;            //  Optimal Kalman gain
  vecXk += matKk*vecYk;                    //  updated vector 
  mat1(0,0)=1; mat1(1,1)=1; 
  mat1(1,0)=0; mat1(0,1)=0;
  covXk2= (mat1-(matKk*matHk));
  //
  //
  //
  covXk =  covXk2*covXk;    
  //
  if (debug){
    (*debug)<<"matU"<<
      "time="<<fTime<<
      "x="<<x<<
      "xerr="<<xerr<<
      "pt="<<ptratio<<
      "matHk.="<<&matHk<<
      "matHkT.="<<&matHkT<<
      "matSk.="<<&matSk<<
      "matKk.="<<&matKk<<
      "covXk2.="<<&covXk2<<
      "covXk.="<<&covXk<<
      "cov.="<<fCovariance<<
      "vecYk.="<<&vecYk<<
      "vecXk.="<<&vecXk<<
      "vec.="<<fState<<
      "\n";
  }
  (*fCovariance)=covXk;
  (*fState)=vecXk;
}

void AliTPCkalmanTime::TestMC(const char * fname){
  //
  // Test of the class
  /*
    Usage:
    AliTPCkalmanTime::TestMC("testKalman.root");
    TFile f("testKalman.root");
    
   */
  //
  TTreeSRedirector *pcstream = new TTreeSRedirector(fname);
  //
  const Double_t kp0=0;
  const Double_t kp1=1;
  const Int_t    klength=10*24*60*60;       // 10 days mesurement
  const Double_t ksigmap0=0.001/(24*60*60.); // 0.005 change in one day
  const Int_t    deltat=5*60;                // 5 minutes step
  const Double_t kmessError=0.0005;
  AliTPCkalmanTime testKalman;

  for (Int_t iter=0; iter<100;iter++){
    Double_t sp0      = kp0+gRandom->Gaus(0,0.01);    // variable to estimate -offset 
    Double_t sp1      = kp1+gRandom->Gaus(0,0.2);    // variable to estimate slope
    Double_t cp0      = sp0;    // variable to estimate 
    Double_t cp1      = sp1;
    
    //
    testKalman.Init(0,cp0+gRandom->Gaus(0,0.05),cp1+gRandom->Gaus(0,0.2),0.05,0.2);
    Double_t dptratio= 0;
    for (Int_t itime=0; itime<klength; itime+=deltat){
      dptratio+=gRandom->Gaus(0,0.0005);
      cp0     +=gRandom->Gaus(0,ksigmap0*deltat);
      //
      Double_t vdrift  = cp0+dptratio*cp1+gRandom->Gaus(0,kmessError);
      testKalman.Propagate(itime,ksigmap0,pcstream);
      Double_t fdrift = (*(testKalman.fState))(0,0) + dptratio*(*(testKalman.fState))(1,0);
      (*pcstream)<<"drift"<<
	"iter="<<iter<<
	"time="<<itime<<
	"vdrift="<<vdrift<<
	"fdrift="<<fdrift<<
	"pt="<<dptratio<<
	"sp0="<<sp0<<
	"sp1="<<sp1<<
	"cp0="<<cp0<<
	"cp1="<<cp1<<
	"vecXk.="<<testKalman.fState<<
	"covXk.="<<testKalman.fCovariance<<
	"\n";
      testKalman.Update(vdrift,kmessError,dptratio,pcstream);
    }
  }
  delete pcstream;
}
Commit	Line	Data
55f2eba2	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	TPC Kalman filter implementation for time dependent variables:
	18
	19
	20	The drift velocity and the gas gain are changing in time.
	21	The drift velocity and gas gain is a function of many parameters, but not all of
	22	them are known. We assume that the most important parameters are pressure and temperature
	23	and the influence of other parameters (gas composition, and electric field) are only
	24	slowly varying in time and can be expressed by smooth function $x_{off}(t)$:
	25	\begin{equation}
	26	x(t) = x_{off}(t)+k_N\frac{\Delta{P/T}}{P/T}
	27	\end{equation}
	28	where x(t) is the parameter which we observe.
	29	\begin{equation}
	30	\begin{split}
	31	x(t)=\frac{\Delta{G}}{G_0} \\
	32	x(t)=\frac{\Delta{v_d}}{v_{d0}}
	33	\end{split}
	34	\end{equation}
	35
	36	Kalman filter parameters are following:
	37	\begin{itemize}
	38	\item State vector ($x_{off}(t)$, $k_N$) at given time
	39	\item Covariance matrix
	40	\end{itemize}
	41
	42	Kalman filter implent following functions:
	43	\begin{itemize}
	44	\item Prediction - adding covariance element $\sigma_{xoff}$
	45	\item Update state vector with new measurement vector ($x_t,\frac{\Delta{P/T}}{P/T}$)
	46	\end{itemize}
	47
	48
	49	*/
	50
	51	#include "AliTPCkalmanTime.h"
26514b4e	52	#include "TTreeStream.h"
26514b4e	53	#include "TRandom.h"
55f2eba2	54
	55
	56	AliTPCkalmanTime::AliTPCkalmanTime():
	57	TNamed(),
	58	fState(0),
	59	fCovariance(0),
	60	fTime(0)
	61	{
	62	//
	63	// Default constructor
	64	//
	65	}
	66
	67	AliTPCkalmanTime::AliTPCkalmanTime(Double_t time, Double_t xoff, Double_t k, Double_t sigmaxoff, Double_t sigmak):
	68	TNamed(),
	69	fState(0),
	70	fCovariance(0),
	71	fTime(0)
	72	{
	73	//
	74	// Default constructor
	75	//
	76	Init(time,xoff,k,sigmaxoff,sigmak);
	77	}
	78
	79
	80	void AliTPCkalmanTime::Init(Double_t time, Double_t xoff, Double_t k, Double_t sigmaxoff, Double_t sigmak){
	81	//
	82	// Default constructor
	83	//
26514b4e	84	fState = new TMatrixD(2,1);
55f2eba2	85	fCovariance = new TMatrixD(2,2);
	86	(*fState)(0,0)= xoff; // offset
	87	(*fState)(1,0)= k; // slope of the taylor
	88	fTime=time;
	89	(fCovariance)(0,0)=sigmaxoffsigmaxoff;
	90	(fCovariance)(1,1)=sigmaksigmak;
	91	(*fCovariance)(0,1)=0;
	92	(*fCovariance)(1,0)=0;
	93	}
	94
	95
26514b4e	96	void AliTPCkalmanTime::Propagate(Double_t time, Double_t sigma, TTreeSRedirector *debug){
55f2eba2	97	//
	98	// Propagate the Kalman
	99	//
	100	if (!fCovariance) return;
	101	if (!fState) return;
26514b4e	102	Double_t deltaT =time-fTime; //delta time - param2 is the current time
55f2eba2	103	Double_t sigmaT2 =(deltaTdeltaT)sigma*sigma;
26514b4e	104	if (debug){
	105	(*debug)<<"matP"<<
	106	"time="<<time<<
	107	"fTime="<<fTime<<
	108	"sigmaT2="<<sigmaT2<<
	109	"cov.="<<fCovariance<<
	110	"\n";
	111	}
55f2eba2	112	(*fCovariance)(0,0)+=sigmaT2;
	113	fTime=time;
	114	}
	115
26514b4e	116	void AliTPCkalmanTime::Update(Double_t x, Double_t xerr, Double_t ptratio,TTreeSRedirector *debug){
55f2eba2	117	//
	118	//
	119	//
	120	static TMatrixD matHk(1,2); // vector to mesurement
	121	static TMatrixD measR(1,1); // measurement error
	122	static TMatrixD matQk(2,2); // prediction noise vector
	123	static TMatrixD vecZk(1,1); // measurement
	124	//
	125	static TMatrixD vecYk(1,1); // Innovation or measurement residual
	126	static TMatrixD matHkT(2,1);
	127	static TMatrixD matSk(1,1); // Innovation (or residual) covariance
	128	static TMatrixD matKk(2,1); // Optimal Kalman gain
	129	static TMatrixD mat1(2,2); // update covariance matrix
	130	static TMatrixD covXk2(2,2);
	131	//
	132	//
	133	TMatrixD covXk(*fCovariance); // X covariance
	134	TMatrixD vecXk(*fState); // X vector
	135	//
	136	vecZk(0,0) = x; // current mesurement
	137	measR(0,0) = xerr*xerr; // measurement error
	138	matHk(0,0)=1; matHk(0,1)= ptratio;
	139	vecYk = vecZk-matHk*vecXk; // Innovation or measurement residual
	140	//
	141	//
	142	matHkT=matHk.T(); matHk.T();
	143	matSk = (matHk(covXkmatHkT))+measR; // Innovation (or residual) covariance
	144	matSk.Invert();
	145	matKk = (covXkmatHkT)matSk; // Optimal Kalman gain
	146	vecXk += matKk*vecYk; // updated vector
26514b4e	147	mat1(0,0)=1; mat1(1,1)=1;
26514b4e	148	mat1(1,0)=0; mat1(0,1)=0;
55f2eba2	149	covXk2= (mat1-(matKk*matHk));
55f2eba2	150	//
26514b4e	151	//
	152	//
	153	covXk = covXk2*covXk;
	154	//
	155	if (debug){
	156	(*debug)<<"matU"<<
	157	"time="<<fTime<<
	158	"x="<<x<<
	159	"xerr="<<xerr<<
	160	"pt="<<ptratio<<
	161	"matHk.="<<&matHk<<
	162	"matHkT.="<<&matHkT<<
	163	"matSk.="<<&matSk<<
	164	"matKk.="<<&matKk<<
	165	"covXk2.="<<&covXk2<<
	166	"covXk.="<<&covXk<<
	167	"cov.="<<fCovariance<<
	168	"vecYk.="<<&vecYk<<
	169	"vecXk.="<<&vecXk<<
	170	"vec.="<<fState<<
	171	"\n";
	172	}
55f2eba2	173	(*fCovariance)=covXk;
	174	(*fState)=vecXk;
	175	}
26514b4e	176
	177	void AliTPCkalmanTime::TestMC(const char * fname){
	178	//
	179	// Test of the class
	180	/*
	181	Usage:
	182	AliTPCkalmanTime::TestMC("testKalman.root");
	183	TFile f("testKalman.root");
	184
	185	*/
	186	//
	187	TTreeSRedirector *pcstream = new TTreeSRedirector(fname);
	188	//
	189	const Double_t kp0=0;
	190	const Double_t kp1=1;
	191	const Int_t klength=102460*60; // 10 days mesurement
	192	const Double_t ksigmap0=0.001/(246060.); // 0.005 change in one day
d9382d87	193	const Int_t deltat=5*60; // 5 minutes step
26514b4e	194	const Double_t kmessError=0.0005;
	195	AliTPCkalmanTime testKalman;
	196
	197	for (Int_t iter=0; iter<100;iter++){
	198	Double_t sp0 = kp0+gRandom->Gaus(0,0.01); // variable to estimate -offset
	199	Double_t sp1 = kp1+gRandom->Gaus(0,0.2); // variable to estimate slope
	200	Double_t cp0 = sp0; // variable to estimate
	201	Double_t cp1 = sp1;
	202
	203	//
	204	testKalman.Init(0,cp0+gRandom->Gaus(0,0.05),cp1+gRandom->Gaus(0,0.2),0.05,0.2);
	205	Double_t dptratio= 0;
	206	for (Int_t itime=0; itime<klength; itime+=deltat){
	207	dptratio+=gRandom->Gaus(0,0.0005);
	208	cp0 +=gRandom->Gaus(0,ksigmap0*deltat);
	209	//
	210	Double_t vdrift = cp0+dptratio*cp1+gRandom->Gaus(0,kmessError);
	211	testKalman.Propagate(itime,ksigmap0,pcstream);
	212	Double_t fdrift = ((testKalman.fState))(0,0) + dptratio(*(testKalman.fState))(1,0);
	213	(*pcstream)<<"drift"<<
	214	"iter="<<iter<<
	215	"time="<<itime<<
	216	"vdrift="<<vdrift<<
	217	"fdrift="<<fdrift<<
	218	"pt="<<dptratio<<
	219	"sp0="<<sp0<<
	220	"sp1="<<sp1<<
	221	"cp0="<<cp0<<
	222	"cp1="<<cp1<<
	223	"vecXk.="<<testKalman.fState<<
	224	"covXk.="<<testKalman.fCovariance<<
	225	"\n";
	226	testKalman.Update(vdrift,kmessError,dptratio,pcstream);
	227	}
	228	}
	229	delete pcstream;
	230	}
	231