Implementatie van add() in CostFunction

We gaan het algoritme eerst met Albert overleggen voor we hier verder
mee gaan.