position filtering added, kalman filter bug fixes