Added Kalman filter, improved sensor init