log likelihoods tested and working. std::cout to ros_info
printing particles