1. A method of determining a three-dimensional (3D) position of a catheter tip, the method comprising: compensating a 2D position of the tip of the catheter for respiratory motion to generate a compensated 2D catheter position; generating weighted 2D sample points around the compensated 2D catheter position; determining 3D correspondent points of the weighted sample points in a 3D image; computing, with a processor, 3D sample points around each 3D correspondent point using a modified transformation derived from evaluating an iterative closest point cost function; computing, with a processor, a weighted mean of each 3D correspondent point from the corresponding 3D sample points using a weight corresponding to the modified transformation; computing, with a processor, a weighted covariance of each 3D correspondent point based on a difference between the corresponding weighted mean and the corresponding 3D sample points; and determining the 3D position of the catheter tip in the 3D image from a fusion of the weighted means and weighted covariances.