Slighlty corrected kalman