Function that doesn't break!

The matrix K is becoming imaginary and this needs to be fixed. The
function is working without breaking anywhere.