reworked the whole tracking math for less cpu intensive calcs.
replace some float to int32_t
use alt in cm.