Cleaning up sensor reading handling. Now no garbage generated.
Cleaning up the sensor fusion code to not generate garbage.