diff --git a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp b/spatio_temporal_voxel_layer/src/measurement_buffer.cpp
index 2e1fcdd..10c160b 100644
--- a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp
+++ b/spatio_temporal_voxel_layer/src/measurement_buffer.cpp
@@ -218,7 +218,7 @@ void MeasurementBuffer::RemoveStaleObservations(void)
   }
 
   for (it = _observation_list.begin(); it != _observation_list.end(); ++it) {
-    const rclcpp::Duration time_diff = _last_updated - it->_cloud->header.stamp;
+    const rclcpp::Duration time_diff = clock_->now() - it->_cloud->header.stamp;
 
     if (time_diff > _observation_keep_time) {
       _observation_list.erase(it, _observation_list.end());
diff --git a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp
index d3c8803..e723a4b 100644
--- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp
+++ b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp
@@ -600,10 +600,8 @@ void SpatioTemporalVoxelLayer::reset(void)
   current_ = false;
   was_reset_ = true;
 
-  observation_buffers_iter it = _observation_buffers.begin();
-  for (; it != _observation_buffers.end(); ++it) {
-    (*it)->ResetLastUpdatedTime();
-  }
+  // @kin-changes (costmap-clearance-issue): Reseting the _last_updated (ResetLastUpdatedTime) of the individual observation buffers was removed,
+  //  since the buffers are not reset in this process either.
 }
 
 /*****************************************************************************/
@@ -675,10 +673,10 @@ void SpatioTemporalVoxelLayer::updateCosts(
     return;
   }
 
-  // if not current due to reset, set current now after clearing
+  // set was_reset to false again
   if (!current_ && was_reset_) {
     was_reset_ = false;
-    current_ = true;
+    // @kin-changes (costmap-clearance-issue): don't force current_ to be true here
   }
 
   if (_update_footprint_enabled) {
@@ -771,6 +769,13 @@ void SpatioTemporalVoxelLayer::updateBounds(
   if (_map_save_duration) {
     should_save = node->now() - _last_map_save_time > *_map_save_duration;
   }
+
+  // mark observations
+  // @kin-changes (costmap-clearance-issue): this was moved before the call to ClearFrustums, to include the
+  //  new markings within one update cycle
+  _voxel_grid->Mark(marking_observations);
+
+  // save map or clear frustrums and populate costmap
   if (!_mapping_mode) {
     _voxel_grid->ClearFrustums(clearing_observations, cleared_cells);
   } else if (should_save) {
@@ -790,9 +795,6 @@ void SpatioTemporalVoxelLayer::updateBounds(
     SaveGridCallback(nullptr, request, response);
   }
 
-  // mark observations
-  _voxel_grid->Mark(marking_observations);
-
   // update the ROS Layered Costmap
   UpdateROSCostmap(min_x, min_y, max_x, max_y, cleared_cells);