StateEstimation: small fixes
Filter: move the queues into the S(3) data structure

No need for them to be static.  There is still a question of whether
there should be one queue shared amongst the S(3) filters which is
more efficient but more complicated logic.