Fixed 3D known feature SLAM
	seems to work, UKF 3sigma bounds are still less than actual errors
	in most cases. unknown why