Improving the PID Controller

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)

tests/simple.js

@@ -1,10 +1,13 @@
+// TODO This is broken. Need to write real tests.
+console.log("The test is broken. Will have to write proper tests :-)");
+
 var autonomy    = require('..');
 var client      = require('./mock/client');
 
-var controller  = new autonomy.Controller(new client(), {state: {x: 0, y:0, z:1, yaw: Math.PI/4}, debug: true});
+var controller  = new autonomy.Controller(new client(), {state: {x: 0, y:0, z:1, yaw: 0}, debug: false});
 
 console.log("State: %j", controller.state());
 
-controller.go({x: 0, y: 0, z:1, yaw: -45}, function(state) {
+controller.go({x: 1, y: 1}, function(state) {
     console.log("Reached state %j", state);
 });