Implemented a decent KF for wall behavior