It turns out that if you do the rotations in a different order (unrotate by psi then unrotate by theta, see equation 9 of the Freescale application note) the resulting equations are easy to solve. I'll post the full solution and code a bit later, after I have a chance to make sure I didn't make any algebraic mistakes.
Edit: derivation attached as PDF file
new_magn_3d.pdf (26.4 KB)