Index: external/TrackCovariance/TrkUtil.h
===================================================================
--- external/TrackCovariance/TrkUtil.h	(revision bd376e3a1d8131d6e838b21d8cd738f47b325aad)
+++ external/TrackCovariance/TrkUtil.h	(revision b750b0a794794673ed5b2468f10e215307d06a20)
@@ -68,15 +68,15 @@
 	//
 	static TVectorD XPtoPar(TVector3 x, TVector3 p, Double_t Q, Double_t Bz);
-	static TVector3 ParToX(TVectorD Par);				// position of minimum distance from z axis
+	static TVector3 ParToX(TVectorD Par);			// position of minimum distance from z axis
 	static TVector3 ParToP(TVectorD Par, Double_t Bz);	// Get Momentum from track parameters
-	static Double_t ParToQ(TVectorD Par);				// Get track charge
+	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
+	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 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
 	//
@@ -88,5 +88,5 @@
 	//
 	static TVectorD ParToMm(TVectorD Par);			// Parameter conversion
-	static TMatrixDSym CovToMm(TMatrixDSym Cov);	// Covariance conversion
+	static TMatrixDSym CovToMm(TMatrixDSym Cov);		// Covariance conversion
 	//
 	// Inside cylindrical volume
Index: external/TrackCovariance/VertexFit.cc
===================================================================
--- external/TrackCovariance/VertexFit.cc	(revision bd376e3a1d8131d6e838b21d8cd738f47b325aad)
+++ external/TrackCovariance/VertexFit.cc	(revision b750b0a794794673ed5b2468f10e215307d06a20)
@@ -1,7 +1,11 @@
-﻿#include <TMath.h>
+/*
+Vertex fitting code
+*/
+#include <TMath.h>
 #include <TVectorD.h>
 #include <TVector3.h>
 #include <TMatrixD.h>
 #include <TMatrixDSym.h>
+#include <TMatrixDSymEigen.h>
 #include "VertexFit.h"
 //
@@ -38,10 +42,8 @@
 	for (Int_t i = 0; i < fNtr; i++)
 	{
-		TVectorD pr = *trkPar[i];
-		fPar.push_back(new TVectorD(pr));
-		fParNew.push_back(new TVectorD(pr));
-		TMatrixDSym cv = *trkCov[i];
-		fCov.push_back(new TMatrixDSym(cv));
-		fCovNew.push_back(new TMatrixDSym(cv));
+		fPar.push_back(new TVectorD(*trkPar[i]));
+		fParNew.push_back(new TVectorD(*trkPar[i]));
+		fCov.push_back(new TMatrixDSym(*trkCov[i]));
+		fCovNew.push_back(new TMatrixDSym(*trkCov[i]));
 	}
 	fChi2List.ResizeTo(fNtr);
@@ -76,26 +78,28 @@
 void VertexFit::ResetWrkArrays()
 {
-	Int_t N = (Int_t)ffi.size();
-	for (Int_t i = 0; i < N; i++)
-	{
-		if (fx0i[i])  { fx0i[i]->Clear();		delete fx0i[i]; }
-		if (fai[i])   { fai[i]->Clear();		delete fai[i]; }
-		if (fdi[i])   { fdi[i]->Clear();		delete fdi[i]; }
-		if (fAti[i])  { fAti[i]->Clear();		delete fAti[i]; }
-		if (fDi[i])   { fDi[i]->Clear();		delete fDi[i]; }
-		if (fWi[i])   { fWi[i]->Clear();		delete fWi[i]; }
-		if (fWinvi[i]){ fWinvi[i]->Clear();     delete fWinvi[i]; }
-	}
-	fa2i.clear();
-	fx0i.clear();
-	fai.clear();
-	fdi.clear();
-	fAti.clear();
-	fDi.clear();
-	fWi.clear();
-	fWinvi.clear();
+	Int_t N = (Int_t)fdi.size();
+	if(N > 0){
+		for (Int_t i = 0; i < N; i++)
+		{
+			if (fx0i[i])  { fx0i[i]->Clear();	delete fx0i[i]; }
+			if (fai[i])   { fai[i]->Clear();	delete fai[i]; }
+			if (fdi[i])   { fdi[i]->Clear();	delete fdi[i]; }
+			if (fAti[i])  { fAti[i]->Clear();	delete fAti[i]; }
+			if (fDi[i])   { fDi[i]->Clear();	delete fDi[i]; }
+			if (fWi[i])   { fWi[i]->Clear();	delete fWi[i]; }
+			if (fWinvi[i]){ fWinvi[i]->Clear();     delete fWinvi[i]; }
+		}
+		fa2i.clear();
+		fx0i.clear();
+		fai.clear();
+		fdi.clear();
+		fAti.clear();
+		fDi.clear();
+		fWi.clear();
+		fWinvi.clear();
+	}
 }
 VertexFit::~VertexFit()
