Index: external/TrackCovariance/AcceptanceClx.cc
===================================================================
--- external/TrackCovariance/AcceptanceClx.cc	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/AcceptanceClx.cc	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -1,5 +1,4 @@
 #include <TVector3.h>
 #include "AcceptanceClx.h"
-#include <iostream>
 //
 // Pt splitting routine
@@ -59,5 +58,5 @@
 	// Initializations
 	//
-	//std::cout << "Entered constructor of AccpeptanceClx" << std::endl;
+	//cout << "Entered constructor of AccpeptanceClx" << endl;
 	// Setup grid
 	// Start grid parameters
@@ -74,5 +73,5 @@
 	TVectorF Tha(NpThInp, ThInit);
 	Int_t NpTh = Tha.GetNrows();	// Nr. of starting theta points
-	//std::cout << "AcceptanceClv:: Pta and Tha arrays defined" << std::endl;
+	//cout << "AcceptanceClv:: Pta and Tha arrays defined" << endl;
 	//
 	// Grid splitting parameters
@@ -128,10 +127,7 @@
 				// Pt split
 				if (dPt > dPtMin && dAp > dAmin && Nsplits < MaxSplits) {
-					//std::cout << "Pt(" << ipt << ") = " << Pta(ipt) << ", dAp = " << dAp << std::endl;
 					NsplitCnt++;	// Total splits counter
 					Nsplits++;	// Increase splits/cycle
-					NpPt++;		// Increase #pt points
-					//std::cout << "New pt split: dAp = " << dAp << ", dPt = " << dPt <<
-					//	", Nsplits = " << Nsplits << ", NpPt = " << NpPt << std::endl;
+					NpPt++;		// Increase #pt points 
 					Float_t newPt = 0.5 * (Pta(ipt + 1) + Pta(ipt));
 					VecInsert(ipt, newPt, Pta);
@@ -156,10 +152,8 @@
 				// Theta split
 				if (dTh > dThMin && dAt > dAmin && Nsplits < MaxSplits) {
-					//std::cout << "Th(" << ith << ") = " << Tha(ith) << ", dAt = " << dAt << std::endl;
+					//cout << "Th(" << ith << ") = " << Tha(ith) << ", dAt = " << dAt << endl;
 					NsplitCnt++;	// Total splits counter
 					Nsplits++;	// Increase splits
-					NpTh++;		// Increase #pt points
-					//std::cout << "New th split: dAt = " << dAt << ", dTh = " << dTh <<
-					//	", Nsplits = " << Nsplits << ", NpTh = " << NpTh << std::endl;
+					NpTh++;		// Increase #pt points 
 					Float_t newTh = 0.5 * (Tha(ith + 1) + Tha(ith));
 					VecInsert(ith, newTh, Tha);
@@ -202,12 +196,8 @@
 	fThArray = Tha;	// Array of Theta nodes
 	//
-	std::cout << "AcceptanceClx:: Acceptance encoding with " << fNPtNodes
+		std::cout << "AcceptanceClx:: Acceptance encoding with " << fNPtNodes
 		<<" pt nodes and "<< fNThNodes <<" theta nodes"<< std::endl;
 	Int_t Nrows = fAcc.GetNrows();
 	Int_t Ncols = fAcc.GetNcols();
-	//std::cout<<"AcceptanceClx:: fAcc rows: "<<Nrows<<", cols: "<<Ncols<<std::endl;
-	//std::cout<<"AcceptanceClx:: Pta size: "<<Pta.GetNrows()<<", Tha size: "<<Tha.GetNrows()<<std::endl;;
-	//std::cout<<"AcceptanceClx:: pt nodes"<<std::endl; fPtArray.Print();
-	//std::cout<<"AcceptanceClx:: th nodes"<<std::endl; fThArray.Print();
 }
 
@@ -290,8 +280,4 @@
 	//
 	// Protect against values out of range
-	//std::cout << "AcceptanceClx::HitNumber: just in pt= " << pt << ", theta = " << theta << std::endl;
-	//std::cout << "AcceptanceClx::HitNumber: ptArr(0)= " << fPtArray(0) << ", thArr(0) = " << fThArray(0) << std::endl;
-	//std::cout << "AcceptanceClx::HitNumber: ptArr(E)= " << fPtArray(fNPtNodes - 1)
-	//	<< ", thArr(E) = " << fThArray(fNThNodes - 1) << std::endl;
 	Float_t eps = 1.0e-4;
 	Float_t pt0 = (Float_t)pt;
@@ -311,5 +297,5 @@
 	{
 		std::cout << "Search error: (ip, pt) = (" << ip << ", " << pt << "), pt0 = " << pt0 << std::endl;
-		std::cout << "Search error: pt nodes = " << fNPtNodes
+		std::cout << "Search error: pt nodes = " << fNPtNodes 
 			<< " , last value = " << fPtArray(fNPtNodes - 1) << std::endl;
 	}
@@ -343,2 +329,3 @@
 }
 //
+
Index: external/TrackCovariance/AcceptanceClx.h
===================================================================
--- external/TrackCovariance/AcceptanceClx.h	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/AcceptanceClx.h	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -23,12 +23,12 @@
 private:
 	TMatrixF fAcc;		// Acceptance matrix
-	Int_t fNPtNodes;	// Numer of Pt nodes
+	Int_t fNPtNodes;	// Numer of Pt nodes 
 	TVectorF fPtArray;	// Array of Pt nodes
-	Int_t fNThNodes;	// Numer of Theta nodes
+	Int_t fNThNodes;	// Numer of Theta nodes 
 	TVectorF fThArray;	// Array of Theta nodes		(Theta in degrees)
 	//
 	// Service routines
 	void VecInsert(Int_t i, Float_t x, TVectorF& Vec);
-	void SplitPt(Int_t i, TVectorF &AccPt);
+	void SplitPt(Int_t i, TVectorF &AccPt);	
 	void SplitTh(Int_t i, TVectorF &AccTh);
 public:
@@ -44,5 +44,5 @@
 	Int_t GetNrPt() { return fNPtNodes; }
 	Int_t GetNrTh() { return fNThNodes; }
-
+	
 	TVectorF* GetPtArray() { return &fPtArray; }
 	TVectorF* GetThArray() { return &fThArray; }
Index: external/TrackCovariance/ObsTrk.cc
===================================================================
--- external/TrackCovariance/ObsTrk.cc	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/ObsTrk.cc	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -6,4 +6,5 @@
 #include <TRandom.h>
 #include <iostream>
+#include "SolGeom.h"
 #include "SolGridCov.h"
 #include "ObsTrk.h"
@@ -11,13 +12,14 @@
 // Constructors
 //
