INS fix

Fixed velocity and position estimates.
Fixed barometric altitude calculations. Off by 10.