GPS datas

[lolodenimes]

I runned the filter with several data sets and find out that the GPS state is totaly out of state.
The acceleration and velocity are correct but not the GPS and i don't succed in finding out what's the problem...
Pls help me =(
I'll link a picture of what's happening (red is the datas and blue the state).

[lolodenimes]

and i don't manage to find anithing about those equations:
_state(0) = _state(0) + (_state(3)/_state(4)) * (sin(_state(4) * dt + _state(2)) - sin(_state(2)));
_state(1) = _state(1) + (_state(3)/_state(4)) * (-cos(_state(4) * dt + _state(2)) + cos(_state(2)));