Multi sensor fusion kalman filter