edited wall position functions in stateEstimator
starting error cleaning; about to merge
merging move_planning and finishing error-cleaning