Found the constrained filtering bug, the fe/f_k variables wasn't
correctly updated for the partcles, due to the copy step in the
PF class.
Seem to have introduced some other problem complete ruining the estimate
at the moment.