added start and goal plotting
Merge branch 'gkahn'
different timesteps for belief penalty