C matrix initialized
cost function added