diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/mapmatrix.bzz
index 79ad775..c3d3393 100644
--- a/buzz_scripts/include/mapmatrix.bzz
+++ b/buzz_scripts/include/mapmatrix.bzz
@@ -56,7 +56,7 @@ function add_obstacle(pos, off, inc_trust) {
     var yi = math.round(pos.y)
 
     if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
-        #log("Add obstacle in cell: ", xi, " ", yi)
+        # log("Add obstacle in cell: ", xi, " ", yi)
         var old=map[xi][yi]
         if(old-inc_trust > 0.0)
           map[xi][yi] = old-inc_trust
@@ -69,8 +69,8 @@ function remove_obstacle(pos, off, dec_trust) {
     var xi = math.round(pos.x)
     var yi = math.round(pos.y)
 
-    if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){
-        #log("Remove obstacle in cell: ", xi, " ", yi)
+    if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
+        # log("Remove obstacle in cell: ", xi, " ", yi)
         var old=map[xi][yi]
         if(old + dec_trust < 1.0)  #x,y
           map[xi][yi] = old+dec_trust
diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz
index 7cc5235..3fa686c 100644
--- a/buzz_scripts/include/rrtstar.bzz
+++ b/buzz_scripts/include/rrtstar.bzz
@@ -7,7 +7,7 @@ include "mapmatrix.bzz"
 
 RRT_TIMEOUT = 500
 RRT_RUNSTEP = 3
-PROX_SENSOR_THRESHOLD_M = 10
+PROX_SENSOR_THRESHOLD_M = 8.0
 GSCALE = {.x=1, .y=1}
 map = nil
 cur_cell = {}
@@ -46,7 +46,7 @@ function pathPlanner(m_goal, m_pos, kh4) {
     arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
     # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
     Q = {}
-    Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0}
+    Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=0,.4=1,.5=0}
     goalReached = 0
     timeout = 0
 
@@ -56,6 +56,7 @@ function pathPlanner(m_goal, m_pos, kh4) {
 }
 
 # get the grid cell position (x,y) equivalent to the input position
+# input should be relative to home position (planing start point)
 function getcell(m_curpos) {
   var cell = {}
   # relative to center (start position)
@@ -79,6 +80,7 @@ function getcell(m_curpos) {
 function populateGrid(m_pos) {
     getproxobs(m_pos)
     getneiobs (m_pos)
+    export_map(map)
 }
 
 # TODO: populate the map with neighors as blobs instead ?
@@ -136,26 +138,43 @@ function getproxobs (m_curpos) {
 
   reduce(proximity,
     function(key, value, acc) {
-      obs = getcell(math.vec2.newp(value.value, value.angle + pose.orientation.yaw))
-      obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + pose.orientation.yaw))
-      per = math.vec2.sub(obs,cur_cell)
-      obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
-      obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
-      obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
-      obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
-      if(value.value < PROX_SENSOR_THRESHOLD_M) {
-        if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
-          add_obstacle(obs, 0, 0.25)
-          add_obstacle(obs2, 0, 0.25)
-          add_obstacle(obsr, 0, 0.25)
-          add_obstacle(obsr2, 0, 0.25)
-          add_obstacle(obsl, 0, 0.25)
-          add_obstacle(obsl2, 0, 0.25)
-          updated_obstacle = 1
+      if (value.angle != -1) {  # down sensor of M100
+        if(value.value < PROX_SENSOR_THRESHOLD_M) {
+          obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(value.value, value.angle + pose.orientation.yaw)))
+          per = math.vec2.sub(obs,cur_cell)
+          obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
+          obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
+          obsr2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
+          obsl2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
+          # obs2 = getcell(math.vec2.newp(math.vec2.length(obs) + 1.0, math.vec2.angle(obs)))
+          # obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
+          # obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
+          if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
+            add_obstacle(obs, 0, 0.25)
+            add_obstacle(obsr, 0, 0.25)
+            add_obstacle(obsl, 0, 0.25)
+            add_obstacle(obsr2, 0, 0.25)
+            add_obstacle(obsl2, 0, 0.25)
+            # add_obstacle(obs2, 0, 0.25)
+            # add_obstacle(obsr2, 0, 0.25)
+            # add_obstacle(obsl2, 0, 0.25)
+            updated_obstacle = 1
+          }
+        } else {
+          var line_length = PROX_SENSOR_THRESHOLD_M;
+          while(line_length > 0) {
+            obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(line_length, value.angle + pose.orientation.yaw)))
+            per = math.vec2.sub(obs,cur_cell)
+            obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
+            obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
+            obsr = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
+            obsl = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
+            remove_obstacle(obs, 0, 0.05)
+            remove_obstacle(obsr, 0, 0.05)
+            remove_obstacle(obsl, 0, 0.05)
+            line_length = line_length - 1;
+          }
         }
-      } else if(map[math.round(obs.x)][math.round(obs.y)]!=1) {
-        remove_obstacle(obs, 0, 0.05)
-        updated_obstacle = 1
       }
       return acc
   }, math.vec2.new(0, 0))