-{
+{	
 	fxCst.Clear();		
 	fCovCst.Clear();
@@ -103,5 +107,5 @@
 	fXv.Clear();		
 	fcovXv.Clear();		
-	fChi2List.Clear();	
+	fChi2List.Clear();
 	//
 	for (Int_t i = 0; i < fNtr; i++)
@@ -111,5 +115,5 @@
 		fCov[i]->Clear();		delete fCov[i];
 		fCovNew[i]->Clear();	delete fCovNew[i];
-	}
+	}	
 	fPar.clear();
 	fParNew.clear();
@@ -118,218 +122,16 @@
 	//
 	ResetWrkArrays();
-	ffi.clear();
+	ffi.clear();	
 	fNtr = 0;
 }
 //
-Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
-{
-	//
-	// Find radius of minimum distance between two tracks
-	//
-	// p = (D,phi, C, z0, ct)
-	//
-	// Define arrays
-	//
-	Double_t C1 = p1(2);
-	Double_t C2 = p2(2);
-	Double_t ph1 = p1(1);
-	Double_t ph2 = p2(1);
-	TVectorD x0 = Fill_x0(p1);
-	TVectorD y0 = Fill_x0(p2);
-	TVectorD n = Fill_a(p1, 0.0);
-	n *= (2*C1);
-	TVectorD k = Fill_a(p2, 0.0);
-	k *= (2*C2);
-	//
-	// Setup and solve linear system
-	//
-	Double_t nn = 0; for (Int_t i = 0; i < 3; i++)nn += n(i) * n(i);
-	Double_t nk = 0; for (Int_t i = 0; i < 3; i++)nk += n(i) * k(i);
-	Double_t kk = 0; for (Int_t i = 0; i < 3; i++)kk += k(i) * k(i);
-	Double_t discr = nn * kk - nk * nk;
-	TMatrixDSym H(2);
-	H(0, 0) = kk;
-	H(0, 1) = nk;
-	H(1, 0) = H(0, 1);
-	H(1, 1) = nn;
-	TVectorD c(2);
-	c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
-	c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
-	TVectorD smin = (H * c);
-	smin *= 1.0 / discr;
-	//
-	TVectorD X = x0 + smin(0) * n;
-	TVectorD Y = y0 + smin(1) * k;
-	//
-	// Higher order corrections
-	X(0) += -C1 * smin(0) * smin(0) * TMath::Sin(ph1);
-	X(1) +=  C1 * smin(0) * smin(0) * TMath::Cos(ph1);
-	Y(0) += -C2 * smin(1) * smin(1) * TMath::Sin(ph2);
-	Y(1) +=  C2 * smin(1) * smin(1) * TMath::Cos(ph2);
-	//
-	TVectorD Xavg = 0.5 * (X + Y);
-	//
-	//
-	return TMath::Sqrt(Xavg(0)*Xavg(0)+Xavg(1)*Xavg(1));
-}
-//
-// Starting radius determination
-Double_t VertexFit::StartRadius()
-{
-	//
-	// Maximum impact parameter
-	Double_t Rd = 0;
-	for (Int_t i = 0; i < fNtr; i++)
-	{
-		TVectorD par = *fPar[i];
-		Double_t Dabs = TMath::Abs(par(0));
-		if (Dabs > Rd)Rd = Dabs;
-	}
-	//-----------------------------
-	//
-	// Find track pair with phi difference closest to pi/2
-	Int_t isel = 0; Int_t jsel = 0;		// selected track indices
-	Double_t dSinMax = 0.0;				// Max phi difference
-	for (Int_t i = 0; i < fNtr - 1; i++)
-	{
-		TVectorD pari = *fPar[i];
-		Double_t phi1 = pari(1);
-
-		for (Int_t j = i + 1; j < fNtr; j++)
-		{
-			TVectorD parj = *fPar[j];
-			Double_t phi2 = parj(1);
-			Double_t Sindphi = TMath::Abs(TMath::Sin(phi2 - phi1));
-			if (Sindphi > dSinMax)
-			{
-				isel = i; jsel = j;
-				dSinMax = Sindphi;
-			}
-		}
-	}
-	//
-	//------------------------------------------
-	//
-	// Find radius of minimum distrance between tracks
-	TVectorD p1 = *fPar[isel];
-	TVectorD p2 = *fPar[jsel];
-	Double_t R = FastRv(p1, p2);
-	//
-	R = 0.9 * R + 0.1 * Rd;		// Protect for overshoot
-	//
-	return R;
-}
-//
-// Regularized symmetric matrix inversion
-//
-TMatrixDSym VertexFit::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;
-}
-//
-//
-//
-TMatrixD VertexFit::Fill_A(TVectorD par, Double_t phi)
-{
-	//
-	// Derivative of track 3D position vector with respect to track parameters at constant phase 
-	//
-	// par = vector of track parameters
-	// phi = phase
-	//
-	TMatrixD A(3, 5);
+//
+//
+TVectorD VertexFit::Fill_x0(TVectorD par)
+{
+	//
+	// Calculate track 3D position at R = |D| (minimum approach to z-axis)
+	//
+	TVectorD x0(3);
 	//
 	// Decode input arrays
@@ -341,38 +143,17 @@
 	Double_t ct = par(4);
 	//
-	// Fill derivative matrix dx/d alpha
-	// D
-	A(0, 0) = -TMath::Sin(p0);
-	A(1, 0) = TMath::Cos(p0);
-	A(2, 0) = 0.0;
-	// phi0
-	A(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
-	A(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
-	A(2, 1) = 0.0;
-	// C
-	A(0, 2) = -(TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C * C);
-	A(1, 2) = (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C * C);
-	A(2, 2) = -ct * phi / (2 * C * C);
-	// z0
-	A(0, 3) = 0.0;
-	A(1, 3) = 0.0;
-	A(2, 3) = 1.0;
-	// ct = lambda
-	A(0, 4) = 0.0;
-	A(1, 4) = 0.0;
-	A(2, 4) = phi / (2 * C);
-	//
-	return A;
-}
-//
-TVectorD VertexFit::Fill_a(TVectorD par, Double_t phi)
-{
-	//
-	// Derivative of track 3D position vector with respect to phase at constant track parameters
-	//
-	// par = vector of track parameters
-	// phi = phase
-	//
-	TVectorD a(3);
+	x0(0) = -D * TMath::Sin(p0);
+	x0(1) = D * TMath::Cos(p0);
+	x0(2) = z0;
+	//
+	return x0;
+}
+//
+TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)
+{
+	//
+	// Calculate track 3D position for a given phase, phi
+	//
+	TVectorD x(3);
 	//
 	// Decode input arrays
@@ -384,48 +165,4 @@
 	Double_t ct = par(4);
 	//
-	a(0) = TMath::Cos(phi + p0) / (2 * C);
-	a(1) = TMath::Sin(phi + p0) / (2 * C);
-	a(2) = ct / (2 * C);
-	//
-	return a;
-}
-//
-TVectorD VertexFit::Fill_x0(TVectorD par)
-{
-	//
-	// Calculate track 3D position at R = |D| (minimum approach to z-axis)
-	//
-	TVectorD x0(3);
-	//
-	// Decode input arrays
-	//
-	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);
-	//
-	x0(0) = -D * TMath::Sin(p0);
-	x0(1) = D * TMath::Cos(p0);
-	x0(2) = z0;
-	//
-	return x0;
-}
-//
-TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)
-{
-	//
-	// Calculate track 3D position for a given phase, phi
-	//
-	TVectorD x(3);
-	//
-	// Decode input arrays
-	//
-	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);
-	//
 	TVectorD x0 = Fill_x0(par);
 	x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
