Some sensor & filter updates and refactoring