-// x(3) track origin, p(3) track momentum at origin, Q charge, B magnetic field in Tesla
-ObsTrk::ObsTrk(TVector3 x, TVector3 p, Double_t Q, Double_t B, SolGridCov *GC)
+// x(3) track origin, p(3) track momentum at origin, Q charge, B ma gnetic field in Tesla
+ObsTrk::ObsTrk(TVector3 x, TVector3 p, Double_t Q, SolGridCov *GC, SolGeom *G)
 {
-	SetB(B);
+	fB = G->B();
+	SetB(fB);
+	fG = G;
 	fGC = GC;
 	fGenX = x;
 	fGenP = p;
 	fGenQ = Q;
-	fB = B;
 	fGenPar.ResizeTo(5);
 	fGenParMm.ResizeTo(5);
@@ -36,10 +38,6 @@
 	fGenParACTS = ParToACTS(fGenPar);
 	fGenParILC = ParToILC(fGenPar);
-	/*
-	cout << "ObsTrk::ObsTrk: fGenPar";
-	for (Int_t i = 0; i < 5; i++)cout << fGenPar(i) << ", ";
-	cout << endl;
-	*/
-	fObsPar = GenToObsPar(fGenPar, fGC);
+	//
+	fObsPar = GenToObsPar(fGenPar);
 	fObsParMm = ParToMm(fObsPar);
 	fObsParACTS = ParToACTS(fObsPar);
@@ -54,12 +52,13 @@
 //
 // x[3] track origin, p[3] track momentum at origin, Q charge, B magnetic field in Tesla
-ObsTrk::ObsTrk(Double_t *x, Double_t *p, Double_t Q, Double_t B, SolGridCov* GC)
+ObsTrk::ObsTrk(Double_t *x, Double_t *p, Double_t Q, SolGridCov* GC, SolGeom *G)
 {
-	SetB(B);
+	fB = G->B();
+	SetB(fB);
+	fG = G;
 	fGC = GC;
 	fGenX.SetXYZ(x[0],x[1],x[2]);
 	fGenP.SetXYZ(p[0],p[1],p[2]);
 	fGenQ = Q;
-	fB = B;
 	fGenPar.ResizeTo(5);
 	fGenParMm.ResizeTo(5);
@@ -77,10 +76,6 @@
 	fGenParACTS = ParToACTS(fGenPar);
 	fGenParILC = ParToILC(fGenPar);
-	/*
-	cout << "ObsTrk::ObsTrk: fGenPar";
-	for (Int_t i = 0; i < 5; i++)cout << fGenPar(i) << ", ";
-	cout << endl;
-	*/
-	fObsPar = GenToObsPar(fGenPar, fGC);
+	//
+	fObsPar = GenToObsPar(fGenPar);
 	fObsParACTS = ParToACTS(fObsPar);
 	fObsParILC = ParToILC(fObsPar);
@@ -113,45 +108,52 @@
 }
 //
-TVectorD ObsTrk::GenToObsPar(TVectorD gPar, SolGridCov *GC)
+TVectorD ObsTrk::GenToObsPar(TVectorD gPar)
 {
-	TVector3 p = ParToP(gPar);
-	Double_t pt = p.Pt();
-	Double_t tanTh = 1.0 / TMath::Abs(gPar(4));
-	Double_t angd = TMath::ATan(tanTh)*180. / TMath::Pi();
 	//
 	// Check ranges
-	Double_t minPt = GC->GetMinPt ();
-	//if (pt < minPt) cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is below grid range of " << minPt << endl;
-	Double_t maxPt = GC->GetMaxPt();
-	//if (pt > maxPt) cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is above grid range of " << maxPt << endl;
-	Double_t minAn = GC->GetMinAng();
-	//if (angd < minAn) cout << "Warning ObsTrk::GenToObsPar: angle " << angd 
-	//	<< " is below grid range of " << minAn << endl;
-	Double_t maxAn = GC->GetMaxAng();
-	//if (angd > maxAn) cout << "Warning ObsTrk::GenToObsPar: angle " << angd
-	//	<< " is above grid range of " << maxAn << endl;
+	Double_t minPt = fGC->GetMinPt();
+	//if (pt < minPt) std::cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is below grid range of " << minPt << std::endl;
+	Double_t maxPt = fGC->GetMaxPt();
+	//if (pt > maxPt) std::cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is above grid range of " << maxPt << std::endl;
+	Double_t minAn = fGC->GetMinAng();
+	//if (angd < minAn) std::cout << "Warning ObsTrk::GenToObsPar: angle " << angd 
+	//	<< " is below grid range of " << minAn << std::endl;
+	Double_t maxAn = fGC->GetMaxAng();
+	//if (angd > maxAn) std::cout << "Warning ObsTrk::GenToObsPar: angle " << angd
+	//	<< " is above grid range of " << maxAn << std::endl;
 	//
-	TMatrixDSym Cov = GC->GetCov(pt, angd);
+	TMatrixDSym Cov(5);
+	//
+	// Check if track origin is inside beampipe and betwen the first disks
+	//
+	Double_t Rin = fG->GetRmin();
+	Double_t ZinPos = fG->GetZminPos();
+	Double_t ZinNeg = fG->GetZminNeg();
+	Bool_t inside = TrkUtil::IsInside(fGenX, Rin, ZinNeg, ZinPos); // Check if in inner box
+	if (inside)
+	{
+		//std::cout<<"ObsTrk:: inside: x= "<<fGenX(0)<<", y= "<<fGenX(1)
+                //                        <<", z= "<<fGenX(2)<<std::endl;
+		// Observed track parameters
+		Double_t pt = fGenP.Pt();
+		Double_t angd = fGenP.Theta() * 180. / TMath::Pi();
+		Cov = fGC->GetCov(pt, angd);				// Track covariance
+	}
+	else
+	{
+		//std::cout<<"ObsTrk:: outside: x= "<<fGenX(0)<<", y= "<<fGenX(1)
+                //                         <<", z= "<<fGenX(2)<<std::endl;
+		SolTrack* trk = new SolTrack(fGenX, fGenP, fG);
+		Bool_t Res = kTRUE; Bool_t MS = kTRUE;
+		trk->CovCalc(Res, MS);					// Calculate covariance matrix
+		Cov = trk->Cov();					// Track covariance
+		delete trk;
+	}
+	//
 	fCov = Cov;
 	//
 	// Now do Choleski decomposition and random number extraction, with appropriate stabilization
 	//
-	TMatrixDSym CvN = Cov;
-	TMatrixDSym DCv(5); DCv.Zero();
-	TMatrixDSym DCvInv(5); DCvInv.Zero();
-	for (Int_t id = 0; id < 5; id++)
-	{
-		Double_t dVal = TMath::Sqrt(Cov(id, id));
-		DCv   (id, id) = dVal;
-		DCvInv(id, id) = 1.0 / dVal;
-	}
-	CvN.Similarity(DCvInv);			// Normalize diagonal to 1
-	TDecompChol Chl(CvN);
-	Bool_t OK = Chl.Decompose();		// Choleski decomposition of normalized matrix
-	TMatrixD U = Chl.GetU();			// Get Upper triangular matrix
-	TMatrixD Ut(TMatrixD::kTransposed, U); // Transposed of U (lower triangular)
-	TVectorD r(5);
-	for (Int_t i = 0; i < 5; i++)r(i) = gRandom->Gaus(0.0, 1.0);		// Array of normal random numbers
-	TVectorD oPar = gPar + DCv*(Ut*r);	// Observed parameter vector
+	TVectorD oPar = TrkUtil::CovSmear(gPar, Cov);
 	//
 	return oPar;
Index: external/TrackCovariance/ObsTrk.h
===================================================================
--- external/TrackCovariance/ObsTrk.h	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/ObsTrk.h	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -6,4 +6,5 @@
 #include <TMatrixDSym.h>
 #include <TDecompChol.h>
+#include "SolGeom.h"
 #include "TrkUtil.h"
 #include "SolGridCov.h"
@@ -23,5 +24,6 @@
 private:	
 	Double_t fB;					// Solenoid magnetic field
-	SolGridCov *fGC;				// Covariance matrix grid
+	SolGridCov* fGC;				// Covariance matrix grid
+	SolGeom*    fG;					// Tracker geometry
 	Double_t fGenQ;					// Generated track charge
 	Double_t fObsQ;					// Observed  track charge
@@ -47,5 +49,5 @@
 	// Service routines
 	//
-	TVectorD GenToObsPar(TVectorD gPar, SolGridCov* GC);
+	TVectorD GenToObsPar(TVectorD gPar);
 	//
 public:
@@ -53,6 +55,6 @@
 	// Constructors
 	// x(3) track origin, p(3) track momentum at origin, Q charge, B magnetic field in Tesla
-	ObsTrk(TVector3 x, TVector3 p, Double_t Q, Double_t B, SolGridCov *GC);	// Initialize and generate smeared 
-	ObsTrk(Double_t *x, Double_t *p, Double_t Q, Double_t B, SolGridCov* GC);	// Initialize and generate smeared track
+	ObsTrk(TVector3 x, TVector3 p, Double_t Q, SolGridCov *GC, SolGeom *G);	// Initialize and generate smeared 
+	ObsTrk(Double_t *x, Double_t *p, Double_t Q, SolGridCov* GC, SolGeom *G);	// Initialize and generate smeared track
 	// Destructor
 	~ObsTrk();
Index: external/TrackCovariance/SolGeom.cc
===================================================================
--- external/TrackCovariance/SolGeom.cc	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/SolGeom.cc	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -87,4 +87,8 @@
     if (flLay == 1) fNm++;
   }
+//
+// Define inner box for fast tracking
+//
+    SetMinBoundaries();
 }
 
@@ -104,2 +108,22 @@
   delete[] fflLay;
 }
+
+//
+// Get inner boundaries of cylindrical box for fast simulation
+//
+void SolGeom::SetMinBoundaries()
+{
+	// Get radius of first barrel layer
+	fRmin = 1000000.0;
+	fZminPos = 1000000.0;
+	fZminNeg = -1000000.0;
+	for (Int_t i = 0; i < fNlay; i++){
+		if (ftyLay[i] == 1) {				// Cylinders
+			if (frPos[i] < fRmin) fRmin = frPos[i];
+		}
+		if (ftyLay[i] == 2) {				// Disks
+			if (frPos[i] > 0.0 && frPos[i] < fZminPos) fZminPos = frPos[i];	// Positive direction
+			if (frPos[i] < 0.0 && frPos[i] > fZminNeg) fZminNeg = frPos[i];	// Negative direction
+		}
+	}
+}
Index: external/TrackCovariance/SolGeom.h
===================================================================
--- external/TrackCovariance/SolGeom.h	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/SolGeom.h	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -6,4 +6,7 @@
 
 // Class to create geometry for solenoid geometry
+// Simplified implementations with cylindrical and disk layers
+//
+// Author: F. Bedeschi, INFN - Pisa
 
 class SolGeom{
@@ -37,4 +40,10 @@
   Double_t *fsgLayL; // Resolution Lower side (meters) - 0 = no measurement
   Bool_t   *fflLay;  // measurement flag = T, scattering only = F
+//
+//
+  Double_t fRmin;	// Radius of first barrel layer
+  Double_t fZminPos;	// Z of first disk in positive direction
+  Double_t fZminNeg;	// Z of first disk in negative direction
+  void SetMinBoundaries();	// define inner box for fast simulation
 
 public:
@@ -61,4 +70,10 @@
   Double_t lSgL(Int_t nlayer)      { return fsgLayL[nlayer]; }
   Bool_t   isMeasure(Int_t nlayer) { return fflLay[nlayer]; }
+  //
+  // Define cylindrical box to use for fast simulation
+  //
+  Double_t GetRmin() { return fRmin; }
+  Double_t GetZminPos() { return fZminPos; }
+  Double_t GetZminNeg() { return fZminNeg; }
 };
 
Index: external/TrackCovariance/SolGridCov.cc
===================================================================
--- external/TrackCovariance/SolGridCov.cc	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/SolGridCov.cc	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -103,6 +103,29 @@
 	return Accept;
 }
-
-
+//
+// Detailed acceptance check
+//
+Bool_t SolGridCov::IsAccepted(TVector3 x, TVector3 p, SolGeom* G)
+{
+	Bool_t Accept = kFALSE;
+	//
+	// Check if track origin is inside beampipe and betwen the first disks
+	//
+	Double_t Rin = G->GetRmin();
+	Double_t ZinPos = G->GetZminPos();
+	Double_t ZinNeg = G->GetZminNeg();
+	Bool_t inside = TrkUtil::IsInside(x, Rin, ZinNeg, ZinPos); // Check if in inner box
+	if (inside) Accept = IsAccepted(p);
+	else
+	{
+		SolTrack* trk = new SolTrack(x, p, G);
+		if (trk->nmHit() >= fNminHits)Accept = kTRUE;
+		delete trk;
+	}
+	//
+	return Accept;
+}
+
+//
 // Find bin in grid
 Int_t SolGridCov::GetMinIndex(Double_t xval, Int_t N, TVectorD x)
@@ -193,5 +216,5 @@
   if (!Chl.Decompose())
   {
-    cout << "SolGridCov::GetCov: Interpolated matrix not positive definite. Recovering ...." << endl;
+    std::cout << "SolGridCov::GetCov: Interpolated matrix not positive definite. Recovering ...." << std::endl;
     TMatrixDSym rCv = MakePosDef(CvN); CvN = rCv;
     TMatrixDSym DCv(5); DCv.Zero();
Index: external/TrackCovariance/SolGridCov.h
===================================================================
--- external/TrackCovariance/SolGridCov.h	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/SolGridCov.h	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -18,6 +18,6 @@
   TVectorD fAnga;    // Array of angle points in degrees
   TMatrixDSym *fCov; // Pointers to grid of covariance matrices
-	AcceptanceClx *fAcc;				// Pointer to acceptance class
-	Int_t fNminHits;					// Minimum number of hits to accept track
+  AcceptanceClx *fAcc;		// Pointer to acceptance class
+  Int_t fNminHits;		// Minimum number of hits to accept track
   // Service routines
   Int_t GetMinIndex(Double_t xval, Int_t N, TVectorD x); // Find bin
@@ -40,7 +40,8 @@
 	void SetMinHits(Int_t MinHits) { fNminHits = MinHits; };	// Set minimum number of hits to accept (default = 6)
 	Int_t GetMinHits() { return fNminHits; };
-	Bool_t IsAccepted(Double_t pt, Double_t Theta);		// From pt (GeV) and theta (degrees)
-	Bool_t IsAccepted(Double_t *p);						// From momentum components (GeV)
-	Bool_t IsAccepted(TVector3 p);						// As above in Vector3 format
+	Bool_t IsAccepted(Double_t pt, Double_t Theta);			// From pt (GeV) and theta (degrees)
+	Bool_t IsAccepted(Double_t *p);					// From momentum components (GeV)
+	Bool_t IsAccepted(TVector3 p);					// As above in Vector3 format
+	Bool_t IsAccepted(TVector3 x, TVector3 p, SolGeom *G);		// As above checking track origin
 
 };
