A method of determining a three-dimensional (3D) position of a catheter tip includes: compensating a 2D position of the tip of the catheter for respiratory motion to generate a compensated 2D catheter position, generating weighted sample points around the compensated 2D catheter position, determining correspondent points of the weighted sample points in a 3D image, computing a weighted mean and a weighted covariance of each correspondent point, and determining the 3D position of the catheter tip in the 3D image from a fusion of the weighted means and weighted covariances.
