Kalman filter and sample code