Two person particle filter working, but problems initialising.