Fix weird Kinect canny issue
Add pose publisher to nav visualizer
Add relative motion to simulator
Add 1st iteration controller