Kalman filter c code example