Code does not correctly calculate cost despite the code seeming right

I think that means you omitted the transposes. Please see this thread. This thread also has the full check list.