"Finish" initial particle filtering implementation
Bug fixes