Fix bug with altitude calculation
Improve calculation of speeds and center of mass