Kalman filter source code c