Kalman filter bias correction