Index: examples/ExamplePVtxFind.C
===================================================================
--- examples/ExamplePVtxFind.C	(revision a47edcce9a940739aed295cc1c06c5d0214bd52a)
+++ examples/ExamplePVtxFind.C	(revision 46d34429082d61a6b55a5d706d0f7041cd5a0609)
@@ -51,7 +51,6 @@
 		// Load selected branches with data from specified event
 		treeReader->ReadEntry(entry);
-		Int_t Ntr = 0;	// # of starting tracks used in primary vertex
 		Int_t NtrG = branchTrack->GetEntries();
-		//std::cout << "Event opened containing " << NtrG << " tracks" << std::endl;
+		if(entry%500 ==0)std::cout << "Event "<<entry<<" opened containing " << NtrG << " tracks" << std::endl;
 		TVectorD** pr = new TVectorD * [NtrG];
 		TMatrixDSym** cv = new TMatrixDSym * [NtrG];
@@ -63,4 +62,7 @@
 		//
 		Double_t Nprim = 0.0;
+		Double_t xpv = 0.0;		// Init true primary vertex position
+		Double_t ypv = 0.0;
+		Double_t zpv = 0.0;
 		if (branchTrack->GetEntries() > 0)
 		{
@@ -80,16 +82,12 @@
 				//std::cout << "Got track parameters for track " << it << std::endl;
 				//
-				// Load tracks for vertex fit if impact parameters is not ridiculous
-				Double_t Dmax = 1.0;	// max is 1 mm
-				if (TMath::Abs(obsD0) < Dmax) {
-					Double_t oPar[5] = { obsD0, obsPhi, obsC, obsZ0, obsCtg };
-					TVectorD obsPar(5, oPar);	// Fill observed parameters
-					pr[Ntr] = new TVectorD(obsPar);
-					cv[Ntr] = new TMatrixDSym(trk->CovarianceMatrix());
-					Ntr++;
-					//std::cout << "Track loaded Ntr= " << Ntr << std::endl;
-				}
-				//
-				// Get associated generated particle
+				// Load all tracks for vertex fit 
+				Double_t oPar[5] = { obsD0, obsPhi, obsC, obsZ0, obsCtg };
+				TVectorD obsPar(5, oPar);	// Fill observed parameters
+				pr[it] = new TVectorD(obsPar);
+				cv[it] = new TMatrixDSym(trk->CovarianceMatrix());
+				//std::cout << "Track loaded it= " << it << std::endl;
+			//
+			// Find true primary vertex
 				GenParticle* gp = (GenParticle*)trk->Particle.GetObject();
 				//std::cout << "GenParticle pointer "<<gp << std::endl;
@@ -99,25 +97,83 @@
 				Double_t y = gp->Y;
 				Double_t z = gp->Z;
-				//std::cout << "Got position of origin " << std::endl;
-				//
-				// Count tracks originating from the primary vertex
-				if (x == 0.0 && y == 0.0) Nprim++;
+				Bool_t prim = kTRUE;	// Is primary?
+				Int_t mp = gp->M1;	// Mother
+				while (mp > 0) {
+					GenParticle* gm =
+						(GenParticle*)branchGenPart->At(mp);
+					Double_t xm = gm->X;
+					Double_t ym = gm->Y;
+					Double_t zm = gm->Z;
+					if (x != xm || y != ym || z != zm) {
+						prim = kFALSE;
+						break;
+					}
+					else mp = gm->M1;
+				}
+				if (prim) {		// It's a primary track
+					Nprim++;
+					xpv = x;	// Store true primary
+					ypv = y;
+					zpv = z;
+				}
 
 			}		// End loop on tracks
 		}
 		//
