Added 3D cooperative SLAM
Not sure if working yet
Perfect feature correspondance assumed
Accuracy on other agent seems better than feature accuracy - need to check
or do Monte Carlo to confirm... blargh