diff --git a/README.md b/README.md
index 8e62550..3c29f85 100644
--- a/README.md
+++ b/README.md
@@ -19,20 +19,18 @@ Different modules of GLocal (left) and GLocal in action for large scale explorat
 # Papers
 If you find this package useful for your research, please consider citing our paper:
 
-* **Note:** Our paper was accepted for publication in IEEE RA-L, the information below will be updated upon publication. To read the paper please refer to ArXiv at the moment.
-
-* Lukas Schmid, Victor Reijgwart, Lionel Ott, Juan Nieto, Roland Siegwart, and Cesar Cadena, "**A Unified Approach for Autonomous Volumetric Exploration of Large Scale Environments under Severe Odometry Drift**", in *IEEE Robotics and Automation Letters*, 2021 \[IEEE | [ArXiv](https://arxiv.org/abs/1909.09548) | Video\]
+* Lukas Schmid, Victor Reijgwart, Lionel Ott, Juan Nieto, Roland Siegwart, and Cesar Cadena, "**A Unified Approach for Autonomous Volumetric Exploration of Large Scale Environments under Severe Odometry Drift**", in *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp. 4504-4511, July 2021 \[[IEEE](https://ieeexplore.ieee.org/document/9387110) | [ArXiv](https://arxiv.org/abs/2010.09859) | Video\]
   ```bibtex
   @ARTICLE{schmid2021glocal,
     author={L. {Schmid} and V. {Reijgwart} and L. {Ott} and J. {Nieto} and R. {Siegwart} and C. {Cadena}},
     journal={IEEE Robotics and Automation Letters},
     title={A Unified Approach for Autonomous Volumetric Exploration of Large Scale Environments under Severe Odometry Drift},
     year={2021},
-    volume={?},
-    number={?},
-    pages={?},
-    doi={?},
-    month={?},
+    volume={6},
+    number={3},
+    pages={4504-4511},
+    doi={10.1109/LRA.2021.3068954},
+    month={July},
   }
   ```
 
diff --git a/glocal_exploration/src/planning/global/skeleton/skeleton_a_star.cpp b/glocal_exploration/src/planning/global/skeleton/skeleton_a_star.cpp
index 1e4ab09..204f293 100644
--- a/glocal_exploration/src/planning/global/skeleton/skeleton_a_star.cpp
+++ b/glocal_exploration/src/planning/global/skeleton/skeleton_a_star.cpp
@@ -202,10 +202,10 @@ bool SkeletonAStar::getPathBetweenVertices(
 
     const voxblox::Point t_odom_current_vertex =
         current_submap.getPose() * current_vertex.point;
+    const FloatingPoint h_score = (goal_point - t_odom_current_vertex).norm();
     g_score_map[current_vertex_id] =
         (t_odom_current_vertex - start_point).norm();
-    f_score_map[current_vertex_id] =
-        (goal_point - t_odom_current_vertex).norm();
+    f_score_map[current_vertex_id] = h_score;
     open_set.insert(current_vertex_id);
   }
 
@@ -264,7 +264,7 @@ bool SkeletonAStar::getPathBetweenVertices(
           g_score_map[current_vertex_id] +
           (goal_point - current_vertex.point).norm();
       if (g_score_map.count(kGoalVertexId) == 0 ||
-          g_score_map[kGoalVertexId] < tentative_g_score) {
+          tentative_g_score < g_score_map[kGoalVertexId]) {
         g_score_map[kGoalVertexId] = tentative_g_score;
         f_score_map[kGoalVertexId] = tentative_g_score;
         parent_map[kGoalVertexId] = current_vertex_id;
@@ -333,11 +333,12 @@ bool SkeletonAStar::getPathBetweenVertices(
                   g_score_map[current_vertex_id] +
                   (t_odom_nearby_vertex - t_odom_current_vertex).norm();
               if (g_score_map.count(nearby_vertex_global_id) == 0 ||
-                  g_score_map[nearby_vertex_global_id] < tentative_g_score) {
+                  tentative_g_score < g_score_map[nearby_vertex_global_id]) {
+                const FloatingPoint h_score =
+                    (goal_point - t_odom_nearby_vertex).norm();
                 g_score_map[nearby_vertex_global_id] = tentative_g_score;
                 f_score_map[nearby_vertex_global_id] =
-                    tentative_g_score +
-                    (goal_point - t_odom_nearby_vertex).norm();
+                    tentative_g_score + h_score;
                 parent_map[nearby_vertex_global_id] = current_vertex_id;
               }
             } else {
@@ -389,10 +390,11 @@ bool SkeletonAStar::getPathBetweenVertices(
       // NOTE: Since the vertex and its neighbor are already in the same
       //       (submap) frame, we can directly compute their distance above
       if (g_score_map.count(neighbor_vertex_id) == 0 ||
-          g_score_map[neighbor_vertex_id] < tentative_g_score) {
+          tentative_g_score < g_score_map[neighbor_vertex_id]) {
+        const FloatingPoint h_score =
+            (goal_point - t_odom_neighbor_vertex).norm();
         g_score_map[neighbor_vertex_id] = tentative_g_score;
-        f_score_map[neighbor_vertex_id] =
-            tentative_g_score + (goal_point - t_odom_neighbor_vertex).norm();
+        f_score_map[neighbor_vertex_id] = tentative_g_score + h_score;
         parent_map[neighbor_vertex_id] = current_vertex_id;
       }
     }