lots of work! haha. Decoupled calculate_pose, added particle filter husk, etc