Better calibration of my own sensor for yaw

Rearranged variables to match Z axis going down through top of sensor