Add check for dimensional consistency in the cost functions

[msparapa]

Pretty much boils down to checking that initial and terminal cost units are equivalent to the path cost units multiplied by independent variable units.