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 an argument to the launch file (this doesn't seem very portable).