Index: external/TrackCovariance/SolTrack.cc
===================================================================
--- external/TrackCovariance/SolTrack.cc	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/SolTrack.cc	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -1,4 +1,5 @@
-#include <iostream>
 
+#include "SolGeom.h"
+#include "SolTrack.h"
 #include <TString.h>
 #include <TMath.h>
@@ -7,66 +8,65 @@
 #include <TDecompChol.h>
 #include <TMatrixDSymEigen.h>
-
-#include "SolGeom.h"
-#include "SolTrack.h"
-
-using namespace std;
-
+#include <TGraph.h>
+#include <iostream>
+//
+// Constructors
 SolTrack::SolTrack(Double_t *x, Double_t *p, SolGeom *G)
 {
-  fG = G;
-  // Store momentum
-  fp[0] = p[0]; fp[1] = p[1]; fp[2] = p[2];
-  Double_t px = p[0]; Double_t py = p[1]; Double_t pz = p[2];
-  fx[0] = x[0]; fx[1] = x[1]; fx[2] = x[2];
-  Double_t xx = x[0]; Double_t yy = x[1]; Double_t zz = x[2];
-  // Store parameters
-  Double_t pt = TMath::Sqrt(px*px + py*py);
-  Double_t Charge = 1.0;            // Don't worry about charge for now
-  Double_t a = -Charge*G->B()*0.2998;     // Normalized speed of light
-  Double_t C = a / (2 * pt);          // pt in GeV, B in Tesla, C in meters
-  Double_t r2 = xx*xx + yy*yy;
-  Double_t cross = xx*py - yy*px;
-  Double_t T = TMath::Sqrt(pt*pt - 2 * a*cross + a*a*r2);
-  Double_t phi0 = TMath::ATan2((py - a*xx) / T, (px + a*yy) / T);
-  Double_t D;
-  if (pt < 10.0) D = (T - pt) / a;
-  else D = (-2 * cross + a*r2) / (T + pt);
-  Double_t B = C*TMath::Sqrt((r2 - D*D) / (1 + 2 * C*D));
-  Double_t st = TMath::ASin(B) / C;
-  Double_t ct = pz / pt;
-  Double_t z0 = zz - ct*st;
-  fpar[0] = D;
-  fpar[1] = phi0;
-  fpar[2] = C;
-  fpar[3] = z0;
-  fpar[4] = ct;
-  // Init covariances
-  fCov.ResizeTo(5, 5);
+	// Set B field
+	fG = G;					// Store geometry
+	Double_t B = G->B();
+	SetB(B);
+	// Store momentum and position
+	fp[0] = p[0]; fp[1] = p[1]; fp[2] = p[2];
+	fx[0] = x[0]; fx[1] = x[1]; fx[2] = x[2];
+	// Get generated parameters
+	TVector3 xv(fx);
+	TVector3 pv(fp);
+	Double_t Charge = 1.0;						// Don't worry about charge for now
+	TVectorD gPar = XPtoPar(xv, pv, Charge);
+	// Store parameters
+	fpar[0] = gPar(0);
+	fpar[1] = gPar(1);
+	fpar[2] = gPar(2);
+	fpar[3] = gPar(3);
+	fpar[4] = gPar(4);
+	//cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl;
+	//
+	// Init covariances
+	//
+	fCov.ResizeTo(5, 5);
 }
 SolTrack::SolTrack(TVector3 x, TVector3 p, SolGeom* G)
 {
-	fG = G;
+	// set B field
+	fG = G;					// Store geometry
+	Double_t B = G->B();
+	SetB(B);
 	// Store momentum
 	fp[0] = p(0); fp[1] = p(1); fp[2] = p(2);
-	Double_t px = p(0); Double_t py = p(1); Double_t pz = p(2);
 	fx[0] = x(0); fx[1] = x(1); fx[2] = x(2);
-	Double_t xx = x(0); Double_t yy = x(1); Double_t zz = x(2);
+	// Get generated parameters
+	Double_t Charge = 1.0;						// Don't worry about charge for now
+	TVectorD gPar = XPtoPar(x, p, Charge);
 	// Store parameters
-	Double_t pt = TMath::Sqrt(px * px + py * py);
-	Double_t Charge = 1.0;						// Don't worry about charge for now
-	Double_t a = -Charge * G->B() * 0.2998;			// Normalized speed of light
-	Double_t C = a / (2 * pt);					// pt in GeV, B in Tesla, C in meters
-	Double_t r2 = xx * xx + yy * yy;
-	Double_t cross = xx * py - yy * px;
-	Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2);
-	Double_t phi0 = TMath::ATan2((py - a * xx) / T, (px + a * yy) / T);
-	Double_t D;
-	if (pt < 10.0) D = (T - pt) / a;
-	else D = (-2 * cross + a * r2) / (T + pt);
-	Double_t B = C * TMath::Sqrt((r2 - D * D) / (1 + 2 * C * D));
-	Double_t st = TMath::ASin(B) / C;
-	Double_t ct = pz / pt;
-	Double_t z0 = zz - ct * st;
+	fpar[0] = gPar(0);
+	fpar[1] = gPar(1);
+	fpar[2] = gPar(2);
+	fpar[3] = gPar(3);
+	fpar[4] = gPar(4);
+	//cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl;
+	//
+	// Init covariances
+	//
+	fCov.ResizeTo(5, 5);
+}
+//
+SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G)
+{
+	fG = G;
+	Double_t B = G->B();
+	SetB(B);
+	// Store parameters
 	fpar[0] = D;
 	fpar[1] = phi0;
@@ -74,100 +74,91 @@
 	fpar[3] = z0;
 	fpar[4] = ct;
-	//cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl;
+	// Store momentum
+	Double_t pt = B * TrkUtil::cSpeed() / TMath::Abs(2 * C);
+	Double_t px = pt*TMath::Cos(phi0);
+	Double_t py = pt*TMath::Sin(phi0);
+	Double_t pz = pt*ct;
+	//
+	fp[0] = px; fp[1] = py; fp[2] = pz;
+	fx[0] = -D*TMath::Sin(phi0); fx[1] = D*TMath::Cos(phi0);  fx[2] = z0;
 	//
 	// Init covariances
 	//
 	fCov.ResizeTo(5, 5);
-}
-
-SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G)
-{
-  fG = G;
-  // Store parameters
-  fpar[0] = D;
-  fpar[1] = phi0;
-  fpar[2] = C;
-  fpar[3] = z0;
-  fpar[4] = ct;
-  // Store momentum
-  Double_t pt = G->B()*0.2998 / TMath::Abs(2 * C);
-  Double_t px = pt*TMath::Cos(phi0);
-  Double_t py = pt*TMath::Sin(phi0);
-  Double_t pz = pt*ct;
-
-  fp[0] = px; fp[1] = py; fp[2] = pz;
-  fx[0] = -D*TMath::Sin(phi0); fx[1] = D*TMath::Cos(phi0);  fx[2] = z0;
-  // Init covariances
-  fCov.ResizeTo(5, 5);
 }
 // Destructor
 SolTrack::~SolTrack()
 {
-	delete[] & fp;
-	delete[] & fpar;
-  fCov.Clear();
-}
+	fCov.Clear();
+}
+//
 // Calculate intersection with given layer
 Bool_t SolTrack::HitLayer(Int_t il, Double_t &R, Double_t &phi, Double_t &zz)
 {
-  Double_t Di = D();
-  Double_t phi0i = phi0();
-  Double_t Ci = C();
-  Double_t z0i = z0();
-  Double_t cti = ct();
-
-  R = 0; phi = 0; zz = 0;
-
-  Bool_t val = kFALSE;
-  if (fG->lTyp(il) == 1)      // Cylinder: layer at constant R
-  {
-    R = fG->lPos(il);
-    Double_t argph = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
-    if (TMath::Abs(argph) < 1.0)
-    {
-      Double_t argz = Ci*TMath::Sqrt((R*R - Di*Di) / (1 + 2 * Ci*Di));
-      if (TMath::Abs(argz) < 1.0)
-      {
-        zz = z0i + cti*TMath::ASin(argz) / Ci;
-        if (zz > fG->lxMin(il) && zz < fG->lxMax(il))
-        {
-          phi = phi0i + TMath::ASin(argph);
-          val = kTRUE;
-        }
-      }
-    }
-  }
-  else if (fG->lTyp(il) == 2)   // disk: layer at constant z
-  {
-    zz = fG->lPos(il);
-    Double_t arg = Ci*(zz - z0i) / cti;
-    if (TMath::Abs(arg) < 1.0 && (zz - z0i) / cti > 0)
-    {
-      R = TMath::Sqrt(Di*Di + (1. + 2.*Ci*Di)*pow(TMath::Sin(arg), 2) / (Ci*Ci));
-      if (R > fG->lxMin(il) && R < fG->lxMax(il))
-      {
-        Double_t arg1 = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
-        if (TMath::Abs(arg1) < 1.0)
-        {
-          phi = phi0i + TMath::ASin(arg1);
-          val = kTRUE;
-        }
-      }
-    }
-  }
-  //
-  return val;
-}
+	Double_t Di = D();
+	Double_t phi0i = phi0();
+	Double_t Ci = C();
+	Double_t z0i = z0();
+	Double_t cti = ct();
+	//
+	R = 0; phi = 0; zz = 0;
+	Bool_t val = kFALSE;
+	Double_t Rmin = TMath::Sqrt(fx[0] * fx[0] + fx[1] * fx[1]); // Smallest track radius
+	if (Rmin < TMath::Abs(Di)) return val;
+	//
+	Double_t ArgzMin = Ci * TMath::Sqrt((Rmin * Rmin - Di * Di) / (1 + 2 * Ci * Di));
+	Double_t stMin = TMath::ASin(ArgzMin) / Ci;					// Arc length at track origin
+	//
+	if (fG->lTyp(il) == 1)			// Cylinder: layer at constant R
+	{
+		R = fG->lPos(il);
+		Double_t argph = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
+		if (TMath::Abs(argph) < 1.0 && R > Rmin)
+		{
+			Double_t argz = Ci*TMath::Sqrt((R*R - Di*Di) / (1 + 2 * Ci*Di));
+			if (TMath::Abs(argz) < 1.0)
+			{
+				zz = z0i + cti*TMath::ASin(argz) / Ci;
+				if (zz > fG->lxMin(il) && zz < fG->lxMax(il))
+				{
+					phi = phi0i + TMath::ASin(argph);
+					val = kTRUE;
+				}
+			}
+		}
+	}
+	else if (fG->lTyp(il) == 2)		// disk: layer at constant z
+	{
+		zz = fG->lPos(il);
+		Double_t st = (zz - z0i) / cti;
+		if (TMath::Abs(Ci * st) < 1.0 && st > stMin)
+		{
+			R = TMath::Sqrt(Di * Di + (1. + 2. * Ci * Di) * pow(TMath::Sin(Ci * st), 2) / (Ci * Ci));
+			if (R > fG->lxMin(il) && R < fG->lxMax(il))
+			{
+				Double_t arg1 = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
+				if (TMath::Abs(arg1) < 1.0)
+				{
+					phi = phi0i + TMath::ASin(arg1);
+					val = kTRUE;
+				}
+			}
+		}
+	}
+	//
+	return val;
+}
+//
 // # of layers hit
 Int_t SolTrack::nHit()
 {
-  Int_t kh = 0;
-  Double_t R; Double_t phi; Double_t zz;
-  for (Int_t i = 0; i < fG->Nl(); i++)
-  if (HitLayer(i, R, phi, zz))kh++;
-
-  return kh;
-}
-
+	Int_t kh = 0;
+	Double_t R; Double_t phi; Double_t zz;
+	for (Int_t i = 0; i < fG->Nl(); i++)
+	if (HitLayer(i, R, phi, zz))kh++;
+	//
+	return kh;
+}
+//
 // # of measurement layers hit
 Int_t SolTrack::nmHit()
