diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h
index fc966a4c7..50c7c25e0 100644
--- a/lib/karto_sdk/include/karto_sdk/Mapper.h
+++ b/lib/karto_sdk/include/karto_sdk/Mapper.h
@@ -2007,7 +2007,7 @@ class KARTO_EXPORT Mapper : public Module
   kt_bool ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
   kt_bool ProcessAgainstNode(LocalizedRangeScan * pScan, const int & nodeId, Matrix3 * covariance = nullptr);
   kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool addScanToLocalizationBuffer = false, Matrix3 * covariance = nullptr);
-  kt_bool ProcessLocalization(LocalizedRangeScan * pScan);
+  kt_bool ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
   kt_bool RemoveNodeFromGraph(Vertex<LocalizedRangeScan> *);
   void AddScanToLocalizationBuffer(LocalizedRangeScan * pScan, Vertex<LocalizedRangeScan> * scan_vertex);
   void ClearLocalizationBuffer();
diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp
index 815fc8ee1..32637e9bd 100644
--- a/lib/karto_sdk/src/Mapper.cpp
+++ b/lib/karto_sdk/src/Mapper.cpp
@@ -2828,7 +2828,7 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad
   return false;
 }
 
-kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
+kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance)
 {
   if (pScan == NULL) {
     return false;
@@ -2866,8 +2866,8 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
     return false;
   }
 
-  Matrix3 covariance;
-  covariance.SetToIdentity();
+  Matrix3 cov;
+  cov.SetToIdentity();
 
   // correct scan (if not first scan)
   if (m_pUseScanMatching->GetValue() && pLastScan != NULL) {
@@ -2875,8 +2875,11 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
     m_pSequentialScanMatcher->MatchScan(pScan,
       m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
       bestPose,
-      covariance);
+      cov);
     pScan->SetSensorPose(bestPose);
+    if (covariance) {
+      *covariance = cov;
+    }
   }
 
   // add scan to buffer and assign id
@@ -2886,7 +2889,7 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
   if (m_pUseScanMatching->GetValue()) {
     // add to graph
     scan_vertex = m_pGraph->AddVertex(pScan);
-    m_pGraph->AddEdges(pScan, covariance);
+    m_pGraph->AddEdges(pScan, cov);
 
     m_pMapperSensorManager->AddRunningScan(pScan);
 
diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp
index f20baa3bf..d6b44d09f 100644
--- a/src/slam_toolbox_localization.cpp
+++ b/src/slam_toolbox_localization.cpp
@@ -160,6 +160,10 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan(
   // Add the localized range scan to the smapper
   boost::mutex::scoped_lock lock(smapper_mutex_);
   bool processed = false, update_reprocessing_transform = false;
+
+  Matrix3 covariance;
+  covariance.SetToIdentity();
+
   if (processor_type_ == PROCESS_NEAR_REGION) {
     if (!process_near_pose_) {
       RCLCPP_ERROR(get_logger(),
@@ -172,13 +176,13 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan(
     range_scan->SetOdometricPose(*process_near_pose_);
     range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
     process_near_pose_.reset(nullptr);
-    processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true);
+    processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance);
 
     // reset to localization mode
     update_reprocessing_transform = true;
     processor_type_ = PROCESS_LOCALIZATION;
   } else if (processor_type_ == PROCESS_LOCALIZATION) {
-    processed = smapper_->getMapper()->ProcessLocalization(range_scan);
+    processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance);
     update_reprocessing_transform = false;
   } else {
     RCLCPP_FATAL(get_logger(), "LocalizationSlamToolbox: "
@@ -194,6 +198,8 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan(
     // compute our new transform
     setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose,
       scan->header.stamp, update_reprocessing_transform);
+
+    publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp);
   }
 
   return range_scan;