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..96fbae2 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -91,6 +91,18 @@ 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]) {
+	double lat = cur_pos[0]*M_PI/180.0;
+	double lon = cur_pos[1]*M_PI/180.0;
+	bearing = bearing*M_PI/180.0;
+ 	out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
+	out[0] = out[0]*180.0/M_PI;
+	out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
+	out[1] = out[1]*180.0/M_PI;
+	out[2] = height; //constant height.
+}
+
 int buzzuav_goto(buzzvm_t vm) {
    buzzvm_lnum_assert(vm, 2);
    buzzvm_lload(vm, 1); /* dx */
@@ -99,20 +111,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 c6fc378..863715f 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -13,7 +13,7 @@ namespace rosbzz_node{
 		/*Compile the .bzz file to .basm, .bo and .bdbg*/
  		Compile_bzz();
 		set_bzz_file(bzzfile_name.c_str());
-/*		double temp_gps[3]={45.564898,-73.563072,10.1};
+		double temp_gps[3]={45.564898,-73.563072,10.1};
 		double temp2_gps[3]={45.565246,-73.561951,10.1};
 		double temp_car[3], temp2_car[3];
 		double out[3], dif[3], out2[3];
@@ -24,10 +24,10 @@ namespace rosbzz_node{
 		for(int i=0;i<3;i++)
 			dif[i]=temp2_car[i]-temp_car[i];
 		cvt_spherical_coordinates(dif,out);
-		cvt_rangebearing_coordinates(temp2_gps,out2,temp_gps);
+		cvt_rangebearingGB_coordinates(temp2_gps,out2,temp_gps);
 //		cvt_rangebearing2D_coordinates(temp2_gps,out2,temp_gps);
 		printf("TEST RESULT: %.7f,%.7f,%.7f\n",out[0],out[1],out[2]);
-		printf("TEST RESULT2: %.7f,%.7f,%.7f\n",out2[0],out2[1],out2[2]);*/
+		printf("TEST RESULT2: %.7f,%.7f,%.7f\n",out2[0],out2[1],out2[2]);
 	}
 
 	/***Destructor***/
@@ -323,6 +323,19 @@ 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[]){
+		double lat1 = cur[0]*M_PI/180.0;
+		double lon1 = cur[1]*M_PI/180.0;
+		double lat2 = nei[0]*M_PI/180.0;
+		double lon2 = nei[1]*M_PI/180.0;
+		out[0] = acos(sin(lat1) * sin(lat2) +cos(lat2) * cos(lat1)*cos(lon2-lon1))*EARTH_RADIUS;
+		double y = sin(lon1-lon2)*cos(lat1);
+		double x =  cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2);
+		out[1] = atan2(y,x)+M_PI;
+		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;
@@ -537,7 +550,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);
@@ -550,7 +563,7 @@ namespace rosbzz_node{
 	//		cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
 			/*Extract robot id of the neighbour*/
 	 		uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));  
-			cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[2] << ", "<< cvt_neighbours_pos_payload[3] << endl;
+			cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
 	//		cout << "Rel Test Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_test[0] << ", "<< cvt_neighbours_pos_test[2] << ", "<< cvt_neighbours_pos_test[3] << endl;
 			/*pass neighbour position to local maintaner*/
 			buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
diff --git a/src/test1.bzz b/src/test1.bzz
index d50abd3..db07bca 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -13,11 +13,11 @@ function updated_neigh(){
 neighbors.broadcast(updated, update_no)
 }
 
-TARGET_ALTITUDE = 10.01
+TARGET_ALTITUDE = 10.0
  
 # 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) {