function po=observabilityFilter(p,o,angle)

n=LineNormals2D(p')';

an=atan2(n(2,:),n(1,:));%Orientation of normal
ap=atan2(o(2)-p(2,:),o(2)-p(1,:));%Orientation of inverse points relative to origin

% for k=1:size(p,2)
% %     text(pi(1,k),pi(2,k),[num2str(an(k)) ' ' num2str(ap(k))])
%     text(p(1,k)+.1,p(2,k)+.1,num2str(an(k)))
%     text(p(1,k)+.1,p(2,k)+.05,num2str(ap(k)))
% end

diff=angleAbsDiff(ap,an);

%Remove unobservable points, and create current points
po=p(:,diff<angle);

apo=ap(diff<angle);

% keyboard
[~, I]=sort(apo,'descend');

po=po(:,I);