-		// Fit primary vertex
-		//
-		Int_t Nfound = Ntr;
-		//std::cout << "Found tracks "<<Nfound << std::endl;
-		Int_t MinTrk = 2;	// Minumum # tracks for vertex fit
-		Double_t MaxChi2 = 6.;
-		if (Ntr >= MinTrk) {
-			VertexFit* Vtx = new VertexFit(Ntr, pr, cv);
+		// Find primary vertex
+		//
+		//Beam constraint
+		TVectorD xpvc(3);
+		xpvc(0) = 1.0;
+		xpvc(1) = -2.0;
+		xpvc(2) = 10.0;
+		TMatrixDSym covpvc(3); covpvc.Zero();
+		covpvc(0, 0) = 0.0097 * 0.0097;
+		covpvc(1, 1) = 2.55e-05 * 2.55e-05;
+		covpvc(2, 2) = 0.64 * 0.64;
+		//
+		//
+		// Skim tracks
+		Int_t nSkim = 0;
+		Int_t* nSkimmed = new Int_t[NtrG];
+		VertexFit* Vskim = new VertexFit();
+		Vskim->AddVtxConstraint(xpvc, covpvc);
+		Double_t MaxChi2 = 2.0+3*sqrt(2.);
+		for (Int_t n = 0; n < NtrG; n++) {
+			Vskim->AddTrk(pr[n], cv[n]);
+			Double_t Chi2One = Vskim->GetVtxChi2();
+			//std::cout<<"Track "<<n<<", Chi2 = "<<Chi2One<<std::endl;
+			if (Chi2One < MaxChi2) {
+				nSkimmed[nSkim] = n;
+				//std::cout << "nSkimmed[" << nSkim << "] = " << n << std::endl;
+				nSkim++;
+			}
+			Vskim->RemoveTrk(0);
+		}
+		// Load tracks for primary fit
+		Int_t MinTrk = 1;	// Minumum # tracks for vertex fit
+		TVectorD** PrFit = new TVectorD * [nSkim];
+		TMatrixDSym** CvFit = new TMatrixDSym * [nSkim];
+		if (nSkim >= MinTrk) {
+			for (Int_t n = 0; n < nSkim; n++) {
+				PrFit[n] = new TVectorD(*pr[nSkimmed[n]]);
+				CvFit[n] = new TMatrixDSym(*cv[nSkimmed[n]]);
+				TVectorD test = *PrFit[n];
+				//std::cout << "Test *PrFit[" << n << "](0) = " << test(0) << std::endl;
+			}
+		}
+		delete[] nSkimmed;
+		delete Vskim;
+		Int_t Nfound = nSkim;
+		if(entry%500 ==0)std::cout << "Found tracks "<<Nfound << std::endl;
+		if (nSkim >= MinTrk) {
+			VertexFit* Vtx = new VertexFit(nSkim, PrFit, CvFit);
 			//std::cout << "Vertex fit created " << std::endl;
+			Vtx->AddVtxConstraint(xpvc, covpvc);
 			//
 			// Remove tracks with large chi2
+			Double_t MaxChi2Fit = 9.;
 			Bool_t Done = kFALSE;
-			while (!Done){
+			while (!Done) {
 				//std::cout << "After while " << std::endl;
 				// Find largest Chi2 contribution
@@ -130,6 +186,8 @@
 				Double_t Chi2Mx = Chi2L[iMax];
 				//std::cout << "Chi2Mx "<<Chi2Mx << std::endl;
-				if (Chi2Mx > MaxChi2 && Nfound > 2) {
-					Vtx->RemoveTrk(iMax); 
+				if (Chi2Mx > MaxChi2Fit && Nfound > 2) {
+					//std::cout << "Before remove.  Nfound = "<<Nfound << std::endl;
+					Vtx->RemoveTrk(iMax);
+					//std::cout << "After remove." << std::endl;
 					Nfound--;
 				}
@@ -145,5 +203,5 @@
 			//
 			// Require minimum number of tracks in vertex
-			Int_t Nmin = 4;
+			Int_t Nmin = 2;
 			if (Nfound >= Nmin) {
 				TVectorD xvtx = Vtx->GetVtx();
@@ -151,8 +209,8 @@
 				TMatrixDSym covX = Vtx->GetVtxCov();
 				Double_t Chi2 = Vtx->GetVtxChi2();
-				Double_t Ndof = 2 * (Double_t)Nfound - 3;
-				Double_t PullX = xvtx(0) / TMath::Sqrt(covX(0, 0));
-				Double_t PullY = xvtx(1) / TMath::Sqrt(covX(1, 1));
-				Double_t PullZ = xvtx(2) / TMath::Sqrt(covX(2, 2));
+				Double_t Ndof = 2 * (Double_t)Nfound;
+				Double_t PullX = (xvtx(0) - xpv) / TMath::Sqrt(covX(0, 0));
+				Double_t PullY = (xvtx(1) - ypv) / TMath::Sqrt(covX(1, 1));
+				Double_t PullZ = (xvtx(2) - zpv) / TMath::Sqrt(covX(2, 2));
 				//
 				// Fill histograms
@@ -173,8 +231,12 @@
 		//
 		// Cleanup
-		for (Int_t i = 0; i < Ntr; i++) delete pr[i];
-		for (Int_t i = 0; i < Ntr; i++) delete cv[i];
+		for (Int_t i = 0; i < NtrG; i++) delete pr[i];
+		for (Int_t i = 0; i < NtrG; i++) delete cv[i];
+		for (Int_t i = 0; i < nSkim; i++) delete PrFit[i];
+		for (Int_t i = 0; i < nSkim; i++) delete CvFit[i];
 		delete[] pr;
 		delete[] cv;
+		delete[] PrFit;
+		delete[] CvFit;
 	}
 	//
@@ -195,5 +257,5 @@
 	CnvN->cd(1);
 	hTrPrim->Draw();
-	CnvN-> cd(2);
+	CnvN->cd(2);
 	hTrFound->SetLineColor(kRed);
 	hTrFound->Draw();
Index: examples/ExamplePVtxFitEC.C
===================================================================
--- examples/ExamplePVtxFitEC.C	(revision 46d34429082d61a6b55a5d706d0f7041cd5a0609)
+++ examples/ExamplePVtxFitEC.C	(revision 46d34429082d61a6b55a5d706d0f7041cd5a0609)
@@ -0,0 +1,176 @@
+/*
+Example of using vertex fitter class to fit primary vertex
+assumed to be generated in (0,0,0)
+*/
+
+#ifdef __CLING__
+R__LOAD_LIBRARY(libDelphes)
+
+#include "classes/DelphesClasses.h"
+#include "external/ExRootAnalysis/ExRootTreeReader.h"
+#include "modules/TrackCovariance.h"
+#include "external/TrackCovariance/TrkUtil.h"
+#include "external/TrackCovariance/VertexFit.h"
+
+#endif
+
+
+
+//------------------------------------------------------------------------------
+
+void ExamplePVtxFitEC(const char* inputFile, Int_t Nevent = 5)
+{
+	// Create chain of root trees
+	TChain chain("Delphes");
+	chain.Add(inputFile);
+
+	// Create object of class ExRootTreeReader
+	ExRootTreeReader* treeReader = new ExRootTreeReader(&chain);
+	Long64_t numberOfEntries = treeReader->GetEntries();
+
+	// Get pointers to branches used in this analysis
+	TClonesArray* branchGenPart = treeReader->UseBranch("Particle");
+	TClonesArray* branchTrack = treeReader->UseBranch("Track");
+
+	// Book histograms
+	Int_t Nbin = 100;
+	TH1D* hXpull = new TH1D("hXpull", "Pull X vertex component", Nbin, -10., 10.);
+	TH1D* hYpull = new TH1D("hYpull", "Pull Y vertex component", Nbin, -10., 10.);
+	TH1D* hZpull = new TH1D("hZpull", "Pull Z vertex component", Nbin, -10., 10.);
+	TH1D* hChi2 = new TH1D("hChi2", "Vertex #chi^{2}/N_{dof}", Nbin, 0., 10.);
+	//
+	// Loop over all events
+	Int_t Nev = TMath::Min(Nevent, (Int_t)numberOfEntries);
+	for (Int_t entry = 0; entry < Nev; ++entry)
+	{
+		// Load selected branches with data from specified event
+		treeReader->ReadEntry(entry);
+		// 
+		Int_t Ntr = 0;	// # of tracks from primary vertex
+		Int_t NtrG = branchTrack->GetEntries();
+		TVectorD** pr = new TVectorD * [NtrG];
+		TMatrixDSym** cv = new TMatrixDSym * [NtrG];
+		//
+		// True vertex
+		Double_t xpv, ypv, zpv;
+		// If event contains at least 1 track
+		//
+		if (branchTrack->GetEntries() > 0)
+		{
+			// Loop on tracks
+			for (Int_t it = 0; it < branchTrack->GetEntries(); it++)
+			{
+				Track* trk = (Track*)branchTrack->At(it);
+				//
+				// Get associated generated particle
+				GenParticle* gp = (GenParticle*)trk->Particle.GetObject();
+				TVector3 xg(1.e-3*gp->X,1.e-3*gp->Y,1.e-3*gp->Z);
+				TVector3 pg(gp->Px,gp->Py,gp->Pz);
+				Double_t Q = (Double_t)gp->Charge;
+				Double_t Bz = 2.0;
+				TVectorD genParM =TrkUtil:: XPtoPar(xg, pg, Q, Bz);
+				TVectorD genPar = TrkUtil::ParToMm(genParM);
+				//
+				// Position of origin in mm
+				Double_t x = gp->X;
+				Double_t y = gp->Y;
+				Double_t z = gp->Z;
+				Bool_t prim = kTRUE;	// Is primary?
+				Int_t mp = gp->M1;	// Mother
+				while(mp>0){
+					GenParticle* gm = 
+					(GenParticle*)branchGenPart->At(mp);
+					Double_t xm = gm->X;
+                                	Double_t ym = gm->Y;
+                                	Double_t zm = gm->Z;
+					if(x!=xm || y!=ym || z!=zm){
+						prim = kFALSE;
+						break;
+					}else mp = gm->M1;
+				}
+				//
+				// group tracks originating from the primary vertex
+				if (prim)
+				{
+					//std::cout<<"Event: "<<entry<<", track: "<<it;
+					//std::cout<<", x = "<<x<<", y = "<<y<<", z = "<<z<<std::endl; 
+					xpv = x;
+					ypv = y;
+					zpv = z; 
+					//
+					// Reconstructed track parameters
+					Double_t obsD0 = trk->D0;
+					Double_t obsPhi = trk->Phi;
+					Double_t obsC = trk->C;
+					Double_t obsZ0 = trk->DZ;
+					//std::cout<<"Z0 track = "<< obsZ0
+					//<<", gen Z0 = "<<genPar(3)
+					//<<", gen cot = "<<genPar(4)<<std::endl;
+					Double_t obsCtg = trk->CtgTheta;
+					Double_t oPar[5] = { obsD0, obsPhi, obsC, obsZ0, obsCtg };
+					TVectorD obsPar(5, oPar);	// Fill observed parameters
+					//
+					pr[Ntr] = new TVectorD(obsPar);
+					//std::cout<<"Cov Matrix:"<<std::endl;
+					//trk->CovarianceMatrix().Print();
+					cv[Ntr] = new TMatrixDSym(trk->CovarianceMatrix());
+					Ntr++;
+				}
+			}		// End loop on tracks
+		}
+		//
+		// Fit primary vertex with beam constraint
+		//
+		//Beam constraint
+		TVectorD xpvc(3);
+		xpvc(0) = 1.0;
+		xpvc(1) = -2.0;
+		xpvc(2) = 10.0;
+		TMatrixDSym covpvc(3); covpvc.Zero();
+		covpvc(0, 0) = 0.0097 * 0.0097;
+		covpvc(1, 1) = 2.55e-05 * 2.55e-05;
+		covpvc(2, 2) = 0.64 * 0.64;
+		Int_t MinTrk = 2;	// Minumum # tracks for vertex fit
+		if (Ntr >= MinTrk) {
+			VertexFit* Vtx = new VertexFit(Ntr, pr, cv);
+			Vtx->AddVtxConstraint(xpvc, covpvc);
+			TVectorD xvtx = Vtx->GetVtx();
+			TMatrixDSym covX = Vtx->GetVtxCov();
+			Double_t Chi2 = Vtx->GetVtxChi2();
+			Double_t Ndof = 2 * (Double_t)Ntr;
+			Double_t PullX = (xvtx(0)-xpv) / TMath::Sqrt(covX(0, 0));
+			Double_t PullY = (xvtx(1)-ypv) / TMath::Sqrt(covX(1, 1));
+			Double_t PullZ = (xvtx(2)-zpv) / TMath::Sqrt(covX(2, 2));
+			//std::cout<<"**** True  vertex (x, y, z) "<<xpv<<", "
+			//<<ypv<<", "<<zpv<<std::endl;
+			//std::cout<<"**** Found vertex (x, y, z) "<<xvtx(0)<<", "
+                        //<<xvtx(1)<<", "<<xvtx(2)<<std::endl;
+			//
+			// Fill histograms
+			hXpull->Fill(PullX);
+			hYpull->Fill(PullY);
+			hZpull->Fill(PullZ);
+			hChi2->Fill(Chi2 / Ndof);
+		}
+
+		//std::cout << "Vertex chi2/Ndof = " << Chi2 / Ndof << std::endl;
+		//
+		// Cleanup
+		for (Int_t i = 0; i < Ntr; i++) delete pr[i];
+		for (Int_t i = 0; i < Ntr; i++) delete cv[i];
+		delete[] pr;
+		delete[] cv;
+	}
+	//
+	// Show resulting histograms
+	//
+	TCanvas* Cnv = new TCanvas("Cnv", "Delphes primary vertex pulls", 50, 50, 900, 500);
+	Cnv->Divide(2, 2);
+	Cnv->cd(1); gPad->SetLogy(1); gStyle->SetOptFit(1111);
+	hXpull->Fit("gaus"); hXpull->Draw();
+	Cnv->cd(2); gPad->SetLogy(1); gStyle->SetOptFit(1111);
+	hYpull->Fit("gaus"); hYpull->Draw();
+	Cnv->cd(3); gPad->SetLogy(1); gStyle->SetOptFit(1111);
+	hZpull->Fit("gaus"); hZpull->Draw();
+	Cnv->cd(4); hChi2->Draw();
+}