@@ -181,41 +172,40 @@
 }
 //
-
-
 // List of layers hit with intersections
 Int_t SolTrack::HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh)
 {
-  // Return lists of hits associated to a track including all scattering layers.
-  // Return value is the total number of measurement hits
-  // kmh = total number of measurement layers hit for given type
-  // ihh = pointer to layer number
-  // rhh = radius of hit
-  // zhh = z of hit
-
-  // ***** NB: double layers with stereo on lower layer not included
-
-  Int_t kh = 0; // Number of layers hit
-  Int_t kmh = 0; // Number of measurement layers of given type
-  for (Int_t i = 0; i < fG->Nl(); i++)
-  {
-    Double_t R; Double_t phi; Double_t zz;
-    if (HitLayer(i, R, phi, zz)) // Only barrel type layers
-    {
-      zhh[kh] = zz;
-      rhh[kh] = R;
-      ihh[kh] = i;
-      if (fG->isMeasure(i))kmh++;
-      kh++;
-    }
-  }
-
-  return kmh;
-}
-
+	//
+	// Return lists of hits associated to a track including all scattering layers. 
+	// Return value is the total number of measurement hits
+	// kmh = total number of measurement layers hit for given type
+	// ihh = pointer to layer number
+	// rhh = radius of hit
+	// zhh = z of hit
+	//
+	// ***** NB: double layers with stereo on lower layer not included
+	//
+	Int_t kh = 0;	// Number of layers hit
+	Int_t kmh = 0;  // Number of measurement layers of given type
+	for (Int_t i = 0; i < fG->Nl(); i++)
+	{
+		Double_t R; Double_t phi; Double_t zz;
+		if (HitLayer(i, R, phi, zz)) 
+		{
+			zhh[kh] = zz;
+			rhh[kh] = R;
+			ihh[kh] = i;
+			if (fG->isMeasure(i))kmh++;
+			kh++;
+		}
+	}
+	//
+	return kmh;
+}
+//
 // List of XYZ measurements without any error
 Int_t SolTrack::HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh)
 {
 	//
-	// Return lists of hits associated to a track for all measurement layers.
+	// Return lists of hits associated to a track for all measurement layers. 
 	// Return value is the total number of measurement hits
 	// kmh = total number of measurement layers hit for given type
@@ -244,4 +234,49 @@
 }
 //
+// Track plot
+//
+TGraph *SolTrack::TrkPlot()
+{
+	//
+	// Fill list of layers hit
+	//
+	Int_t Nhit = nHit();					// Total number of layers hit
+	//cout << "Nhit = " << Nhit << endl;
+	Double_t *zh = new Double_t[Nhit];		// z of hit
+	Double_t *rh = new Double_t[Nhit];		// r of hit
+	Int_t    *ih = new Int_t   [Nhit];		// true index of layer
+	Int_t kmh;								// Number of measurement layers hit
+	//
+	kmh = HitList(ih, rh, zh);				// hit layer list
+	//for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;
+	Double_t *dh = new Double_t[Nhit];		// Hit distance from origin
+	for(Int_t i=0; i<Nhit; i++)dh[i] = TMath::ASin(C() * TMath::Sqrt((rh[i] * rh[i] - D() * D()) / (1. + 2 * C() * D()))) / C();	// Arc length traveled;
+	//
+	Int_t *hord = new Int_t[Nhit];
+	TMath::Sort(Nhit, dh, hord, kFALSE);		// Order by increasing phase
+	Double_t *z = new Double_t[Nhit];		// z of ordered hit
+	Double_t *r = new Double_t[Nhit];		// r of ordered hit
+	for (Int_t i = 0; i < Nhit; i++)
+	{
+		z[i] = zh[hord[i]];
+		r[i] = rh[hord[i]];
+	}
+	//cout << "After ordering" << endl;
+	//for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;
+	TGraph *gr = new TGraph(Nhit, z, r);	// graph intersection with layers
+	gr->SetMarkerStyle(kCircle);
+	gr->SetMarkerColor(kMagenta);
+	gr->SetMarkerSize(1);
+	gr->SetLineColor(kMagenta);
+	//
+	// clean up
+	//
+	delete[] zh;
+	delete[] rh;
+	delete[] ih;
+	delete[] hord;
+	return gr;
+}
+//
 // Covariance matrix estimation
 //
@@ -250,5 +285,5 @@
 	//
 	//
-	// Input flags:
+	// Input flags: 
 	//				Res = .TRUE. turn on resolution effects/Use standard resolutions
 	//					  .FALSE. set all resolutions to 0
@@ -265,23 +300,24 @@
 	Int_t ntry = 0;
 	Int_t ntrymax = 0;
-	Int_t Nhit = nHit();					// Total number of layers hit
+	Int_t Nhit = nHit();				// Total number of layers hit
 	Double_t *zhh = new Double_t[Nhit];		// z of hit
 	Double_t *rhh = new Double_t[Nhit];		// r of hit
 	Double_t *dhh = new Double_t[Nhit];		// distance of hit from origin
 	Int_t    *ihh = new Int_t[Nhit];		// true index of layer
-	Int_t kmh;								// Number of measurement layers hit
+	Int_t kmh;					// Number of measurement layers hit
 	//
 	kmh = HitList(ihh, rhh, zhh);			// hit layer list
-	Int_t mTot = 0;							// Total number of measurements
+	Int_t mTot = 0;					// Total number of measurements
 	for (Int_t i = 0; i < Nhit; i++)
 	{
-		dhh[i] = TMath::Sqrt(rhh[i] * rhh[i] + zhh[i] * zhh[i]);
+		Double_t rr = rhh[i];
+		dhh[i] = TMath::ASin(C() * TMath::Sqrt((rr * rr - D() * D()) / (1. + 2 * C() * D()))) / C();	// Arc length traveled
 		if (fG->isMeasure(ihh[i])) mTot += fG->lND(ihh[i]);	// Count number of measurements
 	}
 	//
-	// Order hit list by increasing distance from origin
+	// Order hit list by increasing arc length
 	//
 	Int_t    *hord = new Int_t[Nhit];		// hit order by increasing distance from origin
-	TMath::Sort(Nhit, dhh, hord, kFALSE);	// Order by increasing distance from origin
+	TMath::Sort(Nhit, dhh, hord, kFALSE);		// Order by increasing distance from origin
 	Double_t *zh = new Double_t[Nhit];		// d-ordered z of hit
 	Double_t *rh = new Double_t[Nhit];		// d-ordered r of hit
@@ -297,5 +333,5 @@
 	// Store interdistances and multiple scattering angles
 	//
-	Double_t sn2t = 1.0 / (1 + ct()*ct());			//sin^2 theta of track
+	Double_t sn2t = 1.0 / (1.0 + ct()*ct());			//sin^2 theta of track
 	Double_t cs2t = 1.0 - sn2t;						//cos^2 theta
 	Double_t snt = TMath::Sqrt(sn2t);				// sin theta
@@ -306,19 +342,25 @@
 	//
 	TMatrixDSym dik(Nhit);	dik.Zero();		// Distances between layers
