added WIP implementation with a kalman filter