Modified KalmanUtil in accordance with C++11 standard