How do I handle yaw wrapping when implementing a simple GPS lever arm correction?

Question

I am trying to implement a lever arm correction. I have an aircraft with a gps antenna which is displaced from an on-board sensor whose GPS position I would like to know.

I have the distance from the antenna to the sensor in body coordinates (NOSE-RIGHTWING-DOWN) which I am calling a lever arm:

$LA^{body} = [x_{body} , y_{body} , z_{body}]$

My methodology is:

First convert gps trajectory of the antenna to NED: $Antenna^{NED}$.
Second obtain a DCM rotation matrix $C_{body}^{NED}$ using Yaw, Pitch, Roll
Third transform $LA^{body}$ to $LA^{NED}$ by $LA^{NED} = C_{body}^{NED} LA^{body}$
Finally find NED position of sensor by adding $LA^{NED}$ to $Antenna^{NED}$ \

However, after doing this I am noticing that the NED position of the sensor will jump whenever the yaw goes below -180, since in that case it wraps to +180, and vice versa. How do I handle this problem?

Answer 1

The answer actually ended up being very simple, and the problem was indeed due to pi wrapping. Wherever the yaw becomes discontinuous (180 -> -180), just force it to become continuous by adding 360. Similarly, subtract 360 whenever the jump goes the other way. Once the yaw is continuous, the input into the DCM will work because tan is invariant under 360 phase shifts.