Initial release of PID Controller

examples/repl.js

@@ -0,0 +1,9 @@
+var arDrone = require('ar-drone');
+var autonomy = require('..');
+
+var client  = arDrone.createClient();
+var ctrl = new autonomy.Controller(client);
+var repl = client.createRepl();
+
+repl._repl.context['ctrl'] = ctrl;
+

index.js

@@ -3,6 +3,7 @@ var autonomy = exports;
 exports.StateEstimator = require('./lib/StateEstimator');
 exports.EKF = require('./lib/EKF');
 exports.Camera = require('./lib/Camera');
+exports.Controller = require('./lib/Controller');
 
 exports.estimateState = function(client, options) {
     var estimator = new autonomy.StateEstimator(client, options);

lib/Controller.js

@@ -0,0 +1,236 @@
+var Timers  = require('timers');
+var PID     = require('./PID');
+var EKF     = require('./EKF');
+var Camera  = require('./Camera');
+
+CONTROL_FREQ = 100; // Interval in ms.
+
+// Settings for controling translation motion
+EPS_LIN      = 0.1; // We are ok with 10 cm precision
+KP_LIN       = 0.15;
+KI_LIN       = 0.05;
+KD_LIN       = 0.05;
+
+// Settings for controlling angular motion
+EPS_ANG      = 0.05;  // We are ok with 0.05 rad precision (2.5 deg)
+KP_ANG       = 1;
+KI_ANG       = 0.1;
+KD_ANG       = 0.1;
+
+module.exports = Controller;
+function Controller(client, options) {
+
+    options = options || {};
+
+    this._state   = options.state || {x: 0, y:0, z:1, yaw: 0};
+    this._tag     = options.tag || {x: 0, y: 0, yaw: 0};
+    this._freq    = options.control_freq || CONTROL_FREQ;
+    this._debug   = options.debug || false;
+    this._pid_x   = new PID({kp: KP_LIN, ki: KI_LIN, kd: KD_LIN});
+    this._pid_y   = new PID({kp: KP_LIN, ki: KI_LIN, kd: KD_LIN});
+    this._pid_z   = new PID({kp: KP_LIN, ki: KI_LIN, kd: KD_LIN});
+    this._pid_yaw = new PID({kp: KP_ANG, ki: KI_ANG, kd: KD_ANG});
+    this._ekf     = new EKF(options);
+    this._camera  = new Camera();
+    this._enabled = false;
+    this._client  = client;
+    this._goal    = null;
+
+    var self = this;
+    client.on('navdata', function(d) {
+        self._processNavdata(d);
+    });
+
+    Timers.setInterval(function() {
+        self._control();
+    }, this._freq);
+}
+
+/*
+ * Enable auto-pilot. The controller will attempt to bring
+ * the drone (and maintain it) to the goal.
+ */
+Controller.prototype.enable = function() {
+    this._enabled = true;
+};
+
+/*
+ * Disable auto-pilot. The controller will stop all actions
+ * and send a stop command to the drone.
+ */
+Controller.prototype.disable = function() {
+    this._enabled = false;
+    this._client.stop();
+}
+
+/*
+ * Return the drone state (x,y,z,yaw) as estimated
+ * by the Kalman Filter.
+ */
+Controller.prototype.state = function() {
+    return this._state;
+}
+
+/*
+ * Sets a new goal and enable the controller. When the goal
+ * is reached, the callback is called with the current state.
+ */
+Controller.prototype.go = function(goal, callback) {
+    // Since we are going to modify goal settings, we
+    // disable the controller, just in case.
+    this.disable();
+
+    // Goals can be incomplete. In this case, we attempt to maintain
+    // the current state.
+    var state    = this._state;
+    if (goal.x == undefined) {goal.x = state.x};
+    if (goal.y == undefined) {goal.y = state.y};
+    if (goal.z == undefined) {goal.z = state.z};
+
+    // Normalize the yaw, to make sure we don't spin 360deg for
+    // nothing :-)
+    if (goal.yaw != undefined) {
+        var yaw = goal.yaw.toRad();
+        goal.yaw = Math.atan2(Math.sin(yaw),Math.cos(yaw));
+    } else {
+        goal.yaw = state.yaw;
+    }
+
+    // Update our goal
+    this._goal = goal;
+
+    // Keep track of the callback to trigger when we reach the goal
+    this._callback = callback;
+
+    // (Re)-Enable the controller
+    this.enable();
+}
+
+Controller.prototype._processNavdata = function(d) {
+
+    // EKF prediction step
+    this._ekf.predict(d);
+
+    // If a tag is detected by the bottom camera, we attempt a correction step
+    // This require prior configuration of the client to detect the oriented
+    // roundel and to enable the vision detect in navdata.
+    // TODO: Add documentation about this
+    if (d.visionDetect && d.visionDetect.nbDetected > 0) {
+        // Fetch detected tag position, size and orientation
+        var xc = d.visionDetect.xc[0]
+          , yc = d.visionDetect.yc[0]
+          , wc = d.visionDetect.width[0]
+          , hc = d.visionDetect.height[0]
+          , yaw = d.visionDetect.orientationAngle[0]
+          , dist = d.visionDetect.dist[0] / 100 // Need meters
+          ;
+
+        // Compute measure tag position (relative to drone) by
+        // back-projecting the pixel position p(x,y) to the drone
+        // coordinate system P(X,Y).
+        // TODO: Should we use dist or the measure altitude ?
+        var measured = camera.p2m(xc + wc/2, yc + hc/2, dist);
+
+        // Rotation is provided by the drone, we convert to radians
+        measured.yaw = yaw.toRad();
+
+        // Execute the EKS correction step
+        this._ekf.correct(measured, this._tag);
+    }
+
+    // Keep a local copy of the state
+    this._state = this._ekf.state();
+    this._state.z = d.demo.altitude;
+}
+
+Controller.prototype._control = function() {
+    // Do not control if not enabled
+    if (!this._enabled) return;
+
+    // Do not control if no known state or no goal defines
+    if (this._goal == null || this._state ==  null) return;
+
+    // Initialize control commands to zero
+    var ux = uy = uz = uyaw = 0;
+
+    // Compute error between current state and goal
+    var ex = this._goal.x - this._state.x
+      , ey = this._goal.y - this._state.y
+      , ez = this._goal.z - this._state.z
+      , eyaw = this._goal.yaw - this._state.yaw
+      ;
+
+    // Compute control commands
+    if (Math.abs(ex) > EPS_LIN) {
+        ux = this._pid_x.getCommand(ex);
+    }
+    if (Math.abs(ey) > EPS_LIN) {
+        uy = this._pid_y.getCommand(ey);
+    }
+    if (Math.abs(ez) > EPS_LIN) {
+        uz = this._pid_z.getCommand(ez);
+    }
+    if (Math.abs(eyaw) > EPS_ANG) {
+        uyaw = this._pid_yaw.getCommand(eyaw);
+    }
+
+    // If all commands are zero, we have reached our destination;
+    // we go into hover mode and trigger the callback if defined.
+    if (ux == 0 && uy == 0 && uz == 0 && uyaw == 0) {
+        this._client.stop();
+
+        if (this._callback != null) {
+            var cb = this._callback;
+            this._callback = null;
+            this.disable();
+            cb(this.state());
+        }
+    }
+    // Else map controller commands to drone commands
+    else {
+        var yaw  = this._state.yaw;
+        var cx   = Math.cos(yaw) * ux - Math.sin(yaw) * uy;
+        var cy   = -Math.sin(yaw) * ux + Math.cos(yaw) * uy;
+        var cz   = uz;
+        var cyaw = uyaw;
+
+
+        // Send commands to drone
+        if (Math.abs(cx) > EPS_LIN) {
+            if (cx >= 0) {
+                this._client.front(Math.min(cx, 1));
+            } else {
+                this._client.back(Math.min(Math.abs(cx), 1));
+            }
+        }
+
+        if (Math.abs(cy) > EPS_LIN) {
+            if (cy >= 0) {
+                this._client.right(Math.min(cy, 1));
+            } else {
+                this._client.left(Math.min(Math.abs(cy), 1));
+            }
+        }
+
+        if (cz >= 0) {
+            this._client.up(Math.min(cz, 1));
+        } else {
+            this._client.down(Math.min(Math.abs(cz), 1));
+        }
+
+        if (cyaw >= 0) {
+            this._client.clockwise(Math.min(cyaw, 1));
+        } else {
+            this._client.counterClockwise(Math.min(Math.abs(cyaw), 1));
+        }
+    }
+
+    if (this._debug) {
+        console.log("--------------------- Control step ----------------------------------------------");
+        console.log("Goal: \t %d,%d,%d,%d", this._goal.x, this._goal.y, this._goal.z, this._goal.yaw);
+        console.log("State:\t %d,%d,%d,%d", this._state.x, this._state.y, this._state.z, this._state.yaw);
+        console.log("Error:\t %d,%d,%d,%d", ex, ey, ez, eyaw);
+        console.log("Control: \t%d,%d,%d,%d", ux, uy, uz, uyaw);
+        console.log("Commands:\t %d,%d,%d,%d", cx, cy, cz, cyaw);
+    }
+}

lib/EKF.js

@@ -7,21 +7,19 @@ var Vector       = sylvester.Vector;
 EKF.DELTA_T = 1 / 15; // In demo mode, 15 navdata per second
 
 module.exports = EKF;
-function EKF(client, options) {
+function EKF(options) {
 
   options = options || {};
 
   this._options     = options;
   this._delta_t     = options.delta_t || EKF.DELTA_T;
-  this._state       = options.state   || {x: 0, y: 0, yaw: 0};
+  this._state       = options.state   || {x: 0, y: 0, z: 1, yaw: 0};
   this._tag         = options.tag     || {x: 0, y: 0, yaw: 0};
   this._last_yaw    = null;
-  
+
   this._sigma       = Matrix.I(3);
   this._q           = Matrix.Diagonal([0.0003, 0.0003, 0.0001]);
   this._r           = Matrix.Diagonal([0.3, 0.3, 0.1]);
-
-  console.log('Initialize Extended Kalman Filter.');
 }
 
 EKF.prototype.state = function() {
@@ -54,14 +52,14 @@ EKF.prototype.predict = function(data) {
     var o = {dx: vx * dt, dy: vy * dt, dphi: yaw - this._last_yaw};
     this._last_yaw  = yaw;
 
-    // Update the state estimate 
+    // Update the state estimate
     var state = this._state;
     state.x   = state.x + o.dx * Math.cos(state.yaw) - o.dy * Math.sin(state.yaw);
     state.y   = state.y + o.dx * Math.sin(state.yaw) + o.dy * Math.cos(state.yaw);
     state.yaw = state.yaw + o.dphi;
 
     // Normalize the yaw value
-    state.yaw = Math.atan2(Math.sin(state.yaw),Math.cos(state.yaw)); 
+    state.yaw = Math.atan2(Math.sin(state.yaw),Math.cos(state.yaw));
 
     // Compute the G term (due to the Taylor approximation to linearize the function).
     var G = $M(
@@ -89,7 +87,7 @@ EKF.prototype.correct = function(measure, pose) {
 
     // Normalized the measure yaw
     measure.yaw = normAngle(measure.yaw);
-    
+
     var z1     = Math.cos(psi) * (pose.x - state.x) + Math.sin(psi) * (pose.y - state.y);
     var z2     = -1 * Math.sin(psi) * (pose.x - state.x) + Math.cos(psi) * (pose.y - state.y);
     var z3     = pose.yaw - psi;
@@ -107,7 +105,7 @@ EKF.prototype.correct = function(measure, pose) {
     // Compute the Kalman Gain
     var Ht = H.transpose();
     var K = this._sigma.multiply(Ht).multiply(H.multiply(this._sigma).multiply(Ht).add(this._r).inverse())
-    
+
     // Correct the pose estimate
     var err = $V([e1, e2, e3]);
     var c = K . multiply(err);

lib/PID.js

@@ -0,0 +1,47 @@
+DEFAULT_KP = 0.15;
+DEFAULT_KI = 0.1;
+DEFAULT_KD = 0.1;
+
+module.exports = PID;
+function PID(options) {
+    this.configure(options || {});
+    this.reset();
+}
+
+PID.prototype.configure = function(options) {
+    this._kp = options.kp || DEFAULT_KP;
+    this._ki = options.ki || DEFAULT_KI;
+    this._kd = options.kd || DEFAULT_KD;
+}
+
+PID.prototype.reset = function() {
+    this._last_time = Date.now();
+    this._last_error = Infinity;
+    this._error_sum = 0;
+}
+
+PID.prototype.getCommand = function(e) {
+    // Compute dt in seconds
+    var time = Date.now();
+    var dt = (time - this._last_time) /1000
+
+    // Compute de (error derivation)
+    var de = 0;
+    if (this._last_error < Infinity) {
+        de = (e - this._last_error) / dt;
+    }
+
+    // Integrate error
+    this._error_sum += e * dt;
+
+    // Update our trackers
+    this._last_time = time;
+    this._last_error = e;
+
+    // Compute commands
+    var command = this._kp * e
+                + this._ki * this._error_sum
+                + this._kd * de;
+
+    return command;
+}

tests/mock/client.js

@@ -0,0 +1,84 @@
+var EventEmitter = require('events').EventEmitter;
+var Timers       = require('timers');
+var util         = require('util');
+
+DT = 30;
+
+module.exports = Client;
+util.inherits(Client, EventEmitter);
+function Client(options) {
+    EventEmitter.call(this);
+
+    options = options || {};
+
+    this._state = options.state || {x: 0, y: 0, z: 1, yaw: 0};
+    this._speed = {vx: 0, vy: 0, vz: 0, vyaw: 0};
+
+    var self = this;
+    Timers.setInterval(function() {
+        self._sendNavdata();
+    }, DT);
+}
+
+Client.prototype.front = function(speed) {
+    this._speed.vx = speed;
+}
+
+Client.prototype.back = function(speed) {
+    this._speed.vx = -speed;
+}
+
+Client.prototype.right = function(speed) {
+    this._speed.vy = speed;
+}
+
+Client.prototype.left = function(speed) {
+    this._speed.vy = -speed;
+}
+
+Client.prototype.up = function(speed) {
+    this._speed.vz = speed;
+}
+
+Client.prototype.down = function(speed) {
+    this._speed.vz = -speed;
+}
+
+Client.prototype.clockwise = function(speed) {
+    this._speed.vyaw = speed;
+}
+
+Client.prototype.counterClockwise = function(speed) {
+    this._speed.vyaw = -speed;
+}
+
+Client.prototype.stop = function() {
+    this._speed = {vx: 0, vy: 0, vz: 0, vyaw: 0};
+}
+
+Client.prototype._sendNavdata = function() {
+    // First we update the state based on speed
+    this._state.z = Math.max(0, this._state.z + this._speed.vz);
+    this._state.yaw = this._state.yaw + this._speed.vyaw;
+
+    var navdata = {
+        demo: {
+            rotation: {
+                pitch: 0,
+                roll: 0,
+                yaw: this._state.yaw
+            },
+            velocity: {
+                x: this._speed.vx * 1000,
+                y: this._speed.vy * 1000,
+                z: this._speed.vz * 1000
+            },
+            altitude: this._state.z
+        },
+        visionDetect: {
+            nbDetected: 0
+        }
+    };
+
+    this.emit('navdata', navdata);
+}

tests/simple.js

@@ -0,0 +1,10 @@
+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});
+
+console.log("State: %j", controller.state());
+
+controller.go({x: 0, y: 0, z:1, yaw: -45}, function(state) {
+    console.log("Reached state %j", state);
+});