Basic and extended Kalman filter implementation