PRM with particle filter localization

Added check for if goal is accessible from start.

Also added particle filter localization back in.  Doesn't work quite as
well since the particle filter isn't localizing very well, but I don't
really care.