Jumps in Calculated Bearing Signal
I am calculating bearing between two GPS positions (latitude and longitude) as below. I use this bearing to calculate heading of the vehicle:
def N9(p1, p2): [lat1, lon1] = p1 [lat2, lon2] = p2 lat1 = np.radians(lat1) lat2 = np.radians(lat2) dLat = np.radians(lat2 - lat1) dLon = np.radians(lon2 - lon1) y = math.sin x = math.sin(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dLon) brng = np.degrees(math.atan2(y, x)) return brng
However, there are jumps in the data, which I am trying to prevent. I believe this may be due to a compensation to prevent negative value. However, I prefer a more continuous data, to match a received data bearing signal which I compare this due.
Below you can see both the received (green) and estimated, the one I’m trying to fix (blue). You can see the parts where it does not match and there are spikes in the blue data.
How can I go about resolving this?