diff --git a/Core/include/Acts/TrackFinding/CombinatorialKalmanFilter.hpp b/Core/include/Acts/TrackFinding/CombinatorialKalmanFilter.hpp
index 52188537805..707b265ff82 100644
--- a/Core/include/Acts/TrackFinding/CombinatorialKalmanFilter.hpp
+++ b/Core/include/Acts/TrackFinding/CombinatorialKalmanFilter.hpp
@@ -590,7 +590,9 @@ class CombinatorialKalmanFilter {
           result.pathLimitReached(state, stepper, navigator, logger());
       const bool isTargetReached =
           targetReached(state, stepper, navigator, logger());
-      if (isEndOfWorldReached || isPathLimitReached || isTargetReached) {
+      const bool allBranchesStopped = result.activeBranches.empty();
+      if (isEndOfWorldReached || isPathLimitReached || isTargetReached ||
+          allBranchesStopped) {
         if (isEndOfWorldReached) {
           ACTS_VERBOSE("End of world reached");
         } else if (isPathLimitReached) {
@@ -617,20 +619,25 @@ class CombinatorialKalmanFilter {
           stepper.releaseStepSize(state.stepping, ConstrainedStep::actor);
         }
 
-        // Record the active branch and remove it from the list
-        storeLastActiveBranch(result);
-        result.activeBranches.pop_back();
+        if (!allBranchesStopped) {
+          // Record the active branch and remove it from the list
+          storeLastActiveBranch(result);
+          result.activeBranches.pop_back();
+        } else {
+          // This can happen if we stopped all branches in the filter step
+          ACTS_VERBOSE("All branches stopped");
+        }
 
         // If no more active branches, done with filtering; Otherwise, reset
         // propagation state to track state at next active branch
-        if (result.activeBranches.empty()) {
-          ACTS_VERBOSE("Kalman filtering finds "
-                       << result.collectedTracks.size() << " tracks");
-          result.finished = true;
-        } else {
+        if (!result.activeBranches.empty()) {
           ACTS_VERBOSE("Propagation jumps to branch with tip = "
                        << result.activeBranches.back().tipIndex());
           reset(state, stepper, navigator, result);
+        } else {
+          ACTS_VERBOSE("Stop Kalman filtering with "
+                       << result.collectedTracks.size() << " found tracks");
+          result.finished = true;
         }
       }
     }
@@ -649,7 +656,8 @@ class CombinatorialKalmanFilter {
               typename navigator_t>
     void reset(propagator_state_t& state, const stepper_t& stepper,
                const navigator_t& navigator, result_type& result) const {
-      auto currentState = result.activeBranches.back().outermostTrackState();
+      auto currentBranch = result.activeBranches.back();
+      auto currentState = currentBranch.outermostTrackState();
 
       // Reset the stepping state
       stepper.resetState(state.stepping, currentState.filtered(),
@@ -791,8 +799,9 @@ class CombinatorialKalmanFilter {
             currentBranch = result.activeBranches.back();
             prevTip = currentBranch.tipIndex();
 
-            if (currentBranch.outermostTrackState().typeFlags().test(
-                    TrackStateFlag::OutlierFlag)) {
+            auto currentState = currentBranch.outermostTrackState();
+
+            if (currentState.typeFlags().test(TrackStateFlag::OutlierFlag)) {
               // We don't need to update the stepper given an outlier state
               ACTS_VERBOSE("Outlier state detected on surface "
                            << surface->geometryId());
@@ -802,16 +811,16 @@ class CombinatorialKalmanFilter {
                            << nBranchesOnSurface << " branches");
               // Update stepping state using filtered parameters of last track
               // state on this surface
-              auto ts = result.activeBranches.back().outermostTrackState();
               stepper.update(state.stepping,
                              MultiTrajectoryHelpers::freeFiltered(
-                                 state.options.geoContext, ts),
-                             ts.filtered(), ts.filteredCovariance(), *surface);
+                                 state.options.geoContext, currentState),
+                             currentState.filtered(),
+                             currentState.filteredCovariance(), *surface);
               ACTS_VERBOSE(
                   "Stepping state is updated with filtered parameter:");
-              ACTS_VERBOSE("-> "
-                           << ts.filtered().transpose()
-                           << " of track state with tip = " << ts.index());
+              ACTS_VERBOSE("-> " << currentState.filtered().transpose()
+                                 << " of track state with tip = "
+                                 << currentState.index());
             }
           }
         }