diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp
index 8b01ec2..79a2a9d 100644
--- a/src/grid_map_geo.cpp
+++ b/src/grid_map_geo.cpp
@@ -129,8 +129,9 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra
     // TODO: This may be wrong if the pixelSizeY > 0
     int x = width - 1 - gridMapIndex(0);
     int y = gridMapIndex(1);
-
-    layer_elevation(x, y) = data[gridMapIndex(0) + width * gridMapIndex(1)];
+    double elevation = data[gridMapIndex(0) + width * gridMapIndex(1)];
+    if (elevation == 0.0) layer_elevation(x, y)= NAN;
+    else layer_elevation(x, y) = data[gridMapIndex(0) + width * gridMapIndex(1)];
   }
 
   /// TODO: This is a workaround with the problem of gdal 3 not translating altitude correctly.
@@ -325,6 +326,13 @@ bool GridMapGeo::AddLayerOffset(const double offset_distance, const std::string
 }
 
 void GridMapGeo::setGlobalOrigin(ESPG src_coord, const Eigen::Vector3d origin) {
+  auto original_espg = maporigin_.espg;
+  auto original_position = maporigin_.position;
+
+  Eigen::Vector3d map_position = original_position - origin;
+  // map_position(1) = -map_position(1);
+  grid_map_.setPosition(map_position.head(2));
+
   // Transform global origin into CH1903 / LV03 coordinates
   maporigin_.espg = src_coord;
   maporigin_.position = origin;