Need a ready implementation of the Kalman filter. He tried to write, but in the end is not smooth, but abrupt (solve the problem to determine the phase modulation), or even at different angles (see pictures). If You explain what can be the problem (if anyone is interested, I can send the code) and/or casts ready implementation in Matlab/Python -- any applied tasks solved with the help of working code with this filter, I would be very grateful.

