Added Zero_Yaw calculations

And everything else we've decided so far with having a moving zero yaw
value