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
Wrong naming convention, should be camelCase: http://wiki.ros.org/CppStyleGuide#Function_.2BAC8_Methods