Add code to get analytical mass matrix and add fwd/bwd algorithm to compute costs