-	Double_t *thms = new Double_t[Nhit];	// Scattering angles/plane
-	Double_t *cs = new Double_t[Nhit];		// Cosine of angle with layer normal
+	Double_t *thms = new Double_t[Nhit];		// Scattering angles/plane
+	Double_t* cs = new Double_t[Nhit];		// Cosine of angle with normal in transverse plane
+	//
 	for (Int_t ii = 0; ii < Nhit; ii++)		// Hit layer loop
 	{
 		Int_t i = ih[ii];					// Get true layer number
+		Int_t il = hord[ii];					// Unordered layer
 		Double_t B = C()*TMath::Sqrt((rh[ii] * rh[ii] - D()*D()) / (1 + 2 * C()*D()));
+		//
 		Double_t pxi = px0*(1-2*B*B)-2*py0*B*TMath::Sqrt(1-B*B);		// Momentum at scattering layer
 		Double_t pyi = py0*(1-2*B*B)+2*px0*B*TMath::Sqrt(1-B*B);
 		Double_t pzi = pz0;
 		Double_t ArgRp = (rh[ii]*C() + (1 + C() * D())*D() / rh[ii]) / (1 + 2 * C()*D());
+		//
 		Double_t phi = phi0() + TMath::ASin(ArgRp);
 		Double_t nx = TMath::Cos(phi);		// Barrel layer normal
 		Double_t ny = TMath::Sin(phi);
 		Double_t nz = 0.0;
-		if (fG->lTyp(i) == 2)								// this is Z layer
+		cs[ii] = TMath::Abs((pxi * nx + pyi * ny) / pt());
+		//
+		if (fG->lTyp(i) == 2)			// this is Z layer
 		{
 			nx = 0.0;
@@ -326,7 +368,6 @@
 			nz = 1.0;
 		}
-		Double_t corr = (pxi*nx + pyi * ny + pzi * nz) / p();
-		cs[ii] = corr;
-		Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i));		// Rad. length fraction
+		Double_t corr = TMath::Abs(pxi*nx + pyi * ny + pzi * nz) / p(); 
+		Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i));					// Rad. length fraction
 		thms[ii] = 0.0136*TMath::Sqrt(Rlf)*(1.0 + 0.038*TMath::Log(Rlf)) / p();		// MS angle
 		if (!MS)thms[ii] = 0;
@@ -334,72 +375,39 @@
 		for (Int_t kk = 0; kk < ii; kk++)	// Fill distances between layers
 		{
-			//dik(ii, kk) = TMath::Sqrt(pow(rh[ii] - rh[kk], 2) + pow(zh[ii] - zh[kk], 2));
-			Double_t Ci = C();
-			dik(ii, kk) = (TMath::ASin(Ci*rh[ii])-TMath::ASin(Ci*rh[kk]))/(Ci*snt);
+			Int_t kl = hord[kk];		// Unordered layer
+			dik(ii, kk) = TMath::Abs(dhh[il] - dhh[kl])/snt;
 			dik(kk, ii) = dik(ii, kk);
 		}
-		//
-		// Store momentum components for resolution correction cosines
-		//
-		Double_t *pRi = new Double_t[Nhit];
-		pRi[ii] = TMath::Abs(pxi * TMath::Cos(phi) + pyi * TMath::Sin(phi)); // Radial component
-		Double_t *pPhi = new Double_t[Nhit];
-		pPhi[ii] = TMath::Abs(pxi * TMath::Sin(phi) - pyi * TMath::Cos(phi)); // Phi component
 	}
 	//
 	// Fill measurement covariance
 	//
-	Int_t *mTl = new Int_t[mTot];		// Pointer from measurement number to true layer number
+	TVectorD tPar(5,fpar);
+	//
 	TMatrixDSym Sm(mTot); Sm.Zero();	// Measurement covariance
-	TMatrixD Rm(mTot, 5);				// Derivative matrix
-	Int_t im = 0;
+	TMatrixD Rm(mTot, 5);			// Derivative matrix
+	Int_t im = 0;						// Initialize number of measurement counter
 	//
 	// Fill derivatives and error matrix with MS
 	//
-	Double_t AngMax = 90.; Double_t AngMx = AngMax * TMath::Pi() / 180.;
-	Double_t csMin = TMath::Cos(AngMx);	// Set maximum angle wrt normal
-	//
 	for (Int_t ii = 0; ii < Nhit; ii++)
 	{
-		Int_t i = ih[ii];					// True layer number
+		Int_t i = ih[ii];				// True layer number
 		Int_t ityp  = fG->lTyp(i);			// Layer type Barrel or Z
 		Int_t nmeai = fG->lND(i);			// # measurements in layer
-		if (fG->isMeasure(i) && nmeai >0 && cs[ii] > csMin)
-		{
-			Double_t Di    = D();				// Get true track parameters
-			Double_t phi0i = phi0();
-			Double_t Ci    = C();
-			Double_t z0i   = z0();
-			Double_t cti   = ct();
-			//
-			Double_t Ri    = rh[ii];
-			Double_t ArgRp = (Ri*Ci + (1 + Ci * Di)*Di / Ri) / (1 + 2 * Ci*Di);
-			Double_t ArgRz = Ci * TMath::Sqrt((Ri*Ri - Di * Di) / (1 + 2 * Ci*Di));
-			TVectorD dRphi(5); dRphi.Zero();		// R-phi derivatives @ const. R
-			TVectorD dRz(5); dRz.Zero();			// z     derivatives @ const. R
-			//
-			// Derivative overflow protection
-			Double_t dMin = 0.8;
-			dRphi(0) = (1 - 2 * Ci*Ci*Ri*Ri) /
-				TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp));	// D derivative
-			dRphi(1) = Ri;														// phi0 derivative
-			dRphi(2) = Ri * Ri /
-				TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp));				// C derivative
-			dRphi(3) = 0.0;												// z0 derivative
-			dRphi(4) = 0.0;												// cot(theta) derivative
-			//
-			dRz(0) = -cti * Di /
-				(Ri*TMath::Max(dMin,TMath::Sqrt(1 - Ci * Ci*Ri*Ri)));	// D
-			dRz(1) = 0.0;												// Phi0
-			dRz(2) = cti * (Ri*Ci / TMath::Sqrt(1-Ri*Ri*Ci*Ci) -		// C
-				TMath::ASin(Ri*Ci)) / (Ci*Ci);
-			dRz(3) = 1.0;												// Z0
-			dRz(4) = TMath::ASin(ArgRz) / Ci;							// Cot(theta)
+		
+		if (fG->isMeasure(i))
+		{
+			Double_t Ri = rh[ii];
+			Double_t zi = zh[ii];
 			//
 			for (Int_t nmi = 0; nmi < nmeai; nmi++)
 			{
-				mTl[im] = i;
-				Double_t stri = 0;
-				Double_t sig = 0;
+				Double_t stri = 0;						// Stereo angle
+				Double_t sig = 0;						// Layer resolution
+				// Constant R derivatives
+				TVectorD dRphi(5); dRphi.Zero();		// R-phi derivatives @ const. R
+				TVectorD dRz(5); dRz.Zero();			// z     derivatives @ const. R
+				//
 				if (nmi + 1 == 1)		// Upper layer measurements
 				{
@@ -407,22 +415,29 @@
 					Double_t csa = TMath::Cos(stri);
 					Double_t ssa = TMath::Sin(stri);
+					//
 					sig = fG->lSgU(i);	// Resolution
 					if (ityp == 1)		// Barrel type layer (Measure R-phi, stereo or z at const. R)
 					{
 						//
-						// Almost exact solution (CD<<1, D<<R)
+						// Exact solution
+						dRphi = derRphi_R(tPar, Ri);
+						dRz   = derZ_R   (tPar, Ri);
+						//
 						Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0);	// D derivative
 						Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1);	// phi0 derivative
 						Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2);	// C derivative
 						Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3);	// z0 derivative
-						Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4);	// cot(theta) derivative
+						Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4);	// cot(theta) derivative	
 					}
-					if (ityp == 2)		// Z type layer (Measure phi at const. Z)
+					if (ityp == 2)		// Z type layer (Measure R-phi at const. Z)
 					{
-						Rm(im, 0) = 1.0;					// D derivative
-						Rm(im, 1) = rh[ii];					// phi0 derivative
-						Rm(im, 2) = rh[ii] * rh[ii];		// C derivative
-						Rm(im, 3) = 0;						// z0 derivative
-						Rm(im, 4) = 0;						// cot(theta) derivative
+						TVectorD dRphz(5); dRphz.Zero();		// R-phi derivatives @ const. z
+						dRphz = derRphi_Z(tPar, zi);
+						//
+						Rm(im, 0) = dRphz(0);					// D derivative
+						Rm(im, 1) = dRphz(1);					// phi0 derivative
+						Rm(im, 2) = dRphz(2);					// C derivative
+						Rm(im, 3) = dRphz(3);					// z0 derivative
+						Rm(im, 4) = dRphz(4);					// cot(theta) derivative
 					}
 				}
@@ -436,5 +451,8 @@
 					{
 						//
-						// Almost exact solution (CD<<1, D<<R)
+						// Exact solution
+						dRphi = derRphi_R(tPar, Ri);
+						dRz   = derZ_R   (tPar, Ri);
+						//
 						Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0);	// D derivative
 						Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1);	// phi0 derivative
@@ -445,9 +463,12 @@
 					if (ityp == 2)			// Z type layer (Measure R at const. z)
 					{
-						Rm(im, 0) = 0;								// D derivative
-						Rm(im, 1) = 0;								// phi0 derivative
-						Rm(im, 2) = 0;								// C derivative
-						Rm(im, 3) = -1.0 / ct();					// z0 derivative
-						Rm(im, 4) = -rh[ii] / ct();					// cot(theta) derivative
+						TVectorD dRRz(5); dRRz.Zero();			// R     derivatives @ const. z
+						dRRz = derR_Z(tPar, zi);
+						//
+						Rm(im, 0) = dRRz(0);					// D derivative
+						Rm(im, 1) = dRRz(1);					// phi0 derivative
+						Rm(im, 2) = dRRz(2);					// C derivative
+						Rm(im, 3) = dRRz(3);					// z0 derivative
+						Rm(im, 4) = dRRz(4);					// cot(theta) derivative
 					}
 				}
@@ -457,20 +478,27 @@
 				//
 				Int_t km = 0;
