Hey @seabubbles as you may have seen I had the same NaN problem as you do right now. To figure out what the problem is, I took some sample data (based on real world scenarios) and let @Fdiaz algorithm approximate the position on my PC (not on the DWM1001). There, it worked pretty fine while the DWM1001 istelf always fails to approximate it. I still could not figure out what the problem is, but I have a suggestion:
As I read in other topics of this forum it might be worth a shot to position anchors at different height levels in order to avoid GDOP. Have a look here.
https://forum.qorvo.com/t/dwm-1001-anchor-placement/4303
I myself haven’t tested that solution yet. Instead I rewrote the custom LE in java which works when I receive only distance data from the DWM1001. Yet I still suffer from positioning errors when my tag’s Z coordinate is high 
Let me know if you’re interested in my java solution.
Regards,
Max