Change config parameters for partcile filter
Lower sense noise slightly
Fix pathfinder bug. Directions/Paths were reversed
Fix bugs and get algorithm working moderately well.

Code not in usable state yet. Contains debugging code