Kalman Filter implementation in Java