+				Double_t CosMin = TMath::Sin(TMath::Pi() / 9.);	// Protect for derivative explosion
 				for (Int_t kk = 0; kk <= ii; kk++)
 				{
-					Int_t k = ih[kk];					// True layer number
-					Int_t ktyp = fG->lTyp(k);			// Layer type Barrel or
+					Int_t k = ih[kk];				// True layer number
+					Int_t ktyp = fG->lTyp(k);			// Layer type Barrel or disk
 					Int_t nmeak = fG->lND(k);			// # measurements in layer
-					if (fG->isMeasure(k) && nmeak > 0 &&cs[kk] > csMin)
+					if (fG->isMeasure(k))
 					{
 						for (Int_t nmk = 0; nmk < nmeak; nmk++)
 						{
 							Double_t strk = 0;
-							if (nmk + 1 == 1) strk = fG->lStU(k);	// Stereo angle
-							if (nmk + 1 == 2) strk = fG->lStL(k);	// Stereo angle
-							if (im == km && Res) Sm(im, km) += sig*sig;	// Detector resolution on diagonal
+							if (nmk + 1 == 1) strk = fG->lStU(k);	// Stereo angle upper 
+							if (nmk + 1 == 2) strk = fG->lStL(k);	// Stereo angle lower
+							//if (im == km && Res) Sm(im, km) += sig*sig;	// Detector resolution on diagonal
+							if (im == km && Res) {
+								Double_t sg = sig;
+								if(TMath::Abs(strk) < TMath::Pi()/6. && cs[kk] < CosMin)
+								TMath::Min(1000.*sig,sg = sig/pow(cs[kk],4));
+								Sm(im, km) += sg * sg;	// Detector resolution on diagonal
+							}
 							//
 							// Loop on all layers below for MS contributions
-							for (Int_t jj = 0; jj < kk; jj++)
+							for (Int_t jj = 0; jj < kk; jj++)		
 							{
 								Double_t di = dik(ii, jj);
@@ -482,7 +510,7 @@
 								if (ktyp == 1) msk = ms / snt;			// Barrel
 								else if (ktyp == 2) msk = ms / cst;		// Disk
-								Double_t ci = TMath::Cos(stri); Double_t si = TMath::Sin(stri);
-								Double_t ck = TMath::Cos(strk); Double_t sk = TMath::Sin(strk);
-								Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk);			// Ms contribution
+								Double_t ci = TMath::Abs(TMath::Cos(stri)); Double_t si = TMath::Abs(TMath::Sin(stri));
+								Double_t ck = TMath::Abs(TMath::Cos(strk)); Double_t sk = TMath::Abs(TMath::Sin(strk));
+								Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk);	// Ms contribution
 							}
 							//
@@ -497,4 +525,5 @@
 	}
 	Sm.ResizeTo(mTot, mTot);
+	TMatrixDSym SmTemp = Sm;
 	Rm.ResizeTo(mTot, 5);
 	//
@@ -506,9 +535,8 @@
 	for (Int_t id = 0; id < mTot; id++) DSmInv(id, id) = 1.0 / TMath::Sqrt(Sm(id, id));
 	TMatrixDSym SmN = Sm.Similarity(DSmInv);	// Normalize diagonal to 1
-	//SmN.Invert();
 	//
 	// Protected matrix inversions
 	//
-	TDecompChol Chl(SmN);
+	TDecompChol Chl(SmN,1.e-12);
 	TMatrixDSym SmNinv = SmN;
 	if (Chl.Decompose())
@@ -519,5 +547,6 @@
 	else
 	{
-		cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << endl;
+		std::cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << std::endl;
+		//cout << "pt = " << pt() << endl;
 		if (ntry < ntrymax)
 		{
@@ -525,4 +554,5 @@
 			ntry++;
 		}
+		//
 		TMatrixDSym rSmN = MakePosDef(SmN); SmN = rSmN;
 		TDecompChol rChl(SmN);
@@ -533,5 +563,4 @@
 	Sm = SmNinv.Similarity(DSmInv);			// Error matrix inverted
 	TMatrixDSym H = Sm.SimilarityT(Rm);		// Calculate half Hessian
-	// Normalize before inversion
 	const Int_t Npar = 5;
 	TMatrixDSym DHinv(Npar); DHinv.Zero();
@@ -541,6 +570,25 @@
 	Hnrm.Invert();
 	fCov = Hnrm.Similarity(DHinv);
-}
-
+	//
+	// debug
+	//
+	if(TMath::IsNaN(fCov(0,0)))
+	{
+		std::cout<<"SolTrack::CovCalc: NaN found in covariance matrix"<<std::endl;
+	}
+	//
+	// Lots of cleanup to do
+	delete[] zhh;
+	delete[] rhh;
+	delete[] dhh;
+	delete[] ihh;
+	delete[] hord;
+	delete[] zh;
+	delete[] rh;
+	delete[] ih;
+	delete[] cs;
+	delete[] thms;
+}
+//
 // Force positive definitness in normalized matrix
 TMatrixDSym SolTrack::MakePosDef(TMatrixDSym NormMat)
@@ -558,5 +606,5 @@
 	if (Check)
 	{
-		cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << endl;
+		std::cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << std::endl;
 		return rMatN;
 	}
@@ -566,4 +614,6 @@
 	TMatrixD U = Eign.GetEigenVectors();
 	TVectorD lambda = Eign.GetEigenValues();
+	//cout << "Eigenvalues:"; lambda.Print();
+	//cout << "Input matrix: "; NormMat.Print();
 	// Reset negative eigenvalues to small positive value
 	TMatrixDSym D(Size); D.Zero(); Double_t eps = 1.0e-13;
@@ -587,4 +637,5 @@
 		}
 	}
+	//cout << "Rebuilt matrix: "; rMatN.Print();
 	return rMatN;
 }
Index: external/TrackCovariance/SolTrack.h
===================================================================
--- external/TrackCovariance/SolTrack.h	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/SolTrack.h	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -1,69 +1,83 @@
+//
 #ifndef G__SOLTRK_H
 #define G__SOLTRK_H
-
+//
 #include <TMath.h>
 #include <TVector3.h>
 #include <TMatrixDSym.h>
-
-class SolGeom;
-
+#include "SolGeom.h"
+#include "TrkUtil.h"
+#include <TGraph.h>
+//
+//
 // Class to store track information
 // Assumes that the geometry has been initialized
-class SolTrack{
-  // Track handling class
-  // Assume tracks originate from (0,0) for the time being
+//
+class SolTrack: public TrkUtil
+{
+	// 
+	// Track handling class
+	// Assume tracks originate from (0,0) for the time being
+	//
 private:
-  Int_t fNl;        // Actual number of layers
-  SolGeom *fG;      // Geometry
-  Double_t fp[3];   // px, py, pz momentum
-  Double_t fx[3];   //  x,  y,  z track origin
-  Double_t fpar[5]; // D, phi0, C, z0, cot(theta)
-
-  TMatrixDSym fCov; // Full covariance matrix
-
+	Int_t fNl;	// Actual number of layers
+	SolGeom *fG;	// Geometry
+	Double_t fp[3];	// px, py, pz momentum 
+	Double_t fx[3];	//  x,  y,  z track origin 
+	Double_t fpar[5];	// D, phi0, C, z0, cot(theta)
+	//
+	TMatrixDSym fCov;		// Full covariance matrix
+	//
+	//
 public:
-  // Constructors
-  SolTrack(Double_t *x, Double_t *p, SolGeom *G);
+	//
+	// Constructors
+	SolTrack(Double_t *x, Double_t *p, SolGeom *G);
 	SolTrack(TVector3 x, TVector3 p, SolGeom* G);
-  SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G);
-  // Destructor
-  ~SolTrack();
-  // Accessors
-  // Position (at minimum approach)
-  Double_t x() { return fx[0]; }
-  Double_t y() { return fx[1]; }
-  Double_t z() { return fx[2]; }
-  // Momentum  (at minimum approach)
-  Double_t px() { return fp[0]; }
-  Double_t py() { return fp[1]; }
-  Double_t pz() { return fp[2]; }
-  Double_t pt() { return TMath::Sqrt(fp[0] * fp[0] + fp[1] * fp[1]); }
-  Double_t p()  { return TMath::Sqrt(fp[0] * fp[0] + fp[1] * fp[1] + fp[2] * fp[2]); }
-  // Track parameters
-  Double_t D()    { return fpar[0]; }
-  Double_t phi0() { return fpar[1]; }
-  Double_t C()    { return fpar[2]; }
-  Double_t z0()   { return fpar[3]; }
-  Double_t ct()   { return fpar[4]; }
-  // Covariance
-  TMatrixDSym Cov()   { return fCov; }
-  // Track parameter covariance calculation
-  void CovCalc(Bool_t Res, Bool_t MS);
-  // Parameter errors
-  Double_t s_D()    { return TMath::Sqrt(fCov(0, 0)); }
-  Double_t s_phi0() { return TMath::Sqrt(fCov(1, 1)); }
-  Double_t s_C()    { return TMath::Sqrt(fCov(2, 2)); }
-  Double_t s_pt()   { return 2 * s_C()*pt() / (0.2998*fG->B()); } // Dpt/pt
-  Double_t s_z0()   { return TMath::Sqrt(fCov(3, 3)); }
-  Double_t s_ct()   { return TMath::Sqrt(fCov(4, 4)); }
-  // Track hit management
-  Int_t nHit();
-  Int_t nmHit();
-  Bool_t HitLayer(Int_t Layer, Double_t &R, Double_t &phi, Double_t &zz);
-  Int_t HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh);
-  Int_t HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh);
-
-  // Make normalized matrix positive definite
-  TMatrixDSym MakePosDef(TMatrixDSym NormMat);
+	SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G);
+	// Destructor
+	~SolTrack();
+	// Accessors
+	// Position (at minimum approach)
+	Double_t x() { return fx[0]; }
+	Double_t y() { return fx[1]; }
+	Double_t z() { return fx[2]; }
+	// Momentum  (at minimum approach)
+	Double_t px() { return fp[0]; }
+	Double_t py() { return fp[1]; }
+	Double_t pz() { return fp[2]; }
+	Double_t pt() { return TMath::Sqrt(fp[0] * fp[0] + fp[1] * fp[1]); }
+	Double_t p()  { return TMath::Sqrt(fp[0] * fp[0] + fp[1] * fp[1] + fp[2] * fp[2]); }
+	// Track parameters
+	Double_t D()    { return fpar[0]; }
+	Double_t phi0() { return fpar[1]; }
+	Double_t C()    { return fpar[2]; }
+	Double_t z0()   { return fpar[3]; }
+	Double_t ct()   { return fpar[4]; }
+	// Covariance
+	TMatrixDSym Cov()		{ return fCov; }
+	// Track parameter covariance calculation
+	void CovCalc(Bool_t Res, Bool_t MS);
+	// Parameter errors
+	Double_t s_D()    { return TMath::Sqrt(fCov(0, 0)); }
+	Double_t s_phi0() { return TMath::Sqrt(fCov(1, 1)); }
+	Double_t s_C()    { return TMath::Sqrt(fCov(2, 2)); }
+	Double_t s_pt()   { return 2 * s_C()*pt() / (0.2998*fG->B()); }	// Dpt/pt
+	Double_t s_z0()   { return TMath::Sqrt(fCov(3, 3)); }
+	Double_t s_ct()   { return TMath::Sqrt(fCov(4, 4)); }
+	//
+	// Track hit management
+	Int_t nHit();
+	Int_t nmHit();
+	Bool_t HitLayer(Int_t Layer, Double_t &R, Double_t &phi, Double_t &zz);
+	Int_t HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh);
+	Int_t HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh);
+	//
+	// Track graph
+	TGraph *TrkPlot();	// Graph with R-z plot of track trajectory
+	//
+	// Make normalized matrix positive definite
+	TMatrixDSym MakePosDef(TMatrixDSym NormMat);
 };
