UTILS
cost_function_matrix: Build matrices for cost function
cost_value: Calculate the total cost
local_constraint_matrix: Build local constraint matrices
penalty_matrix: Build matrices for penalty cost calculation
uf_subproblem: Solve the sub-optimization problem in distributed algorithm for user friendly problem