Your algorithm always replaces Smallest and SecondSmallest at each iteration. Does that make sense? E.g., if the current Smallest = 5 and SecondSmallest = 7, and v(CurrentPosition) = 10, should your algorithm be replacing either of them? No, it shouldn't. But your algorithm does. You need to step through this one line at a time with a small example vector to see what is going wrong and change your logic. In some iterations your algorithm should not be replacing either of Smallest or SecondSmallest. So the if-test inside your loop should have logic for doing nothing. E.g.,
if( v(CurrentPosition) < Smallest )
% do something
elseif( v(CurrentPosition) < SecondSmallest )
% do something
end
With the above logic, if neither of the tests is satisfied, then nothing is done to Smallest or SecondSmallest.
