The attached VI will take care of phase jumps in either direction. I developed this for another project and it may need modification for your application.
Have you tried a smoothing filter or sliding window averager to get rid of your noise? These often work better than a simple lowpass filter.
When the system is moving slowly or stopped recalling the last valid position and allowing only small adjustments to it may prevent the big jumps. In other words, if you can't beat the noise, ignore it. This can be easy to talk about but is not always so easy to implement.
Lynn