Formatting
Moved GUI update to the 'main' (external controller). Now the wrapper
for ROS has the responsibility to update RVIZ
Removed compilation warnings on assertion
Make the trajectory points almost transparent and add some comments
Moved trajectory handling to main/ros_ekfoa (external controller)