Check if matrix has postive eigenvalues or not.
Normalize the given angle from -PI to PI
Kalman is working (maybe). Added function to check if the new uncertainty Cp has positive eigenvalues, otherwise it wont be comupted.