init_kf function

Initialization of Kalman filter