diff --git a/Tracking/Acts/FaserActsKalmanFilter/CMakeLists.txt b/Tracking/Acts/FaserActsKalmanFilter/CMakeLists.txt
index 66b2d2037c1bb60e3051d12c334c2275ae47e2b5..54c4bfd2d9aea06354610daec9a2006df5ac29a8 100755
--- a/Tracking/Acts/FaserActsKalmanFilter/CMakeLists.txt
+++ b/Tracking/Acts/FaserActsKalmanFilter/CMakeLists.txt
@@ -30,6 +30,8 @@ atlas_add_component(FaserActsKalmanFilter
     FaserActsKalmanFilter/ITrackFinderTool.h
     FaserActsKalmanFilter/TruthTrackFinderTool.h
     FaserActsKalmanFilter/Measurement.h
+    FaserActsKalmanFilter/RootTrajectoryStatesWriterTool.h
+    FaserActsKalmanFilter/SegmentFitClusterTrackFinderTool.h
     FaserActsKalmanFilter/SegmentFitTrackFinderTool.h
     FaserActsKalmanFilter/SimWriterTool.h
     FaserActsKalmanFilter/SPSeedBasedInitialParameterTool.h
@@ -40,6 +42,8 @@ atlas_add_component(FaserActsKalmanFilter
     FaserActsKalmanFilter/TruthSeededTrackFinderTool.h
     src/CombinatorialKalmbanFilterAlg.cxx
     src/FaserActsKalmanFilterAlg.cxx
+    src/RootTrajectoryStatesWriterTool.cxx
+    src/SegmentFitClusterTrackFinderTool.cxx
     src/SegmentFitTrackFinderTool.cxx
     src/SimWriterTool.cxx
     src/SPSeedBasedInitialParameterTool.cxx
diff --git a/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/FaserActsKalmanFilterAlg.h b/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/FaserActsKalmanFilterAlg.h
index bd8ec6e86e6ca2f633f5b6025c945bf217e833de..29c847ebc3d5e6384e2d362adc5f09da216e91a9 100755
--- a/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/FaserActsKalmanFilterAlg.h
+++ b/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/FaserActsKalmanFilterAlg.h
@@ -50,6 +50,7 @@
 #include "FaserActsKalmanFilter/Measurement.h"
 #include "FaserActsKalmanFilter/ITrackFinderTool.h"
 #include "FaserActsKalmanFilter/ProtoTrackWriterTool.h"
+#include "FaserActsKalmanFilter/RootTrajectoryStatesWriterTool.h"
 
 // STL
 #include <memory>
@@ -84,6 +85,7 @@ public:
   StatusCode execute() override;
   StatusCode finalize() override;
 
+  using IndexedParams = std::unordered_map<size_t, Acts::BoundTrackParameters>;
   using TrackFitterOptions =
     Acts::KalmanFitterOptions<MeasurementCalibrator, Acts::VoidOutlierFinder,
                               Acts::VoidReverseFilteringLogic>;
@@ -120,6 +122,7 @@ private:
   ToolHandle<ITrackFinderTool> m_trackFinderTool {this, "TrackFinderTool", "TruthTrackFinderTool"};
   ToolHandle<IFaserActsTrackingGeometryTool> m_trackingGeometryTool {this, "TrackingGeometryTool", "FaserActsTrackingGeometryTool"};
   ToolHandle<TrajectoryWriterTool> m_trajectoryWriterTool {this, "OutputTool", "TrajectoryWriterTool"};
+  ToolHandle<RootTrajectoryStatesWriterTool> m_trajectoryStatesWriterTool {this, "RootTrajectoryStatesWriterTool", "RootTrajectoryStatesWriterTool"};
   ToolHandle<ProtoTrackWriterTool> m_protoTrackWriterTool {this, "ProtoTrackWriterTool", "ProtoTrackWriterTool"};
 
   SG::ReadHandleKey<McEventCollection> m_mcEventKey { this, "McEventCollection", "BeamTruthEvent" };
diff --git a/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/FaserActsRecMultiTrajectory.h b/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/FaserActsRecMultiTrajectory.h
index 35baa239600d5f273dd91c2145eecfd8e1c8b52a..06924bed8e4adbe304697a4f71019d9134be72f8 100644
--- a/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/FaserActsRecMultiTrajectory.h
+++ b/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/FaserActsRecMultiTrajectory.h
@@ -6,82 +6,88 @@
 
 #pragma once
 
-#include <unordered_map>
-#include <utility>
-
-// ACTS
 #include "Acts/EventData/MultiTrajectory.hpp"
 #include "Acts/EventData/TrackParameters.hpp"
-
-// PACKAGE
 #include "FaserActsKalmanFilter/IndexSourceLink.h"
+#include <unordered_map>
+#include <utility>
 
-using IndexedParams = std::unordered_map<size_t, Acts::BoundTrackParameters>;
-
-struct FaserActsRecMultiTrajectory
-{
+/// Store reconstructed trajectories from track finding/fitting.
+///
+/// It contains a MultiTrajectory with a vector of entry indices for
+/// individual trajectories, and a map of fitted parameters indexed by the
+/// entry index. In case of track fitting, there is at most one trajectory
+/// in the MultiTrajectory; In case of track finding, there could be
+/// multiple trajectories in the MultiTrajectory.
+struct FaserActsRecMultiTrajectory {
  public:
-  FaserActsRecMultiTrajectory() = default;
+  /// (Reconstructed) trajectory with multiple states.
+  using MultiTrajectory = ::Acts::MultiTrajectory<IndexSourceLink>;
+  /// Fitted parameters identified by indices in the multi trajectory.
+  using IndexedParams = std::unordered_map<size_t, Acts::BoundTrackParameters>;
 
-  FaserActsRecMultiTrajectory(const Acts::MultiTrajectory<IndexSourceLink>& multiTraj,
-                     const std::vector<size_t>& tTips,
-                     const IndexedParams& parameters)
+  /// Default construct an empty object. Required for container compatibility
+  /// and to signal an error.
+  FaserActsRecMultiTrajectory() = default;
+  /// Construct from fitted multi trajectory and parameters.
+  ///
+  /// @param multiTraj The multi trajectory
+  /// @param tTips Tip indices that identify valid trajectories
+  /// @param parameters Fitted track parameters indexed by trajectory index
+  FaserActsRecMultiTrajectory(const MultiTrajectory& multiTraj,
+                              const std::vector<size_t>& tTips,
+                              const IndexedParams& parameters)
       : m_multiTrajectory(multiTraj),
         m_trackTips(tTips),
         m_trackParameters(parameters) {}
 
-  FaserActsRecMultiTrajectory(const FaserActsRecMultiTrajectory& rhs)
-      : m_multiTrajectory(rhs.m_multiTrajectory),
-        m_trackTips(rhs.m_trackTips),
-        m_trackParameters(rhs.m_trackParameters) {}
-
-  FaserActsRecMultiTrajectory(FaserActsRecMultiTrajectory&& rhs)
-      : m_multiTrajectory(std::move(rhs.m_multiTrajectory)),
-        m_trackTips(std::move(rhs.m_trackTips)),
-        m_trackParameters(std::move(rhs.m_trackParameters)) {}
-
-  ~FaserActsRecMultiTrajectory() = default;
-
-  FaserActsRecMultiTrajectory& operator=(const FaserActsRecMultiTrajectory& rhs) {
-    m_multiTrajectory = rhs.m_multiTrajectory;
-    m_trackTips = rhs.m_trackTips;
-    m_trackParameters = rhs.m_trackParameters;
-    return *this;
-  }
+  /// Return true if there exists no valid trajectory.
+  bool empty() const { return m_trackTips.empty(); }
 
-  FaserActsRecMultiTrajectory& operator=(FaserActsRecMultiTrajectory&& rhs) {
-    m_multiTrajectory = std::move(rhs.m_multiTrajectory);
-    m_trackTips = std::move(rhs.m_trackTips);
-    m_trackParameters = std::move(rhs.m_trackParameters);
-    return *this;
-  }
+  /// Access the underlying multi trajectory.
+  const MultiTrajectory& multiTrajectory() const { return m_multiTrajectory; }
 
-  bool hasTrajectory(const size_t& entryIndex) const {
-    return std::count(m_trackTips.begin(), m_trackTips.end(), entryIndex) > 0;
-  }
+  /// Access the tip indices that identify valid trajectories.
+  const std::vector<size_t>& tips() const { return m_trackTips; }
 
-  bool hasTrackParameters(const size_t& entryIndex) const {
-    return m_trackParameters.count(entryIndex) > 0;
+  /// Check if a trajectory exists for the given index.
+  ///
+  /// @param entryIndex The trajectory entry index
+  /// @return Whether there is trajectory with provided entry index
+  bool hasTrajectory(size_t entryIndex) const {
+    return (0 < std::count(m_trackTips.begin(), m_trackTips.end(), entryIndex));
   }
 
-  std::pair<std::vector<size_t>, Acts::MultiTrajectory<IndexSourceLink>>
-  trajectory() const {
-    return std::make_pair(m_trackTips, m_multiTrajectory);
+  /// Check if fitted track parameters exists for the given index.
+  ///
+  /// @param entryIndex The trajectory entry index
+  /// @return Whether having fitted track parameters or not
+  bool hasTrackParameters(size_t entryIndex) const {
+    return (0 < m_trackParameters.count(entryIndex));
   }
 
-  const Acts::BoundTrackParameters& trackParameters(const size_t& entryIndex) const {
+  /// Access the fitted track parameters for the given index.
+  ///
+  /// @param entryIndex The trajectory entry index
+  /// @return The fitted track parameters of the trajectory
+  const Acts::BoundTrackParameters& trackParameters(size_t entryIndex) const {
     auto it = m_trackParameters.find(entryIndex);
-    if (it != m_trackParameters.end()) {
-      return it->second;
-    } else {
+    if (it == m_trackParameters.end()) {
       throw std::runtime_error(
           "No fitted track parameters for trajectory with entry index = " +
           std::to_string(entryIndex));
     }
+    return it->second;
   }
 
- private:
-  Acts::MultiTrajectory<IndexSourceLink> m_multiTrajectory;
+private:
+  // The multiTrajectory
+  MultiTrajectory m_multiTrajectory;
+  // The entry indices of trajectories stored in multiTrajectory
   std::vector<size_t> m_trackTips = {};
+  // The fitted parameters at the provided surface for individual trajectories
   IndexedParams m_trackParameters = {};
 };
+
+/// Container for multiple trajectories.
+using TrajectoriesContainer = std::vector<FaserActsRecMultiTrajectory>;
diff --git a/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/SegmentFitClusterTrackFinderTool.h b/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/SegmentFitClusterTrackFinderTool.h
new file mode 100644
index 0000000000000000000000000000000000000000..7fa09656f6079dedc8f811144effc08161e62ecc
--- /dev/null
+++ b/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/SegmentFitClusterTrackFinderTool.h
@@ -0,0 +1,91 @@
+#ifndef FASERACTSKALMANFILTER_SEGMENTFITCLUSTERTRACKFINDERTOOL_H
+#define FASERACTSKALMANFILTER_SEGMENTFITCLUSTERTRACKFINDERTOOL_H
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "Gaudi/Property.h"
+#include "GaudiKernel/EventContext.h"
+#include "GaudiKernel/IInterface.h"
+#include "GaudiKernel/StatusCode.h"
+#include "StoreGate/ReadHandleKey.h"
+#include <string>
+#include <vector>
+
+#include "TrackerSpacePoint/FaserSCT_SpacePointContainer.h"
+#include "FaserActsGeometryInterfaces/IFaserActsTrackingGeometryTool.h"
+#include "FaserActsKalmanFilter/ITrackFinderTool.h"
+#include "TrackerSpacePoint/FaserSCT_SpacePoint.h"
+#include "TrkTrack/TrackCollection.h"
+
+class FaserSCT_ID;
+namespace TrackerDD {
+class SCT_DetectorManager;
+}
+
+
+class SegmentFitClusterTrackFinderTool : public extends<AthAlgTool, ITrackFinderTool> {
+public:
+  SegmentFitClusterTrackFinderTool(const std::string& type, const std::string& name, const IInterface* parent);
+  virtual ~SegmentFitClusterTrackFinderTool() = default;
+  virtual StatusCode initialize() override;
+  virtual StatusCode finalize() override;
+  virtual StatusCode run() override;
+
+  virtual const std::shared_ptr<const Acts::CurvilinearTrackParameters> initialTrackParameters() const override;
+  virtual const std::shared_ptr<const Acts::Surface> initialSurface() const override;
+  virtual const std::shared_ptr<std::vector<IndexSourceLink>> sourceLinks() const override;
+  virtual const std::shared_ptr<std::vector<Measurement>> measurements() const override;
+  virtual const std::shared_ptr<std::vector<Tracker::FaserSCT_SpacePoint>> spacePoints() const override;
+
+private:
+  std::shared_ptr<const Acts::CurvilinearTrackParameters> m_initialTrackParameters;
+  std::shared_ptr<const Acts::Surface> m_initialSurface;
+  std::shared_ptr<std::vector<IndexSourceLink>> m_sourceLinks {};
+  std::shared_ptr<std::vector<Measurement>> m_measurements {};
+  std::shared_ptr<std::vector<Tracker::FaserSCT_SpacePoint>> m_spacePoints {};
+
+  const FaserSCT_ID* m_idHelper {nullptr};
+  const TrackerDD::SCT_DetectorManager* m_detManager {nullptr};
+  static double momentum(const std::map<int, Amg::Vector3D>& pos, double B=0.57) ;
+
+  ToolHandle<IFaserActsTrackingGeometryTool> m_trackingGeometryTool {
+      this, "TrackingGeometryTool", "FaserActsTrackingGeometryTool"};
+  SG::ReadHandleKey<TrackCollection> m_trackCollection {
+      this, "TrackCollection", "SegmentFit", "Input track collection name" };
+  SG::ReadHandleKey<FaserSCT_SpacePointContainer> m_spacePointContainerKey {
+      this, "SpacePoints", "SCT_SpacePointContainer", "Space point container"};
+
+  // covariance of the initial parameters
+  Gaudi::Property<double> m_covLoc0 {this, "covLoc0", 1};
+  Gaudi::Property<double> m_covLoc1 {this, "covLoc1", 1};
+  Gaudi::Property<double> m_covPhi {this, "covPhi", 1};
+  Gaudi::Property<double> m_covTheta {this, "covTheta", 1};
+  Gaudi::Property<double> m_covQOverP {this, "covQOverP", 1};
+  Gaudi::Property<double> m_covTime {this, "covTime", 1};
+};
+
+
+inline const std::shared_ptr<const Acts::CurvilinearTrackParameters>
+SegmentFitClusterTrackFinderTool::initialTrackParameters() const {
+  return m_initialTrackParameters;
+}
+
+inline const std::shared_ptr<const Acts::Surface>
+SegmentFitClusterTrackFinderTool::initialSurface() const {
+  return m_initialSurface;
+}
+
+inline const std::shared_ptr<std::vector<IndexSourceLink>>
+SegmentFitClusterTrackFinderTool::sourceLinks() const {
+  return m_sourceLinks;
+}
+
+inline const std::shared_ptr<std::vector<Measurement>>
+SegmentFitClusterTrackFinderTool::measurements() const {
+  return m_measurements;
+}
+
+inline const std::shared_ptr<std::vector<Tracker::FaserSCT_SpacePoint>>
+SegmentFitClusterTrackFinderTool::spacePoints() const {
+  return m_spacePoints;
+}
+#endif // FASERACTSKALMANFILTER_SEGMENTFITCLUSTERTRACKFINDERTOOL_H
diff --git a/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/TrajectoryWriterTool.h b/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/TrajectoryWriterTool.h
index 2a11334f4d9dfa6f690f45e8faeeab0d04629189..0643ad9ce973f65453e72063701befaa3b2789ec 100644
--- a/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/TrajectoryWriterTool.h
+++ b/Tracking/Acts/FaserActsKalmanFilter/FaserActsKalmanFilter/TrajectoryWriterTool.h
@@ -7,7 +7,7 @@
 
 class TFile;
 class TTree;
-class FaserActsRecMultiTrajectory;
+struct FaserActsRecMultiTrajectory;
 using TrajectoriesContainer = std::vector<FaserActsRecMultiTrajectory>;
 
 class TrajectoryWriterTool : public AthAlgTool {
@@ -60,6 +60,10 @@ class TrajectoryWriterTool : public AthAlgTool {
   mutable std::vector<float> m_x_hit;
   mutable std::vector<float> m_y_hit;
   mutable std::vector<float> m_z_hit;
+  mutable std::vector<float> m_meas_eLOC0;
+  mutable std::vector<float> m_meas_eLOC1;
+  mutable std::vector<float> m_meas_cov_eLOC0;
+  mutable std::vector<float> m_meas_cov_eLOC1;
   mutable std::vector<float> m_res_x_hit;
   mutable std::vector<float> m_res_y_hit;
   mutable std::vector<float> m_err_x_hit;
diff --git a/Tracking/Acts/FaserActsKalmanFilter/python/FaserActsKalmanFilterConfig.py b/Tracking/Acts/FaserActsKalmanFilter/python/FaserActsKalmanFilterConfig.py
index 9c3ddda06b4177c30d78104968e8aa45ecd84832..7fcfb466797651a2923ea35828769ab5cf74be94 100644
--- a/Tracking/Acts/FaserActsKalmanFilter/python/FaserActsKalmanFilterConfig.py
+++ b/Tracking/Acts/FaserActsKalmanFilter/python/FaserActsKalmanFilterConfig.py
@@ -79,19 +79,18 @@ def FaserActsKalmanFilter_OutputAODCfg(flags):
 
 def FaserActsKalmanFilterCfg(flags, **kwargs):
     acc = ComponentAccumulator()
-    trajectory_writer_tool = CompFactory.TrajectoryWriterTool()
-    trajectory_writer_tool.FilePath = "KalmanFilterTrajectories.root"
-    track_finder_tool = CompFactory.SegmentFitTrackFinderTool()
+    track_finder_tool = CompFactory.SegmentFitClusterTrackFinderTool()
+    # track_finder_tool = CompFactory.SegmentFitTrackFinderTool()
     # track_finder_tool.covLoc0 = 1e-3
     # track_finder_tool.covLoc1 = 3.61e-4
     # track_finder_tool.covPhi = 1.1e-3
     # track_finder_tool.covTheta = 8.4e-8
     # track_finder_tool.covQOverP = 1e-3
-    track_finder_tool.covLoc0 = 1e-4
-    track_finder_tool.covLoc1 = 1e-4
-    track_finder_tool.covPhi = 1e-4
-    track_finder_tool.covTheta = 1e-4
-    track_finder_tool.covQOverP = 1e-4
+    # track_finder_tool.covLoc0 = 1e-4
+    # track_finder_tool.covLoc1 = 1e-4
+    # track_finder_tool.covPhi = 1e-4
+    # track_finder_tool.covTheta = 1e-4
+    # track_finder_tool.covQOverP = 1e-4
 
     # track_finder_tool = CompFactory.TruthSeededTrackFinderTool()
     # track_finder_tool.useTrueInitialParameters = False
@@ -133,10 +132,15 @@ def FaserActsKalmanFilterCfg(flags, **kwargs):
     # track_finder_tool.covPhi = 1e-3
     # track_finder_tool.covTheta = 1e-3
     # track_finder_tool.covQOverP = 1e-3
+    trajectory_writer_tool = CompFactory.TrajectoryWriterTool()
+    trajectory_writer_tool.FilePath = "KalmanFilterTrajectories.root"
+    # trajectory_states_writer_tool = CompFactory.RootTrajectoryStatesWriterTool()
+    # trajectory_states_writer_tool.FilePath = "TrajectoryStates.root"
     kalman_filter = CompFactory.FaserActsKalmanFilterAlg(**kwargs)
+    # kalman_filter.RootTrajectoryStatesWriterTool = trajectory_states_writer_tool
     kalman_filter.OutputTool = trajectory_writer_tool
     kalman_filter.TrackFinderTool = track_finder_tool
-    kalman_filter.ActsLogging = "INFO"
+    kalman_filter.ActsLogging = "VERBOSE"
     acc.addEventAlgo(kalman_filter)
     acc.merge(FaserActsKalmanFilter_OutputCfg(flags))
     acc.merge(FaserActsKalmanFilter_OutputAODCfg(flags))
diff --git a/Tracking/Acts/FaserActsKalmanFilter/src/FaserActsKalmanFilterAlg.cxx b/Tracking/Acts/FaserActsKalmanFilter/src/FaserActsKalmanFilterAlg.cxx
index a31bf4ba89722e01ab283a10ff3fec3ddb19aaa0..a8a9147d1c9f1653116217851292d98a3094fd0e 100755
--- a/Tracking/Acts/FaserActsKalmanFilter/src/FaserActsKalmanFilterAlg.cxx
+++ b/Tracking/Acts/FaserActsKalmanFilter/src/FaserActsKalmanFilterAlg.cxx
@@ -80,6 +80,7 @@ StatusCode FaserActsKalmanFilterAlg::initialize() {
   ATH_CHECK(m_trackingGeometryTool.retrieve());
   ATH_CHECK(m_trackFinderTool.retrieve());
   ATH_CHECK(m_trajectoryWriterTool.retrieve());
+  ATH_CHECK(m_trajectoryStatesWriterTool.retrieve());
   ATH_CHECK(m_protoTrackWriterTool.retrieve());
   ATH_CHECK(m_trackCollection.initialize());
   ATH_CHECK(detStore()->retrieve(m_idHelper,"FaserSCT_ID"));
@@ -102,7 +103,7 @@ StatusCode FaserActsKalmanFilterAlg::execute() {
   //make TrackCollection
   ATH_CHECK(m_trackCollection.initialize());
   SG::WriteHandle<TrackCollection> trackContainer{m_trackCollection,ctx};
-  std::unique_ptr<TrackCollection> outputTracks = std::make_unique<TrackCollection>();
+//  std::unique_ptr<TrackCollection> outputTracks = std::make_unique<TrackCollection>();
 
   std::shared_ptr<const Acts::TrackingGeometry> trackingGeometry
       = m_trackingGeometryTool->trackingGeometry();
@@ -124,6 +125,7 @@ StatusCode FaserActsKalmanFilterAlg::execute() {
   // FIXME this will only work for SegmentFitTrackFinder
   std::shared_ptr<std::vector<Tracker::FaserSCT_SpacePoint>> spacePoints =
       m_trackFinderTool->spacePoints();
+
   ATH_CHECK(m_protoTrackWriterTool->write(initialTrackParameters, measurements, geoctx));
 
   TrajectoryContainer trajectories;
@@ -150,10 +152,10 @@ StatusCode FaserActsKalmanFilterAlg::execute() {
   if (result.ok()) {
     // Get the fit output object
     const auto& fitOutput = result.value();
-    std::unique_ptr<Trk::Track> track = makeTrack(geoctx, result, *spacePoints);
-    if (track) {
-      outputTracks->push_back(std::move(track));
-    }
+    // std::unique_ptr<Trk::Track> track = makeTrack(geoctx, result, *spacePoints);
+    // if (track) {
+    //   outputTracks->push_back(std::move(track));
+    // }
 
     // The track entry indices container. One element here.
     std::vector<size_t> trackTips;
@@ -184,8 +186,9 @@ StatusCode FaserActsKalmanFilterAlg::execute() {
 
   std::vector<Acts::CurvilinearTrackParameters> initialTrackParametersVector {*initialTrackParameters};
   m_trajectoryWriterTool->writeout(trajectories, geoctx, initialTrackParametersVector);
+//  ATH_CHECK(m_trajectoryStatesWriterTool->write(trajectories, geoctx, initialTrackParametersVector));
 
-  ATH_CHECK(trackContainer.record(std::move(outputTracks)));
+//  ATH_CHECK(trackContainer.record(std::move(outputTracks)));
 
   return StatusCode::SUCCESS;
 }
diff --git a/Tracking/Acts/FaserActsKalmanFilter/src/ProtoTrackWriterTool.cxx b/Tracking/Acts/FaserActsKalmanFilter/src/ProtoTrackWriterTool.cxx
index f1628bc8da362cb20f3c7d99c0967fe180c3a6c9..5ec0dfc1a1d430208b10c14a7c368ac89ac08ff3 100644
--- a/Tracking/Acts/FaserActsKalmanFilter/src/ProtoTrackWriterTool.cxx
+++ b/Tracking/Acts/FaserActsKalmanFilter/src/ProtoTrackWriterTool.cxx
@@ -87,18 +87,21 @@ StatusCode ProtoTrackWriterTool::write(
   m_params->Fill();
 
   for (const Measurement& variantMeasurement : *measurements) {
-    auto meas = std::get<Acts::Measurement<IndexSourceLink, Acts::BoundIndices, 2>>(variantMeasurement);
-    Acts::GeometryIdentifier geoId = meas.sourceLink().geometryId();
-    Identifier id = geoIdentifierMap.at(geoId);
-    m_station = m_idHelper->station(id);
-    m_layer = m_idHelper->layer(id);
-    m_phi = m_idHelper->phi_module(id);
-    m_eta = m_idHelper->eta_module(id);
-    m_side = m_idHelper->side(id);
-    auto params = meas.parameters();
-    m_meas_eLOC0 = params[Acts::eBoundLoc0];
-    m_meas_eLOC1 = params[Acts::eBoundLoc1];
-    m_meas->Fill();
+    // auto meas = std::get<Acts::Measurement<IndexSourceLink, Acts::BoundIndices, 2>>(variantMeasurement);
+    std::visit([&](const auto& meas) {
+      Acts::GeometryIdentifier geoId = meas.sourceLink().geometryId();
+      Identifier id = geoIdentifierMap.at(geoId);
+      m_station = m_idHelper->station(id);
+      m_layer = m_idHelper->layer(id);
+      m_phi = m_idHelper->phi_module(id);
+      m_eta = m_idHelper->eta_module(id);
+      m_side = m_idHelper->side(id);
+      auto params = meas.parameters();
+      m_meas_eLOC0 = params[Acts::eBoundLoc0];
+      // write out as many variables as there are
+      // m_meas_eLOC1 = params[Acts::eBoundLoc1];
+      m_meas->Fill();
+    }, variantMeasurement);
   };
 
   return StatusCode::SUCCESS;
diff --git a/Tracking/Acts/FaserActsKalmanFilter/src/SegmentFitClusterTrackFinderTool.cxx b/Tracking/Acts/FaserActsKalmanFilter/src/SegmentFitClusterTrackFinderTool.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..5e78fc1a02509321c63328d4b405933c87e92aed
--- /dev/null
+++ b/Tracking/Acts/FaserActsKalmanFilter/src/SegmentFitClusterTrackFinderTool.cxx
@@ -0,0 +1,143 @@
+#include "FaserActsKalmanFilter/SegmentFitClusterTrackFinderTool.h"
+
+#include "Acts/Definitions/TrackParametrization.hpp"
+#include "Acts/EventData/Measurement.hpp"
+#include "Acts/Geometry/GeometryContext.hpp"
+#include "Acts/Geometry/GeometryIdentifier.hpp"
+#include "Acts/Geometry/TrackingGeometry.hpp"
+#include "Acts/Surfaces/Surface.hpp"
+#include "FaserActsKalmanFilter/IndexSourceLink.h"
+#include "Identifier/Identifier.h"
+#include "TrackerIdentifier/FaserSCT_ID.h"
+#include "TrackerReadoutGeometry/SCT_DetectorManager.h"
+#include "TrkTrack/Track.h"
+#include "TrkTrack/TrackStateOnSurface.h"
+#include "TrackerRIO_OnTrack/FaserSCT_ClusterOnTrack.h"
+#include "TrackerPrepRawData/FaserSCT_Cluster.h"
+#include "TrackerSpacePoint/FaserSCT_SpacePoint.h"
+#include "TrackerPrepRawData/FaserSCT_ClusterCollection.h"
+#include <random>
+
+
+SegmentFitClusterTrackFinderTool::SegmentFitClusterTrackFinderTool(const std::string& type, const std::string& name, const IInterface* parent)
+    : base_class(type, name, parent) {}
+
+
+StatusCode SegmentFitClusterTrackFinderTool::initialize() {
+  ATH_CHECK(detStore()->retrieve(m_idHelper, "FaserSCT_ID"));
+  ATH_CHECK(detStore()->retrieve(m_detManager, "SCT"));
+  ATH_CHECK(m_trackingGeometryTool.retrieve());
+  ATH_CHECK(m_trackCollection.initialize());
+  ATH_CHECK(m_spacePointContainerKey.initialize());
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode SegmentFitClusterTrackFinderTool::run() {
+  SG::ReadHandle<TrackCollection> trackCollection {m_trackCollection};
+  ATH_CHECK(trackCollection.isValid());
+
+  SG::ReadHandle<FaserSCT_SpacePointContainer> spacePointContainer {m_spacePointContainerKey};
+  ATH_CHECK(spacePointContainer.isValid());
+
+  using IdentifierMap = std::map<Identifier, Acts::GeometryIdentifier>;
+  std::shared_ptr<IdentifierMap> identifierMap = m_trackingGeometryTool->getIdentifierMap();
+
+  // create source links and measurements
+  const int kSize = 1;
+  using ThisMeasurement = Acts::Measurement<IndexSourceLink, Acts::BoundIndices, kSize>;
+  std::array<Acts::BoundIndices, kSize> Indices = {Acts::eBoundLoc0};
+  std::vector<IndexSourceLink> sourceLinks;
+  std::vector<Measurement> measurements;
+  std::map<int, Amg::Vector3D> positions;
+  std::map<int, Amg::Vector3D> directions;
+  // TODO only take best track (most hits, smallest chi2)
+  // for now I assume that there is only one track in each station
+  for (const Trk::Track* track : *trackCollection) {
+    auto fitParameters = track->trackParameters()->front();
+    const Amg::Vector3D fitPosition = fitParameters->position();
+    const Amg::Vector3D fitMomentum = fitParameters->momentum();
+    Identifier id;
+    for (const Trk::TrackStateOnSurface* state : *(track->trackStateOnSurfaces())) {
+      auto clusterOnTrack = dynamic_cast<const Tracker::FaserSCT_ClusterOnTrack*> (
+          state->measurementOnTrack());
+      if (clusterOnTrack) {
+        const Tracker::FaserSCT_Cluster* cluster = clusterOnTrack->prepRawData();
+        id = cluster->detectorElement()->identify();
+        // create source link
+        Acts::GeometryIdentifier geoId = identifierMap->at(id);
+        ATH_MSG_DEBUG(geoId);
+        ATH_MSG_DEBUG(m_idHelper->station(id) << "/" <<
+          m_idHelper->layer(id) << "/" <<
+          m_idHelper->phi_module(id) << "/" <<
+          m_idHelper->eta_module(id) << "/" <<
+          m_idHelper->side(id)
+        );
+        IndexSourceLink sourceLink(geoId, measurements.size());
+        // create measurement
+        const auto& par = cluster->localPosition();
+        const auto& cov = cluster->localCovariance();
+        ATH_MSG_DEBUG("?? parameters" << par.head(1));
+        auto tmp = cov.block<1,1>(0,0);
+        ATH_MSG_DEBUG("?? cov" << tmp);
+        ThisMeasurement meas(sourceLink, Indices, par.head(1), cov.block<1,1>(0,0));
+        sourceLinks.push_back(sourceLink);
+        measurements.emplace_back(std::move(meas));
+      }
+    }
+    int station = m_idHelper->station(id);
+    positions[station] = fitPosition;
+    directions[station] = fitMomentum;
+  }
+
+  m_sourceLinks = std::make_shared<std::vector<IndexSourceLink>>(sourceLinks);
+  m_measurements = std::make_shared<std::vector<Measurement>>(measurements);
+
+  // TODO how should we handle events without a track in each station?
+  if (positions.size() != 3) {
+    return StatusCode::RECOVERABLE;
+  }
+
+  // create initial surface
+  m_initialSurface = Acts::Surface::makeShared<Acts::PlaneSurface>(
+      Acts::Vector3 {0, 0, 0}, Acts::Vector3{0, 0, 1});
+
+  // create initial parameters
+  Acts::Vector3 pos = positions[1] - positions[1].z()/directions[1].z() * directions[1];
+  Acts::Vector4 pos4 {pos.x(), pos.y(), pos.z(), 0};
+  Acts::Vector3 dir = positions[2] - positions[1];
+  double abs_momentum = momentum(positions);
+  // TODO get charge from momentum calculation
+  double charge = 1;
+
+  Acts::BoundSymMatrix cov = Acts::BoundSymMatrix::Zero();
+  cov(Acts::eBoundLoc0, Acts::eBoundLoc0) = m_covLoc0;
+  cov(Acts::eBoundLoc1, Acts::eBoundLoc1) = m_covLoc1;
+  cov(Acts::eBoundPhi, Acts::eBoundPhi) = m_covPhi;
+  cov(Acts::eBoundTheta, Acts::eBoundTheta) = m_covTheta;
+  cov(Acts::eBoundQOverP, Acts::eBoundQOverP) = m_covQOverP;
+  cov(Acts::eBoundTime, Acts::eBoundTime) = m_covTime;
+
+  auto initialParameters = Acts::CurvilinearTrackParameters(
+      pos4, dir, abs_momentum, charge, cov);
+  m_initialTrackParameters = std::make_shared<const Acts::CurvilinearTrackParameters>(initialParameters);
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode SegmentFitClusterTrackFinderTool::finalize() {
+  return StatusCode::SUCCESS;
+}
+
+
+double SegmentFitClusterTrackFinderTool::momentum(const std::map<int, Amg::Vector3D>& pos, double B) {
+  Acts::Vector3 vec_l = pos.at(3) - pos.at(1);
+  double abs_l = std::sqrt(vec_l.y() * vec_l.y() + vec_l.z() * vec_l.z());
+  double t = (pos.at(2).z() - pos.at(1).z()) / (pos.at(3).z() - pos.at(1).z());
+  Acts::Vector3 vec_m = pos.at(1) + t * vec_l;
+  Acts::Vector3 vec_s = pos.at(2) - vec_m;
+  double abs_s = std::sqrt(vec_s.y() * vec_s.y() + vec_s.z() * vec_s.z());
+  double p_yz = 0.3 * abs_l * abs_l * B / (8 * abs_s * 1000);
+  return p_yz;
+}
diff --git a/Tracking/Acts/FaserActsKalmanFilter/src/TrajectoryWriterTool.cxx b/Tracking/Acts/FaserActsKalmanFilter/src/TrajectoryWriterTool.cxx
index ce14e6092f003bca518fab66f79a9b54f8d4b18d..663870d576ade1d284a184273facd2e591b84705 100644
--- a/Tracking/Acts/FaserActsKalmanFilter/src/TrajectoryWriterTool.cxx
+++ b/Tracking/Acts/FaserActsKalmanFilter/src/TrajectoryWriterTool.cxx
@@ -80,9 +80,15 @@ void TrajectoryWriterTool::initializeTree() {
   m_tree->Branch("g_x_fit" , &m_x_fit);
   m_tree->Branch("g_y_fit" , &m_y_fit);
   m_tree->Branch("g_z_fit" , &m_z_fit);
+  m_tree->Branch("lx_hit" , &m_lx_hit);
+  m_tree->Branch("ly_hit" , &m_ly_hit);
   m_tree->Branch("x_hit" , &m_x_hit);
   m_tree->Branch("y_hit" , &m_y_hit);
   m_tree->Branch("z_hit" , &m_z_hit);
+  m_tree->Branch("meas_eLOC0" , &m_meas_eLOC0);
+  m_tree->Branch("meas_eLOC1" , &m_meas_eLOC1);
+  m_tree->Branch("meas_cov_eLOC0" , &m_meas_cov_eLOC0);
+  m_tree->Branch("meas_cov_eLOC1" , &m_meas_cov_eLOC1);
 
   m_tree->Branch("nPredicted", &m_nPredicted);
   m_tree->Branch("predicted", &m_prt);
@@ -231,7 +237,10 @@ void TrajectoryWriterTool::writeout(TrajectoriesContainer trajectories,
         m_eta_ini.push_back(eta(initialparameters[iTraj].position(geoContext)));
 
     // The trajectory entry indices and the multiTrajectory
-    const auto& [trackTips, mj] = traj.trajectory();
+    // const auto& [trackTips, mj] = traj.multiTrajectory();
+    const auto& mj = traj.multiTrajectory();
+    const auto& trackTips = traj.tips();
+
     if (trackTips.empty()) {
       ATH_MSG_WARNING("Empty multiTrajectory.");
       m_nMeasurements = -1;
@@ -311,11 +320,16 @@ void TrajectoryWriterTool::writeout(TrajectoriesContainer trajectories,
 
       // extract local and global position
       Acts::Vector2 local(meas[Acts::eBoundLoc0], meas[Acts::eBoundLoc1]);
-const Acts::Vector3 dir = Acts::makeDirectionUnitFromPhiTheta(meas[Acts::eBoundPhi], meas[Acts::eBoundTheta]);
+      const Acts::Vector3 dir = Acts::makeDirectionUnitFromPhiTheta(meas[Acts::eBoundPhi], meas[Acts::eBoundTheta]);
       Acts::Vector3 mom(1, 1, 1);
       Acts::Vector3 global =
           surface.localToGlobal(geoContext, local, dir);
 
+      m_meas_eLOC0.push_back(state.calibrated()[Acts::eBoundLoc0]);
+      m_meas_eLOC1.push_back(state.calibrated()[Acts::eBoundLoc1]);
+      m_meas_cov_eLOC0.push_back(state.calibratedCovariance()(Acts::eBoundLoc0, Acts::eBoundLoc0));
+      m_meas_cov_eLOC1.push_back(state.calibratedCovariance()(Acts::eBoundLoc1, Acts::eBoundLoc1));
+
       // fill the measurement info
       m_lx_hit.push_back(local[Acts::ePos0]);
       m_ly_hit.push_back(local[Acts::ePos1]);
@@ -343,15 +357,15 @@ const Acts::Vector3 dir = Acts::makeDirectionUnitFromPhiTheta(meas[Acts::eBoundP
 
         /// Predicted residual
         m_res_eLOC0_prt.push_back(residual(Acts::eBoundLoc0));
-        m_res_eLOC1_prt.push_back(residual(Acts::eBoundLoc1));
+//        m_res_eLOC1_prt.push_back(residual(Acts::eBoundLoc1));
 
         /// Predicted parameter pulls
         m_pull_eLOC0_prt.push_back(
             residual(Acts::eBoundLoc0) /
             sqrt(resCov(Acts::eBoundLoc0, Acts::eBoundLoc0)));
-        m_pull_eLOC1_prt.push_back(
-            residual(Acts::eBoundLoc1) /
-            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
+//        m_pull_eLOC1_prt.push_back(
+//            residual(Acts::eBoundLoc1) /
+//            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
 
         /// Predicted parameter
         m_eLOC0_prt.push_back(parameter.parameters()[Acts::eBoundLoc0]);
@@ -432,15 +446,15 @@ const Acts::Vector3 dir = Acts::makeDirectionUnitFromPhiTheta(meas[Acts::eBoundP
 
         /// Filtered residual
         m_res_eLOC0_flt.push_back(residual(Acts::eBoundLoc0));
-        m_res_eLOC1_flt.push_back(residual(Acts::eBoundLoc1));
+//        m_res_eLOC1_flt.push_back(residual(Acts::eBoundLoc1));
 
         /// Filtered parameter pulls
         m_pull_eLOC0_flt.push_back(
             residual(Acts::eBoundLoc0) /
             sqrt(resCov(Acts::eBoundLoc0, Acts::eBoundLoc0)));
-        m_pull_eLOC1_flt.push_back(
-            residual(Acts::eBoundLoc1) /
-            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
+//        m_pull_eLOC1_flt.push_back(
+//            residual(Acts::eBoundLoc1) /
+//            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
 
         /// Filtered parameter
         m_eLOC0_flt.push_back(parameter.parameters()[Acts::eBoundLoc0]);
@@ -523,30 +537,30 @@ const Acts::Vector3 dir = Acts::makeDirectionUnitFromPhiTheta(meas[Acts::eBoundP
         auto residual = state.effectiveCalibrated() - H * state.smoothed();
 
         m_res_x_hit.push_back(residual(Acts::eBoundLoc0));
-        m_res_y_hit.push_back(residual(Acts::eBoundLoc1));
+//        m_res_y_hit.push_back(residual(Acts::eBoundLoc1));
         m_err_x_hit.push_back(
             sqrt(resCov(Acts::eBoundLoc0, Acts::eBoundLoc0)));
-        m_err_y_hit.push_back(
-            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
+//        m_err_y_hit.push_back(
+//            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
         m_pull_x_hit.push_back(
             residual(Acts::eBoundLoc0) /
             sqrt(resCov(Acts::eBoundLoc0, Acts::eBoundLoc0)));
-        m_pull_y_hit.push_back(
-            residual(Acts::eBoundLoc1) /
-            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
+//        m_pull_y_hit.push_back(
+//            residual(Acts::eBoundLoc1) /
+//            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
         m_dim_hit.push_back(state.calibratedSize());
 
         /// Smoothed residual
         m_res_eLOC0_smt.push_back(residual(Acts::eBoundLoc0));
-        m_res_eLOC1_smt.push_back(residual(Acts::eBoundLoc1));
+//        m_res_eLOC1_smt.push_back(residual(Acts::eBoundLoc1));
 
         /// Smoothed parameter pulls
         m_pull_eLOC0_smt.push_back(
             residual(Acts::eBoundLoc0) /
             sqrt(resCov(Acts::eBoundLoc0, Acts::eBoundLoc0)));
-        m_pull_eLOC1_smt.push_back(
-            residual(Acts::eBoundLoc1) /
-            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
+//        m_pull_eLOC1_smt.push_back(
+//            residual(Acts::eBoundLoc1) /
+//            sqrt(resCov(Acts::eBoundLoc1, Acts::eBoundLoc1)));
 
         /// Smoothed parameter
         m_eLOC0_smt.push_back(parameter.parameters()[Acts::eBoundLoc0]);
@@ -641,6 +655,10 @@ void TrajectoryWriterTool::clearVariables() const {
   m_x_hit.clear();
   m_y_hit.clear();
   m_z_hit.clear();
+  m_meas_eLOC0.clear();
+  m_meas_eLOC1.clear();
+  m_meas_cov_eLOC0.clear();
+  m_meas_cov_eLOC1.clear();
   m_res_x_hit.clear();
   m_res_y_hit.clear();
   m_err_x_hit.clear();
diff --git a/Tracking/Acts/FaserActsKalmanFilter/src/components/FaserActsKalmanFilter_entries.cxx b/Tracking/Acts/FaserActsKalmanFilter/src/components/FaserActsKalmanFilter_entries.cxx
index f02dd329586e5016fc281bdaac1af1a7ef57fcdc..23806a2dfb58c151c282565e87f0eaff3a9d0a3e 100755
--- a/Tracking/Acts/FaserActsKalmanFilter/src/components/FaserActsKalmanFilter_entries.cxx
+++ b/Tracking/Acts/FaserActsKalmanFilter/src/components/FaserActsKalmanFilter_entries.cxx
@@ -12,6 +12,8 @@
 #include "FaserActsKalmanFilter/SimWriterTool.h"
 #include "FaserActsKalmanFilter/TruthSeededTrackFinderTool.h"
 #include "FaserActsKalmanFilter/ProtoTrackWriterTool.h"
+#include "FaserActsKalmanFilter/RootTrajectoryStatesWriterTool.h"
+#include "FaserActsKalmanFilter/SegmentFitClusterTrackFinderTool.h"
 #include "FaserActsKalmanFilter/SegmentFitTrackFinderTool.h"
 
 
@@ -25,4 +27,6 @@ DECLARE_COMPONENT(TruthTrackFinderTool)
 DECLARE_COMPONENT(SimWriterTool)
 DECLARE_COMPONENT(TruthSeededTrackFinderTool)
 DECLARE_COMPONENT(ProtoTrackWriterTool)
+DECLARE_COMPONENT(SegmentFitClusterTrackFinderTool)
 DECLARE_COMPONENT(SegmentFitTrackFinderTool)
+DECLARE_COMPONENT(RootTrajectoryStatesWriterTool)
diff --git a/Tracking/Acts/FaserActsKalmanFilter/test/FaserActsKalmanFilterAlg.py b/Tracking/Acts/FaserActsKalmanFilter/test/FaserActsKalmanFilterAlg.py
index a3f1ccacd536a7d821903cd02ea3219290a1ebc1..815f214a65554c1d5f8e4cc29d0c4cfcebde2c8a 100644
--- a/Tracking/Acts/FaserActsKalmanFilter/test/FaserActsKalmanFilterAlg.py
+++ b/Tracking/Acts/FaserActsKalmanFilter/test/FaserActsKalmanFilterAlg.py
@@ -23,6 +23,7 @@ Configurable.configurableRun3Behavior = True
 # Configure
 ConfigFlags.Input.Files = ['my.RDO.pool.root']
 ConfigFlags.Output.ESDFileName = "FaserActsKalmanFilter.ESD.root"
+ConfigFlags.Output.AODFileName = "FaserActsKalmanFilter.AOD.pool.root"
 ConfigFlags.IOVDb.GlobalTag = "OFLCOND-FASER-01"
 ConfigFlags.GeoModel.Align.Dynamic = False
 ConfigFlags.Beam.NumberOfCollisions = 0.
@@ -39,7 +40,7 @@ acc.merge(SegmentFitAlgCfg(ConfigFlags))
 acc.merge(TrackerSpacePointFinderCfg(ConfigFlags))
 acc.merge(TruthSeededTrackFinderCfg(ConfigFlags))
 acc.merge(FaserActsKalmanFilterCfg(ConfigFlags))
-# acc.getEventAlgo("FaserActsKalmanFilterAlg").OutputLevel = DEBUG
+acc.getEventAlgo("FaserActsKalmanFilterAlg").OutputLevel = DEBUG
 
 # logging.getLogger('forcomps').setLevel(VERBOSE)
 # acc.foreach_component("*").OutputLevel = VERBOSE
@@ -50,5 +51,5 @@ acc.merge(FaserActsKalmanFilterCfg(ConfigFlags))
 # ConfigFlags.dump()
 
 # Execute and finish
-sc = acc.run(maxEvents=100)
+sc = acc.run(maxEvents=10)
 sys.exit(not sc.isSuccess())