Major fix on tps cost

Fix tps_cost math bug -- change sum to trace
Change tps obj to include constant term
Add code to verify the simplified tps obj against the straight forward one
Add numerical tps cost (partial) -- it's very unstable
Change color convention