Refactored nav_finken and some PIDs
Merge pull request #3 from ovgu-FINken/pid-testing

Refactored nav_finken and some PIDs