Filter Audit

This pull request is currently intended for discussion around improvements and clean-up of the ambiguity test and kalman filter code. It is not intended to be merged as is.