@@ -225,7 +244,7 @@ function rrtstar() {
                 nbCount = nbCount + 1;
                 if(intersects != 1) {
                     #log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
-                    var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5]
+                    var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][4]][5]
 
                     if(distance < minCounted) {
                         minCounted = distance;
@@ -422,17 +441,18 @@ function getPath(Q,nb,gps){
     var pathL={}
     var npt=0
     # get the path from the point list
-    while(nb > 1) {
+    while(nb > 0) {
         npt=npt+1
         pathL[npt] = {}
         pathL[npt][1]=Q[nb][1]
         pathL[npt][2]=Q[nb][2]
-        if(nb != Q[Q[nb][3]][3])
-            nb = Q[nb][3];
-        else {  # TODO: why is this happening?
-            log("ERROR - Recursive path !!!")
-            return nil
+        if( nb > 1) {
+          if(nb == Q[Q[nb][3]][3]) {# TODO: why is this happening?
+              log("ERROR - Recursive path !!!")
+              return nil
+          }
         }
+        nb = Q[nb][3];
     }
 
     # Re-Order the list.
diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz
index 316d281..8a27c38 100644
--- a/buzz_scripts/include/uavstates.bzz
+++ b/buzz_scripts/include/uavstates.bzz
@@ -9,7 +9,7 @@ include "rrtstar.bzz"
 TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
-GOTO_MAXVEL = 2 # m/steps
+GOTO_MAXVEL = 1.5 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 0.5  # m.
 GOTOANG_TOL = 0.1 # rad.
diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz
index f586fe5..3ad4cc6 100644
--- a/buzz_scripts/testRRT.bzz
+++ b/buzz_scripts/testRRT.bzz
@@ -5,7 +5,7 @@ include "vstigenv.bzz"
 
 function action() {
   statef=action
-  uav_storegoal(45.5088103899,-73.1540826153,5.0)
+  uav_storegoal(45.5088103899,-73.1540826153,2.5)
   set_goto(idle)
 }
 
@@ -14,7 +14,7 @@ function init() {
   	uav_initstig()
 	uav_initswarm()
 
-	TARGET_ALTITUDE = 5.0 # m.
+	TARGET_ALTITUDE = 2.5 # m
 	# statef=turnedoff
 	# BVMSTATE = "TURNEDOFF"
 	statef = takeoff
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 3c58403..20e0456 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -179,6 +179,7 @@ int buzz_exportmap(buzzvm_t vm)
 / Buzz closure to export a 2D map
 /----------------------------------------*/
 {
+  grid.clear();
   buzzvm_lnum_assert(vm, 1);
   // Get the parameter
   buzzvm_lload(vm, 1);
@@ -193,7 +194,7 @@ int buzz_exportmap(buzzvm_t vm)
       buzzvm_dup(vm);
       buzzvm_pushi(vm, j);
       buzzvm_tget(vm);
-      row.insert(std::pair<int,int>(j,round(buzzvm_stack_at(vm, 1)->f.value*100.0)));
+      row.insert(std::pair<int,int>(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value*100.0)));
       buzzvm_pop(vm);
     }
     grid.insert(std::pair<int,std::map<int, int>>(i,row));
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index a59aac4..cfd47c0 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -473,8 +473,8 @@ void roscontroller::grid_publisher()
     grid_msg.info.resolution = 0.01;//gridMap.getResolution();
     grid_msg.info.width = g_w;
     grid_msg.info.height = g_h;
-    grid_msg.info.origin.position.x = 0.0;
-    grid_msg.info.origin.position.y = 0.0;
+    grid_msg.info.origin.position.x = round(g_w/2.0) * grid_msg.info.resolution;
+    grid_msg.info.origin.position.y = round(g_h/2.0) * grid_msg.info.resolution;
     grid_msg.info.origin.position.z = 0.0;
     grid_msg.info.origin.orientation.x = 0.0;
     grid_msg.info.origin.orientation.y = 0.0;