Shaky fusion implemented
Refactored to take maps of joints/bones for all sensor data reporting/fusion