Localization filter takes ground sensor into account.
Several improvements in the Kalman Filter and some bugs fixed.