Added transform to right CS
Cleaned up
Minor changes due to higher IMU pub frequency
Added code to find and update desired plane