An example embodiment includes a method for identifying true feature matches from a plurality of candidate feature matches for vision based navigation. A weight for each of the plurality of candidate feature matches can be set. The method also includes iteratively performing for N iterations: calculating a fundamental matrix for the plurality of candidate feature matches using a weighted estimation that accounts for the weight of each of the plurality of candidate feature matches; calculating a distance from the fundamental matrix for each of the plurality of candidate feature matches; and updating the weight for each of the plurality of candidate feature matches as a function of the distance for the respective candidate feature match. After N iterations candidate feature matches having a distance less than a distance threshold can be selected as true feature matches.