Improve collision detection
Adapt control algorithm and estimation to Qt frame of reference