diff --git a/include/roscontroller.h b/include/roscontroller.h
index c1f3b4a..b2db0d8 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -99,6 +99,9 @@ private:
 	/*convert from spherical to cartesian coordinate system callback */
 	void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
 
+	/*convert from spherical to cartesian coordinate system callback */
+	void cvt_rangebearingGB_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
+
 	/*convert from spherical to cartesian coordinate system callback */
 	void cvt_rangebearing2D_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
 	
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index a40dd48..e94edd4 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -91,6 +91,13 @@ int buzzros_print(buzzvm_t vm) {
 //                std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
                 }
         }
+
+void gps_from_rb(double  range, double bearing, double out[3]) {
+ 	out[0] = asin(sin(cur_pos[0]) * cos(range/EARTH_RADIUS) + cos(cur_pos[0]) * sin(range/EARTH_RADIUS) * cos(bearing));
+	out[1] = cur_pos[1] + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(cur_pos[0]), cos(bearing/EARTH_RADIUS) - sin(cur_pos[0])*sin(out[0]));
+	out[3] = height; //constant height.
+}
+
 int buzzuav_goto(buzzvm_t vm) {
    buzzvm_lnum_assert(vm, 2);
    buzzvm_lload(vm, 1); /* dx */
@@ -99,20 +106,21 @@ int buzzuav_goto(buzzvm_t vm) {
    //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
    buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
    buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-   float dx = buzzvm_stack_at(vm, 1)->f.value;
-   float dy = buzzvm_stack_at(vm, 2)->f.value;
-   printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
+   float d = buzzvm_stack_at(vm, 1)->f.value;
+   float b  = buzzvm_stack_at(vm, 2)->f.value;
+   printf(" Vector for Goto: %.7f,%.7f\n",d,b);
    //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
-   double cur_pos_cartesian[3];
-   cartesian_coordinates(cur_pos,cur_pos_cartesian);
-   double goto_cartesian[3];
-   goto_cartesian[0] = dx + cur_pos_cartesian[0];
-   goto_cartesian[1] = dy + cur_pos_cartesian[1];
-   goto_cartesian[2] = cur_pos_cartesian[2];
-   spherical_coordinates(goto_cartesian, goto_pos);
+   //double cur_pos_cartesian[3];
+   //cartesian_coordinates(cur_pos,cur_pos_cartesian);
+   //double goto_cartesian[3];
+   //goto_cartesian[0] = dx + cur_pos_cartesian[0];
+   //goto_cartesian[1] = dy + cur_pos_cartesian[1];
+   //goto_cartesian[2] = cur_pos_cartesian[2];
+   //spherical_coordinates(goto_cartesian, goto_pos);
 //   goto_pos[0]=dx;
 //   goto_pos[1]=dy;
-   goto_pos[2]=height;	// force a constant altitude to avoid loop increases
+   //goto_pos[2]=height;	// force a constant altitude to avoid loop increases
+   gps_from_rb(d, b, goto_pos);
    cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
    printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
    buzz_cmd=2;
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 0b63b2a..23be135 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -320,6 +320,15 @@ namespace rosbzz_node{
 
 	#define EARTH_RADIUS 6371000.0
 	#define LAT_AVERAGE 45.564898
+	/*rectangular projection range and bearing coordinate system callback */
+	void roscontroller::cvt_rangebearingGB_coordinates(double nei[], double out[], double cur[]){
+		out[1] = acos(sin(cur[0]) * sin(nei[0]) +cos(nei[0]) * cos(cur[0])*cos(nei[1]-cur[1]))*EARTH_RADIUS;
+		double y = sin(cur[1]-nei[1])*cos(cur[0]);
+		double x =  cos(cur[0])*sin(nei[0]) - sin(nei[0])*cos(cur[0])*cos(cur[1]-nei[1]);
+		out[1] = atan2(y,x)/M_PI*180+180;
+		out[2] = 0.0;
+	}
+
 	/*rectangular projection range and bearing coordinate system callback */
 	void roscontroller::cvt_rangebearing2D_coordinates(double spherical_pos_payload [], double out[], double cur[]){
 		double nei_lat = spherical_pos_payload[0] / 180.0 * M_PI;
@@ -534,7 +543,7 @@ namespace rosbzz_node{
 			buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
 	//		cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
 			double cvt_neighbours_pos_test[3];
-			cvt_rangebearing_coordinates(neighbours_pos_payload, cvt_neighbours_pos_test, cur_pos);
+			cvt_rangebearingGB_coordinates(neighbours_pos_payload, cvt_neighbours_pos_test, cur_pos);
 			/*Convert obtained position to relative CARTESIAN position*/
 			double cartesian_neighbours_pos[3], cartesian_cur_pos[3];
 			cvt_cartesian_coordinates(neighbours_pos_payload, cartesian_neighbours_pos);
diff --git a/src/test1.bzz b/src/test1.bzz
index 993eca4..d15a7fa 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -16,7 +16,7 @@ TARGET_ALTITUDE = 10.01
  
 # Lennard-Jones parameters
 TARGET     = 50.0	#0.000001001
-EPSILON    = 80.0	#0.001
+EPSILON    = 200.0	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {