tentatively finished implementation of the particle filter icluding body pose