# GPS navigation custom implementation works well when world is centered at zero lat long but fails when spawned at different co-ordinate

I made my GPS navigation package that takes in GPS , IMU readings, calculates the required bearing, current heading and commands the robot to move to a defined goal.

To find out the initial direction in which the robot is heading. I record the mean of source (src) lat/ long (by standing still for 10 sec), make the robot move straight for 20 sec, record the mean of new (dst) lat/ long (by standing still for 10 sec), then apply this formula to find the heading

init_delta_long = mean_lon_dst - mean_lon_src
X = math.cos(mean_lat_dst) * math.sin(init_delta_long)
Y = (math.cos(mean_lat_src) * math.sin(mean_lat_dst)) - (math.sin(mean_lat_src) * math.cos(mean_lat_dst) * math.cos(init_delta_long))


I also record the IMU yaw value at this time (let's call it imu_heading_offset) which basically doesn't change so far) so that I can update the robot's heading as it moves.

Robot's current/ true heading is calculated as self.true_heading = self.init_heading - (self.roll - self.imu_heading_offset) whenever a message on the IMU is read. Not to confuse but roll here is actually yaw (idk why does roll value update instead of yaw when i rotate the 4-wheeled robot).

I bring down the true heading in -180 to 180 degree range using below where gps_nav is just the instantiation of the class

if gps_nav.true_heading < math.radians(-180):


Then, I just compare the bearing with true heading and determine which direction to move or rotate. Now, if the world is centered at 0 lat, 0 long, everything works well but if it is centered at some other coordinate, the bearing calculation is incorrect (it doesn't point in the direction of the destination point). It works however, if I do atan2(Y,X) but that's not the right way.

This is the part from sdf file that ignition gazebo uses

<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<world_frame_orientation>ENU</world_frame_orientation>
<!-- Works with atan2(y,x) but not otherwise -->
<!-- <latitude_deg>-22.986687</latitude_deg>
<longitude_deg>-43.202501</longitude_deg> -->

<!-- works with atan2(x,y) but not otherwise -->
<!-- <latitude_deg>0.0</latitude_deg>
<longitude_deg>0.0</longitude_deg> -->

<elevation>0</elevation>