@@ -439,25 +176,28 @@
 {
 	//
-	// Get track parameters and their covariance
+	// Get track parameters, covariance and phase
+	Double_t fs = ffi[i];			// Get phase
 	TVectorD par = *fParNew[i];
 	TMatrixDSym Cov = *fCov[i];
 	//
 	// Fill all track related work arrays arrays
-	Double_t fs = ffi[i];						// Get phase
+	TMatrixD A = derXdPar(par, fs);		// A = dx/da = derivatives wrt track parameters
+	TMatrixDSym Winv = Cov;				
+	Winv.Similarity(A);			// W^-1 = A*C*A'
+
+	TMatrixD At(TMatrixD::kTransposed, A);		// A transposed
+	fAti.push_back(new TMatrixD(At));		// Store A'
+	fWinvi.push_back(new TMatrixDSym(Winv));	// Store W^-1 matrix
+	//
 	TVectorD xs = Fill_x(par, fs);
 	fx0i.push_back(new TVectorD(xs));			// Start helix position
-	//
-	TMatrixD A = Fill_A(par, fs);				// A = dx/da = derivatives wrt track parameters
-	TMatrixD At(TMatrixD::kTransposed, A);		// A transposed
-	fAti.push_back(new TMatrixD(At));			// Store A' 
-	TVectorD di = A * (par - *fPar[i]);		// x-shift
-	fdi.push_back(new TVectorD(di));			// Store x-shift
-	TMatrixDSym Winv = Cov.Similarity(A);		// W^-1 = A*C*A'
-	fWinvi.push_back(new TMatrixDSym(Winv));	// Store W^-1 matrix
+	// 
+	TVectorD di = A * (par - *fPar[i]);			// x-shift
+	fdi.push_back(new TVectorD(di));			// Store x-shift	
 	//
 	TMatrixDSym W = RegInv(Winv);				// W = (A*C*A')^-1
 	fWi.push_back(new TMatrixDSym(W));			// Store W matrix
 	//
-	TVectorD a = Fill_a(par, fs);				// a = dx/ds = derivatives wrt phase
+	TVectorD a = derXds(par, fs);				// a = dx/ds = derivatives wrt phase
 	fai.push_back(new TVectorD(a));				// Store a
 	//
@@ -473,4 +213,73 @@
 }
 //
