particle filter working
mean reinitialization works
initial tests of pf reinitialization look good