Kalman filter implementation c