kalman filter, implemented, need to test