correct heading angle so that it always remains in [-pi, pi] :param theta: :return:

:param x: vehicle state vector = [x position, y position, heading, velocity] :param u: control vector = [acceleration, steering angl

for background please refer to wikipedia: https://en.wikipedia.org/wiki/Kalman_filter :param x_vec_est: :param u_vec:

for background please refer to wikipedia: https://en.wikipedia.org/wiki/Extended_Kalman_filter :param x_vec_est: :param u_ve

input matrices must be numpy arrays :param A: state transition matrix :param B: state control matrix :param C: measu