Added thresholded Kalman filter
Hopefully fixed merge confilcts