+void VertexFit::VtxFitNoSteer()
+{
+	//
+	// Initialize
+	//
+	std::vector<TVectorD*> x0i;				// Tracks at ma
+	std::vector<TVectorD*> ni;				// Track derivative wrt phase
+	std::vector<TMatrixDSym*> Ci;			// Position error matrix at fixed phase
+	std::vector<TVectorD*> wi;				// Ci*ni
+	//
+	// Track loop
+	for (Int_t i = 0; i < fNtr; i++)
+	{
+		Double_t s = 0.;
+		TVectorD par = *fPar[i];
+		TMatrixDSym Cov = *fCov[i];
+		x0i.push_back(new TVectorD(Fill_x0(par)));
+		ni.push_back(new TVectorD(derXds(par, s)));
+		TMatrixD A = derXdPar(par, s);
+		Ci.push_back(new TMatrixDSym(Cov.Similarity(A)));
+		TMatrixDSym Cinv = RegInv(*Ci[i]);
+		wi.push_back(new TVectorD(Cinv * (*ni[i])));
+	}
+	//std::cout << "Vtx init completed. fNtr = "<<fNtr << std::endl;
+	//
+	// Get fit vertex
+	//
+	TMatrixDSym D(3); D.Zero();
+	TVectorD Dx(3); Dx.Zero();
+	for (Int_t i = 0; i < fNtr; i++)
+	{
+		TMatrixDSym Cinv = RegInv(*Ci[i]);
+		TMatrixDSym W(3);
+		W.Rank1Update(*wi[i], 1. / Ci[i]->Similarity(*wi[i]));
+		TMatrixDSym Dd = Cinv - W;
+		D += Dd;
+		Dx += Dd * (*x0i[i]);
+	}
+	if(fVtxCst){
+		D  += fCovCstInv;
+		Dx += fCovCstInv*fxCst;
+	}
+	fXv = RegInv(D) * Dx;
+	//std::cout << "Fast vertex (x, y, z) = "<<fXv(0)<<", "<<fXv(1)<<", "<<fXv(2) << std::endl;
+	//
+	// Get fit phases
+	//
+	for (Int_t i = 0; i < fNtr; i++){
+		Double_t si = Dot(*wi[i], fXv - (*x0i[i])) / Ci[i]->Similarity(*wi[i]);
+		ffi.push_back(si);
+		//TVectorD xvi = Fill_x(*fPar[i],si);
+		//std::cout << "Fast vertex "<<i<<": xvi = "<<xvi(0)<<", "<<xvi(1)<<", "<<xvi(2) 
+		//	<<", si = "<<si<< std::endl;
+	}
+	//
+	// Cleanup
+	for (Int_t i = 0; i < fNtr; i++)
+	{
+		x0i[i]->Clear();	delete x0i[i];
+		ni[i]->Clear();		delete ni[i];
+		Ci[i]->Clear();		delete Ci[i];
+		wi[i]->Clear();		delete wi[i];
+	}
+	x0i.clear();;
+	ni.clear();
+	Ci.clear(); 
+	wi.clear();
+}
+//
 void  VertexFit::VertexFitter()
 {
@@ -487,17 +296,7 @@
 	TMatrixDSym covX(3);
 	Double_t Chi2 = 0;
-	TVectorD x0 = fXv;	// If previous fit done
-	if (fRold < 0.0) {
-		// External constraint
-		if (fVtxCst) fRold = TMath::Sqrt(fxCst(0) * fxCst(0) + fxCst(1) * fxCst(1));
-		// No constraint
-		else for (Int_t i = 0; i < 3; i++)x0(i) = 1000.;	// Set to arbitrary large value 
-	}
-	//
-	// Starting vertex radius approximation
-	//
-	Double_t R = fRold;						// Use previous fit if available
-	if (R < 0.0) R = StartRadius();			// Rough vertex estimate
-	//std::cout << "Start radius: " << R << std::endl;
+	//
+	VtxFitNoSteer();	// Fast vertex finder on first pass (set ffi and fXv)
+	TVectorD x0 = fXv;
 	//
 	// Iteration properties
@@ -515,5 +314,5 @@
 		TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
 		covX.Zero();		// Reset vertex covariance
-		cterm.Zero();	// Reset constant term
+		cterm.Zero();		// Reset constant term
 		H.Zero();		// Reset H matrix
 		DW1D.Zero();
@@ -530,15 +329,4 @@
 			TVectorD par = *fPar[i];
 			TMatrixDSym Cov = *fCov[i];
-			//
-			// For first iteration only
-			Double_t fs;
-			if (Ntry <= 0)	// Initialize all phases on first pass
-			{
-				Double_t D = par(0);
-				Double_t C = par(2);
-				Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));
-				fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));
-				ffi.push_back(fs);
-			}
 			//
 			// Update track related arrays
