Kalman filter in c code