Added servo lag model, heading now goes negative so PID actually should work
Improved accuracy of mass data