@@ -555,10 +343,10 @@
 			// update constant term
 			TVectorD xs = *fx0i[i] - *fdi[i];
-			TVectorD xx0 = *fx0i[i];
-			/*
-			std::cout << "Iter. " << Ntry << ", trk " << i << ", x= "
-				<< xx0(0) << ", " << xx0(1) << ", " << xx0(2)<<
-				", ph0= "<<par(1)<< std::endl;
-			*/
+			//TVectorD xx0 = *fx0i[i];
+			
+			//std::cout << "Iter. " << Ntry << ", trk " << i << ", xs= "
+			//	<< xs(0) << ", " << xs(1) << ", " << xs(2)<<
+			//	", ph0= "<<par(1)<< std::endl;
+			
 			cterm += Ds * xs;
 		}				// End loop on tracks
@@ -584,8 +372,12 @@
 			TMatrixDSym Wm1 = *fWinvi[i];
 			fChi2List(i) = Wm1.Similarity(lambda);
+			if(fChi2List(i) < 0.0){
+				//std::cout<<"# "<<i<<", Chi2= "<<fChi2List(i)<<", Wm1:"<<std::endl; Wm1.Print();
+				//std::cout<<"Lambda= "<<std::endl; lambda.Print();
+			}
 			Chi2 += fChi2List(i);
 			TVectorD a = *fai[i];
-			TVectorD b = (*fWi[i]) * (x - (*fx0i[i]));
-			for (Int_t j = 0; j < 3; j++)ffi[i] += a(j) * b(j) / fa2i[i];
+			TVectorD b = (*fWi[i]) * (x - *fx0i[i] + *fdi[i]);
+			ffi[i] += Dot(a, b) / fa2i[i];
 			TVectorD newPar = *fPar[i] - ((*fCov[i]) * (*fAti[i])) * lambda;
 			fParNew[i] = new TVectorD(newPar);
