Kalman filter worked example