fkf function

Fast Kalman filter