optimized kalman a bit, still behind