+//
 #endif
Index: external/TrackCovariance/TrkUtil.cc
===================================================================
--- external/TrackCovariance/TrkUtil.cc	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/TrkUtil.cc	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -3,4 +3,5 @@
 #include <algorithm>
 #include <TSpline.h>
+#include <TDecompChol.h>
 
 // Constructor
@@ -33,4 +34,82 @@
 	fZmin = 0.0;				// Lower		DCH z
 	fZmax = 0.0;				// Higher	DCH z
+}
+//
+// Distance between two lines
+//
+void TrkUtil::LineDistance(TVector3 x0, TVector3 y0, TVector3 dirx, TVector3 diry, Double_t &sx, Double_t &sy, Double_t &distance)
+{
+	TMatrixDSym M(2);
+	M(0,0) = dirx.Mag2();
+	M(1,1) = diry.Mag2();
+	M(0,1) = -dirx.Dot(diry);
+	M(1,0) = M(0,1);
+	M.Invert();
+	TVectorD c(2);
+	c(0) = dirx.Dot(y0-x0);
+	c(1) = diry.Dot(x0-y0);
+	TVectorD st = M*c;
+	//
+	// Fill output
+	sx = st(0);
+	sy = st(1);
+	//
+	TVector3 x = x0+sx*dirx;
+	TVector3 y = y0+sy*diry;
+	TVector3 d = x-y;
+	distance = d.Mag();
+}
+//
+// Covariance smearing
+//
+TVectorD TrkUtil::CovSmear(TVectorD x, TMatrixDSym C)
+{
+	//
+	// Check arrays
+	//
+	// Consistency of dimensions
+	Int_t Nvec = x.GetNrows();
+	Int_t Nmat = C.GetNrows();
+	if (Nvec != Nmat || Nvec == 0)
+	{
+		std::cout << "TrkUtil::CovSmear: vector/matrix mismatch. Aborting." << std::endl;
+		exit(EXIT_FAILURE);
+	}
+	// Positive diagonal elements
+	for (Int_t i = 0; i < Nvec; i++)
+	{
+		if (C(i, i) <= 0.0)
+		{
+			std::cout << "TrkUtil::CovSmear: covariance matrix has negative diagonal elements. Aborting." << std::endl;
+			exit(EXIT_FAILURE);
+		}
+	}
+	//
+	// Do a Choleski decomposition and random number extraction, with appropriate stabilization
+	//
+	TMatrixDSym CvN = C;
+	TMatrixDSym DCv(Nvec); DCv.Zero();
+	TMatrixDSym DCvInv(Nvec); DCvInv.Zero();
+	for (Int_t id = 0; id < Nvec; id++)
+	{
+		Double_t dVal = TMath::Sqrt(C(id, id));
+		DCv(id, id) = dVal;
+		DCvInv(id, id) = 1.0 / dVal;
+	}
+	CvN.Similarity(DCvInv);			// Normalize diagonal to 1
+	TDecompChol Chl(CvN);
+	Bool_t OK = Chl.Decompose();		// Choleski decomposition of normalized matrix
+	if (!OK)
+	{
+		std::cout << "TrkUtil::CovSmear: covariance matrix is not positive definite. Aborting." << std::endl;
+		exit(EXIT_FAILURE);
+	}
+	TMatrixD U = Chl.GetU();			// Get Upper triangular matrix
+	TMatrixD Ut(TMatrixD::kTransposed, U); // Transposed of U (lower triangular)
+	TVectorD r(Nvec);
+	for (Int_t i = 0; i < Nvec; i++)r(i) = gRandom->Gaus(0.0, 1.0);		// Array of normal random numbers
+	TVectorD xOut = x + DCv * (Ut * r);	// Observed parameter vector
+	//
+	return xOut;
 }
 //
@@ -48,6 +127,6 @@
 	Double_t r2 = x(0) * x(0) + x(1) * x(1);
 	Double_t cross = x(0) * p(1) - x(1) * p(0);
-	Double_t T = sqrt(pt * pt - 2 * a * cross + a * a * r2);
-	Double_t phi0 = atan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T);	// Phi0
+	Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2);
+	Double_t phi0 = TMath::ATan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T);	// Phi0
 	Double_t D;							// Impact parameter D
 	if (pt < 10.0) D = (T - pt) / a;
@@ -58,6 +137,6 @@
 	Par(2) = C;		// Store C
 	//Longitudinal parameters
-	Double_t B = C * sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
-	Double_t st = asin(B) / C;
+	Double_t B = C * TMath::Sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
+	Double_t st = TMath::ASin(B) / C;
 	Double_t ct = p(2) / pt;
 	Double_t z0;
@@ -235,4 +314,346 @@
 	//
 	return Cmm;
