Out-of-plane motion in freehand 3D ultrasound can be estimated using the correlation of corresponding patches, leading to sensorless freehand 3D ultrasound systems. The correlation between two images is related to their distance by calibrating the ultrasound probe: the probe is moved with an accurate stage (or with a robot in this work) and images of a phantom are collected, such that the position of each image is known. Since parts of the calibration curve with higher derivative gives lower displacement estimation error, previous work limits displacement estimation to parts with maximum derivative. In this paper, we first propose a novel method for exploiting the entire calibration curve by using a maximum likelihood estimator (MLE). We then propose for the first time using constrains inside the image to enhance the accuracy of out-of-plane motion estimation. We specifically use continuity constraint of a needle to reduce the variance of the estimated out-of-plane motion. Simulation and real tissue experimental results are presented.