In this paper an algorithm for estimating tree positions is presented. The sensors used for the algorithm is GNSS and LIDAR, and data is collected in an orchard with grapefruit trees while driving along the rows. The positions of the trees are estimated using ellipse fitting on point clouds. The average accuracy for the center point estimation is 0.2 m in the along track direction and 0.35 m in the across track direction. The goal of the tree mapping algorithm is create a database of individual trees, and be the basis for creation of a graph map that can be used for mission planning and localization for an autonomous robot.