@@ -610,5 +402,7 @@
 	//
 	fVtxDone = kTRUE;		// Set fit completion flag
-	fRold = TMath::Sqrt(fXv(0)*fXv(0) + fXv(1)*fXv(1));	// Store fit radius
+	fRold = TMath::Sqrt(fXv(0)*fXv(0) + fXv(1)*fXv(1));	// Store fit 
+	//std::cout << "Found vertex " << fXv(0) << ", " << fXv(1) << ", " << fXv(2) 
+	//	<< ", after "<<Ntry<<" iterations"<<std::endl;
 	//
 }
Index: external/TrackCovariance/VertexFit.h
===================================================================
--- external/TrackCovariance/VertexFit.h	(revision bd376e3a1d8131d6e838b21d8cd738f47b325aad)
+++ external/TrackCovariance/VertexFit.h	(revision b750b0a794794673ed5b2468f10e215307d06a20)
@@ -6,4 +6,5 @@
 #include <TVectorD.h>
 #include <TMatrixDSym.h>
+#include "TrkUtil.h"
 #include "ObsTrk.h"
 #include <vector>
@@ -12,5 +13,6 @@
 // Class for vertex fitting
 
-class VertexFit {
+class VertexFit: public TrkUtil
+{
 	//
 	// Vertex fitting with track parameters steering
@@ -21,5 +23,5 @@
 	//
 	// Inputs
-	Int_t fNtr;					// Number of tracks
+	Int_t fNtr;							// Number of tracks
 	std::vector<TVectorD*> fPar;		// Input parameter array
 	std::vector<TVectorD*> fParNew;		// Updated parameter array
@@ -27,11 +29,11 @@
 	std::vector<TMatrixDSym*> fCovNew;	// Updated parameter covariances
 	// Constraints
-	Bool_t fVtxCst;				// Vertex constraint flag
-	TVectorD fxCst;				// Constraint value
+	Bool_t fVtxCst;					// Vertex constraint flag
+	TVectorD fxCst;					// Constraint value
 	TMatrixDSym fCovCst;			// Constraint 
 	TMatrixDSym fCovCstInv;			// Inverse of constraint covariance
 	//
 	// Results
-	Bool_t fVtxDone;				// Flag vertex fit completed
+	Bool_t fVtxDone;			// Flag vertex fit completed
 	Double_t fRold;				// Current value of vertex radius
 	TVectorD fXv;				// Found vertex
@@ -52,15 +54,15 @@
 	//
 	// Service routines
-	//void InitWrkArrays();							// Initializations
 	void ResetWrkArrays();							// Clear work arrays
-	Double_t StartRadius();							// Starting vertex radius determination
-	Double_t FastRv(TVectorD p1, TVectorD p2);		// Fast vertex radius determination
-	TMatrixDSym RegInv(TMatrixDSym& Smat0);			// Regularized 3D matrix inversion 
-	TMatrixD Fill_A(TVectorD par, Double_t phi);		// Derivative of track position wrt track parameters
-	TVectorD Fill_a(TVectorD par, Double_t phi);		// Derivative of track position wrt track phase
+	//Double_t StartRadius();							// Starting vertex radius determination
+	//Double_t FastRv(TVectorD p1, TVectorD p2);		// Fast vertex radius determination
+	//TMatrixDSym RegInv(TMatrixDSym& Smat0);		// Regularized 3D matrix inversion 
+	//TMatrixD Fill_A(TVectorD par, Double_t phi);	// Derivative of track position wrt track parameters
+	//TVectorD Fill_a(TVectorD par, Double_t phi);	// Derivative of track position wrt track phase
 	TVectorD Fill_x0(TVectorD par);					// Track position at dma to z-axis
-	TVectorD Fill_x(TVectorD par, Double_t phi);		// Track position at given phase
+	TVectorD Fill_x(TVectorD par, Double_t phi);	// Track position at given phase
 	void UpdateTrkArrays(Int_t i);					// Fill track realted arrays
-	void VertexFitter();								// Vertex finder routine
+	void VtxFitNoSteer();							// Vertex fitter routine w/o parameter steering
+	void VertexFitter();							// Vertex fitter routine w/  parameter steering
 public:
 	//
