Added dynamic Obstacle Expansion. Fixed a few bugs.