Pulling in changes from MultiModal branch
1. CamAruco2DObservationModel.cpp: Had some changes to handle two landmarks with same id

2. MotionModel has a new function generate path