+}//
+// Regularized symmetric matrix inversion
+//
+TMatrixDSym TrkUtil::RegInv(TMatrixDSym& Min)
+{
+	TMatrixDSym M = Min;				// Decouple from input
+	Int_t N = M.GetNrows();			// Matrix size
+	TMatrixDSym D(N); D.Zero();		// Normaliztion matrix
+	TMatrixDSym R(N);				// Normarized matrix
+	TMatrixDSym Rinv(N);				// Inverse of R
+	TMatrixDSym Minv(N);				// Inverse of M
+	//
+	// Check for 0's and normalize
+	for (Int_t i = 0; i < N; i++)
+	{
+		if (M(i, i) != 0.0) D(i, i) = 1. / TMath::Sqrt(TMath::Abs(M(i, i)));
+		else D(i, i) = 1.0;
+	}
+	R = M.Similarity(D);
+	//
+	// Recursive algorithms stops when N = 2
+	//
+	//****************
+	// case N = 2  ***
+	//****************
+	if (N == 2)
+	{
+		Double_t det = R(0, 0) * R(1, 1) - R(0, 1) * R(1, 0);
+		if (det == 0)
+		{
+			std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
+			Rinv.Zero();	// Return null matrix
+		}
+		else
+		{
+			// invert matrix 
+			Rinv(0, 0) = R(1, 1);
+			Rinv(0, 1) = -R(0, 1);
+			Rinv(1, 0) = Rinv(0, 1);
+			Rinv(1, 1) = R(0, 0);
+			Rinv *= 1. / det;
+		}
+	}
+	//****************
+	// case N > 2  ***
+	//****************
+	else
+	{
+		// Break up matrix
+		TMatrixDSym Q = R.GetSub(0, N - 2, 0, N - 2);	// Upper left 
+		TVectorD p(N - 1);
+		for (Int_t i = 0; i < N - 1; i++)p(i) = R(N - 1, i);
+		Double_t q = R(N - 1, N - 1);
+		//Invert pieces and re-assemble
+		TMatrixDSym Ainv(N - 1);
+		TMatrixDSym A(N - 1);
+		if (TMath::Abs(q) > 1.0e-15)
+		{
+			// Case |q| > 0
+			Ainv.Rank1Update(p, -1.0 / q);
+			Ainv += Q;
+			A = RegInv(Ainv);		// Recursive call
+			TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
+			//
+			TVectorD b = (-1.0 / q) * (A * p);
+			for (Int_t i = 0; i < N - 1; i++)
+			{
+				Rinv(N - 1, i) = b(i);
+				Rinv(i, N - 1) = b(i);
+			}
+			//
+			Double_t pdotb = 0.;
+			for (Int_t i = 0; i < N - 1; i++)pdotb += p(i) * b(i);
+			Double_t c = (1.0 - pdotb) / q;
+			Rinv(N - 1, N - 1) = c;
+		}
+		else
+		{
+			// case q = 0
+			TMatrixDSym Qinv = RegInv(Q);		// Recursive call
+			Double_t a = Qinv.Similarity(p);
+			Double_t c = -1.0 / a;
+			Rinv(N - 1, N - 1) = c;
+			//
+			TVectorD b = (1.0 / a) * (Qinv * p);
+			for (Int_t i = 0; i < N - 1; i++)
+			{
+				Rinv(N - 1, i) = b(i);
+				Rinv(i, N - 1) = b(i);
+			}
+			//
+			A.Rank1Update(p, -1 / a);
+			A += Q;
+			A.Similarity(Qinv);
+			TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
+		}
+	}
+	Minv = Rinv.Similarity(D);
+	return Minv;
+}
+//
+// Track tracjectory
+//
+TVector3 TrkUtil::Xtrack(TVectorD par, Double_t s)
+{
+	//
+	// unpack parameters
+	Double_t D = par(0);
+	Double_t p0 = par(1);
+	Double_t C = par(2);
+	Double_t z0 = par(3);
+	Double_t ct = par(4);
+	//
+	Double_t x = -D * TMath::Sin(p0) + (TMath::Sin(s + p0) - TMath::Sin(p0)) / (2 * C);
+	Double_t y =  D * TMath::Cos(p0) - (TMath::Cos(s + p0) - TMath::Cos(p0)) / (2 * C);	
+	Double_t z = z0 + ct * s / (2 * C);
+	//
+	TVector3 Xt(x, y, z);
+	return Xt;
+}
+//
+// Track derivatives
+//
+// Constant radius
+// R-Phi
+TVectorD TrkUtil::derRphi_R(TVectorD par, Double_t R)
+{
+	TVectorD dRphi(5);	// return vector
+	//
+	// unpack parameters
+	Double_t D = par(0);
+	Double_t C = par(2);
+	//
+	Double_t s = 2 * TMath::ASin(C * TMath::Sqrt((R * R - D * D)/(1 + 2 * C * D)));
+	TVector3 X = Xtrack(par, s);		// Intersection point
+	TVector3 v(-X.y()/R, X.x()/R, 0.);	// measurement direction
+	TMatrixD derX = derXdPar(par, s);	// dX/dp
+	TVectorD derXs = derXds(par, s);	// dX/ds
+	TVectorD ders = dsdPar_R(par, R);	// ds/dp	
+	//
+	for (Int_t i = 0; i < 5; i++)
+	{
+		dRphi(i) = 0.;
+		for (Int_t j = 0; j < 3; j++)
+		{
+			dRphi(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
+		}
+	}
+	//
+	return dRphi;
+}
+// z
+TVectorD TrkUtil::derZ_R(TVectorD par, Double_t R)
+{
+
+	TVectorD dZ(5);	// return vector
+	//
+	// unpack parameters
+	Double_t D = par(0);
+	Double_t C = par(2);
+	//
+	Double_t s = 2 * TMath::ASin(C * TMath::Sqrt((R * R - D * D)/(1 + 2 * C * D))); // phase
+	TVector3 v(0., 0., 1.);				// measurement direction
+	TMatrixD derX = derXdPar(par, s);	// dX/dp
+	TVectorD derXs = derXds(par, s);	// dX/ds
+	TVectorD ders = dsdPar_R(par, R);	// ds/dp	
+	//
+	for (Int_t i = 0; i < 5; i++)
+	{
+		dZ(i) = 0.;
+		for (Int_t j = 0; j < 3; j++)
+		{
+			dZ(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
+		}
+	}
+	//
+	return dZ;
+}
+//
+// constant z
+// R-Phi
+TVectorD TrkUtil::derRphi_Z(TVectorD par, Double_t z)
+{
+	TVectorD dRphi(5);	// return vector
+	//
+	// unpack parameters
+	Double_t C = par(2);
+	Double_t z0 = par(3);
+	Double_t ct = par(4);
+	//
+	Double_t s = 2 * C * (z - z0) / ct;
+	TVector3 X = Xtrack(par, s);			// Intersection point
+	TVector3 v(-X.y() / X.Pt(), X.x() / X.Pt(), 0.);	// measurement direction
+	TMatrixD derX = derXdPar(par, s);		// dX/dp
+	TVectorD derXs = derXds(par, s);		// dX/ds
+	TVectorD ders = dsdPar_z(par, z);		// ds/dp	
+	//
+	for (Int_t i = 0; i < 5; i++)
+	{
+		dRphi(i) = 0.;
+		for (Int_t j = 0; j < 3; j++)
+		{
+			dRphi(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
+		}
+	}
+	//
+	return dRphi;
+
+}
+// R
+TVectorD TrkUtil::derR_Z(TVectorD par, Double_t z)
+{
+	TVectorD dR(5);	// return vector
+	//
+	// unpack parameters
+	Double_t C = par(2);
+	Double_t z0 = par(3);
+	Double_t ct = par(4);
+	//
+	Double_t s = 2 * C * (z - z0) / ct;
+	TVector3 X = Xtrack(par, s);			// Intersection point
+	TVector3 v(X.x() / X.Pt(), X.y() / X.Pt(), 0.);	// measurement direction
+	TMatrixD derX = derXdPar(par, s);		// dX/dp
+	TVectorD derXs = derXds(par, s);		// dX/ds
+	TVectorD ders = dsdPar_z(par, z);	// ds/dp	
+	//
+	for (Int_t i = 0; i < 5; i++)
+	{
+		dR(i) = 0.;
+		for (Int_t j = 0; j < 3; j++)
+		{
+			dR(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
+		}
+	}
+	//
+	return dR;
+
+}
+//
+// derivatives of track trajectory
+//
+// dX/dPar
+TMatrixD TrkUtil::derXdPar(TVectorD par, Double_t s)
+{
+	TMatrixD dxdp(3, 5);	// return matrix
+	//
+	// unpack parameters
+	Double_t D = par(0);
+	Double_t p0 = par(1);
+	Double_t C = par(2);
+	Double_t z0 = par(3);
+	Double_t ct = par(4);
+	//
+	// derivatives
+	// dx/dD
+	dxdp(0, 0) = -TMath::Sin(p0);
+	dxdp(1, 0) =  TMath::Cos(p0);
+	dxdp(2, 0) = 0.;
+	// dx/dphi0
+	dxdp(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(s + p0) - TMath::Cos(p0)) / (2 * C);
+	dxdp(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(s + p0) - TMath::Sin(p0)) / (2 * C);
+	dxdp(2, 1) = 0;
+	// dx/dC
+	dxdp(0, 2) = -(TMath::Sin(s + p0) - TMath::Sin(p0)) / (2 * C * C);
+	dxdp(1, 2) =  (TMath::Cos(s + p0) - TMath::Cos(p0)) / (2 * C * C);
+	dxdp(2, 2) = -ct * s / (2 * C * C);
+	// dx/dz0
+	dxdp(0, 3) = 0;
+	dxdp(1, 3) = 0;
+	dxdp(2, 3) = 1.;
+	// dx/dCtg
+	dxdp(0, 4) = 0;
+	dxdp(1, 4) = 0;
+	dxdp(2, 4) = s / (2 * C);
+	//
+	return dxdp;
+}
+//
+// dX/ds
+//
+TVectorD TrkUtil::derXds(TVectorD par, Double_t s)
+{
+	TVectorD dxds(3);	// return vector
+	//
+	// unpack parameters
+	Double_t p0 = par(1);
+	Double_t C = par(2);
+	Double_t ct = par(4);
+	//
+	// dX/ds
+	dxds(0) = TMath::Cos(s + p0) / (2 * C);
+	dxds(1) = TMath::Sin(s + p0) / (2 * C);
+	dxds(2) = ct / (2 * C);
+	//
+	return dxds;
+}
+//
+// derivative of trajectory phase s
+//Constant R
+TVectorD TrkUtil::dsdPar_R(TVectorD par, Double_t R)
+{
+	TVectorD dsdp(5);	// return vector
+	//
+	// unpack parameters
+	Double_t D = par(0);
+	Double_t p0 = par(1);
+	Double_t C = par(2);
+	//
+	// derivatives
+	Double_t opCD = 1. + 2 * C * D;
+	Double_t A = C*TMath::Sqrt((R*R-D*D)/opCD);
+	Double_t sqA0 = TMath::Sqrt(1. - A * A);
+	Double_t dMin = 0.01;
+	Double_t sqA = TMath::Max(dMin, sqA0);	// Protect against divergence
+	//
+	dsdp(0) = -2 * C * C * (D * (1. + C * D) + C * R * R) / (A * sqA * opCD * opCD);
+	dsdp(1) = 0;
+	dsdp(2) = 2 * A * (1 + C * D) / (C * sqA * opCD);
+	dsdp(3) = 0;
+	dsdp(4) = 0;
+	//
+	return dsdp;
+}
+// Constant z
+TVectorD TrkUtil::dsdPar_z(TVectorD par, Double_t z)
+{
+	TVectorD dsdp(5);	// return vector
+	//
+	// unpack parameters
+	Double_t C = par(2);
+	Double_t z0 = par(3);
+	Double_t ct = par(4);
+	//
+	// derivatives
+	//
+	dsdp(0) = 0;
+	dsdp(1) = 0;
+	dsdp(2) = 2*(z-z0)/ct;
+	dsdp(3) = -2*C/ct;
+	dsdp(4) = -2*C*(z-z0)/(ct*ct);
+	//
+	return dsdp;
 }
 //
Index: external/TrackCovariance/TrkUtil.h
===================================================================
--- external/TrackCovariance/TrkUtil.h	(revision fc6bce2a74930298b5d74b9c243f653cb3112011)
+++ external/TrackCovariance/TrkUtil.h	(revision ebf40fd3f3ff25430347fd821d73d468acce4df6)
@@ -28,4 +28,11 @@
 	TVectorD XPtoPar(TVector3 x, TVector3 p, Double_t Q);
 	TVector3 ParToP(TVectorD Par);
+	TMatrixDSym RegInv(TMatrixDSym& Min);
+	//
+	// Track trajectory derivatives
+	TMatrixD derXdPar(TVectorD par, Double_t s);	// derivatives of position wrt parameters
+	TVectorD derXds(TVectorD par, Double_t s);		// derivatives of position wrt phase
+	TVectorD dsdPar_R(TVectorD par, Double_t R);	// derivatives of phase at constant R
+	TVectorD dsdPar_z(TVectorD par, Double_t z);	// derivatives of phase at constant z
 	//
 	// Conversion to ACTS parametrization
@@ -54,5 +61,5 @@
 		Double_t c = 2.99792458e8;	// speed of light m/sec
 		//return TMath::C()*1.0e-9;	// Incompatible with root5
-		return c*1.0e-9; 		// Reduced speed of light	
+		return c*1.0e-9; 			// Reduced speed of light	
 	}
 	//
@@ -63,4 +70,17 @@
 	static TVector3 ParToP(TVectorD Par, Double_t Bz);	// Get Momentum from track parameters
 	static Double_t ParToQ(TVectorD Par);				// Get track charge
+	static void LineDistance(TVector3 x0, TVector3 y0, TVector3 dirx, TVector3 diry, Double_t &sx, Double_t &sy, Double_t &distance);
+	//
+	// Track trajectory
+	//
+	static TVector3 Xtrack(TVectorD par, Double_t s);		// Parametric track trajectory
+	TVectorD derRphi_R(TVectorD par, Double_t R);	// Derivatives of R-phi at constant R
+	TVectorD derZ_R(TVectorD par, Double_t R);		// Derivatives of z at constant R
+	TVectorD derRphi_Z(TVectorD par, Double_t z);	// Derivatives of R-phi at constant z
+	TVectorD derR_Z(TVectorD par, Double_t z);		// Derivatives of R at constant z
+	//
+	// Smear with given covariance matrix
+	//
+	static TVectorD CovSmear(TVectorD x, TMatrixDSym C);
 	//
 	// Conversion from meters to mm
@@ -68,4 +88,13 @@
 	static TVectorD ParToMm(TVectorD Par);			// Parameter conversion
 	static TMatrixDSym CovToMm(TMatrixDSym Cov);	// Covariance conversion
+	//
+	// Inside cylindrical volume
+	//
+	static Bool_t IsInside(TVector3 x, Double_t Rout, Double_t Zmin, Double_t Zmax)
+	{
+		Bool_t Is = kFALSE;
+		if (x.Pt() <= Rout && x.z() >= Zmin && x.z() <= Zmax)Is = kTRUE;
+		return Is;
+	}
 	//
 	// Cluster counting in gas
