count_z and part of particle filter
it seems that pfilter works now