It looks like you are using the same approach as in my answer at How to compute inverse of matrix , which references my answer at Generalizing "trace_inv" for matrix quadratic forms . I didn’t check carefully that you have implemented that correctly (all transposes in correct place), but nothing jumped out at me as wrong.
Note that declaration of symmetric
in addition to semidefinite
for M
is redundant (not necessary), but I don’t think it causes any harm.