diff --git a/config/TMS_Default_Config.toml b/config/TMS_Default_Config.toml
index 9b2f147..baf9c69 100644
--- a/config/TMS_Default_Config.toml
+++ b/config/TMS_Default_Config.toml
@@ -46,7 +46,7 @@
     # dist = sqrt(xplanes*xplanes + zplanes*zplanes)
     MinDist = 4.0
     # Do we run DBSCAN clustering before the Hough transform and then run Hough on each cluster?
-    FirstCluster = true #false
+    FirstCluster = false
     # Do we merge adjacent tracks afterwards?
     MergeTracks = true
     # Do we run AStar afterwards on start and end points
@@ -81,7 +81,7 @@
     # Y difference for expectation of reconstruction from UV and from X hit [mm]
     YDifference = 3000.0
     # Distance from start to calculate track direction for [Number of planes]. Track matching done in plane pairs -> do not set to 1
-    DirectionDistance = 10
+    DirectionDistance = 14
 
   [Recon.TrackSmoothing]
     UseTrackSmoothing = true
@@ -107,7 +107,7 @@
     IsGreedy = false
 
   [Recon.Kalman]
-    Run = true # Whether we run the Kalman filter or not
+    Run = true #false # Whether we run the Kalman filter or not
 
   [Recon.Stopping]
     nLastHits = 5 # The number of hits at the end of track to look at for stopping
diff --git a/src/TMS_Reco.cpp b/src/TMS_Reco.cpp
index 5321766..a5f7fab 100644
--- a/src/TMS_Reco.cpp
+++ b/src/TMS_Reco.cpp
@@ -1663,21 +1663,23 @@ std::vector<TMS_Track> TMS_TrackFinder::TrackMatching3D() {
 #ifdef DEBUG
             std::cout << "Added TrackEnergyDeposit: " << aTrack.EnergyDeposit << std::endl;
 #endif
-//            // Track Direction
-//            if (TMS_Manager::GetInstance().Get_Reco_TRACKMATCH_DirectionDistance() >= aTrack.Hits.size()) {
-//              aTrack.Direction[0] = aTrack.Start[0] - aTrack.End[0];
-//              aTrack.Direction[1] = aTrack.Start[1] - aTrack.End[1];
-//              aTrack.Direction[2] = aTrack.Start[2] - aTrack.End[2];
-//            } else {
-//              aTrack.Direction[0] = aTrack.Start[0] - aTrack.Hits[TMS_Manager::GetInstance().Get_Reco_TRACKMATCH_DirectionDistance()].GetRecoX();
-//              aTrack.Direction[1] = aTrack.Start[1] - aTrack.Hits[TMS_Manager::GetInstance().Get_Reco_TRACKMATCH_DirectionDistance()].GetRecoY();
-//              aTrack.Direction[2] = aTrack.Start[2] - aTrack.Hits[TMS_Manager::GetInstance().Get_Reco_TRACKMATCH_DirectionDistance()].GetZ();    
-//            }
+            // Track Direction
+            if (TMS_Manager::GetInstance().Get_Reco_TRACKMATCH_DirectionDistance() >= aTrack.Hits.size()) {
+              double direction_x = aTrack.Start[0] - aTrack.End[0];
+              double direction_y = aTrack.Start[1] - aTrack.End[1];
+              double direction_z = aTrack.Start[2] - aTrack.End[2];
+              aTrack.SetStartDirection(direction_x, direction_y, direction_z);
+            } else {
+              double direction_x = aTrack.Start[0] - aTrack.Hits[TMS_Manager::GetInstance().Get_Reco_TRACKMATCH_DirectionDistance()].GetRecoX();
+              double direction_y = aTrack.Start[1] - aTrack.Hits[TMS_Manager::GetInstance().Get_Reco_TRACKMATCH_DirectionDistance()].GetRecoY();
+              double direction_z = aTrack.Start[2] - aTrack.Hits[TMS_Manager::GetInstance().Get_Reco_TRACKMATCH_DirectionDistance()].GetZ();
+              aTrack.SetStartDirection(direction_x, direction_y, direction_z);
+            }
 #ifdef DEBUG          
             std::cout << "Start: " << aTrack.Start[0] << " | " << aTrack.Start[1] << " | " << aTrack.Start[2] << std::endl;
             std::cout << "End: " << aTrack.End[0] << " | " << aTrack.End[1] << " | " << aTrack.End[2] << std::endl;
-            std::cout << "Added Direction: " << aTrack.Direction[0] << " | " << aTrack.Direction[1] << " | " << aTrack.Direction[2] << std::endl;
-#endif          
+            std::cout << "Added Direction: " << aTrack.StartDirection[0] << " | " << aTrack.StartDirection[1] << " | " << aTrack.StartDirection[2] << std::endl;
+#endif       
   
             returned.push_back(aTrack);
           }
diff --git a/src/TMS_Track.cpp b/src/TMS_Track.cpp
index 9409e75..5ae8c11 100644
--- a/src/TMS_Track.cpp
+++ b/src/TMS_Track.cpp
@@ -7,6 +7,25 @@ void TMS_Track::Print()
   //0x90; // TODO: add a function here
 };
 
+
+// Set the start direction of the track object, normalised so magnitude == 1
+void TMS_Track::SetStartDirection(double ax, double ay, double az) {
+  double mag = sqrt(ax*ax + ay*ay + az*az);
+ 
+  StartDirection[0] = ax/mag;
+  StartDirection[1] = ay/mag;
+  StartDirection[2] = az/mag;
+};
+
+// Set the end direction of the track object, normalised so magnitude == 1
+void TMS_Track::SetEndDirection(double ax, double ay, double az) {
+  double mag = sqrt(ax*ax + ay*ay + az*az);
+
+  EndDirection[0] = ax/mag;
+  EndDirection[1] = ay/mag;
+  EndDirection[2] = az/mag;
+};
+
 std::vector<size_t> TMS_Track::findYTransitionPoints() {
   // Finds and corrects the reco y positions for all points
   // where U and V transition to another y value
@@ -236,14 +255,3 @@ void TMS_Track::ApplyTrackSmoothing() {
 }
 
 
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/TMS_Track.h b/src/TMS_Track.h
index 73c25ac..7e8c935 100644
--- a/src/TMS_Track.h
+++ b/src/TMS_Track.h
@@ -40,8 +40,8 @@ class TMS_Track {
     void SetMomentum      (double val) {Momentum      = val;};
 
     // Set direction unit vectors from only x and y slope
-    void SetStartDirection(double ax, double ay, double az) {StartDirection[0]=ax; StartDirection[1]=ay; StartDirection[2]=az;};
-    void SetEndDirection  (double ax, double ay, double az) {EndDirection[0]=ax;   EndDirection[1]=ay;   EndDirection[2]=az;};
+    void SetStartDirection(double ax, double ay, double az);// {StartDirection[0]=ax; StartDirection[1]=ay; StartDirection[2]=az;};
+    void SetEndDirection  (double ax, double ay, double az);// {EndDirection[0]=ax;   EndDirection[1]=ay;   EndDirection[2]=az;};
 
     // Set position unit vectors
     void SetStartPosition(double ax, double ay, double az) {Start[0]=ax; Start[1]=ay; Start[2]=az;};