Change Kalman Library for correct math expression

I am NOT sure, please check