Returns canonical system values for scaling too.
Added matlab and cpp files for evaluating costs in external programs
Demo program for evaluating cost in external program
Viapoint task can be run in Matlab or external cpp program now.