Pull after blend

First working demo after blending calibration was up and running. Tons of new stuff. Note the streamlined cost functions which make use of templated subroutines
Should this be ROS_DEBUG?