SLAM: use list comprehension to make masking nicer.  include code for calculating robot's rotation
SLAM: make _draw_features show features in green instead of black
SLAM: add the blue trapezoid to _draw_features