7699fd9710
This is a WIP, the controller needs more work, but it is slowly getting there :-) - Filter background noise in EKF - Decouple goals (set goal per dof leaving others floating) - Exeute control on navdata (to reduce delay) - Improved PID coefficient (need more work)