Kalman filter opencv c code