From 09774c5839415c83037550705778aa8a1bc3c7e1 Mon Sep 17 00:00:00 2001 From: Laurent Eschenauer Date: Fri, 7 Jun 2013 23:37:01 +0200 Subject: [PATCH] Initial tag detection and EKF implementation --- index.js | 2 + lib/Camera.js | 55 +++++++++++++++++++++ lib/EKF.js | 134 ++++++++++++++++++++++++++++++++++++++++++++++++++ package.json | 1 + 4 files changed, 192 insertions(+) create mode 100644 lib/Camera.js create mode 100644 lib/EKF.js diff --git a/index.js b/index.js index d3668d7..d05c21c 100644 --- a/index.js +++ b/index.js @@ -1,6 +1,8 @@ var autonomy = exports; exports.StateEstimator = require('./lib/StateEstimator'); +exports.EKF = require('./lib/EKF'); +exports.Camera = require('./lib/Camera'); exports.estimateState = function(client, options) { var estimator = new autonomy.StateEstimator(client, options); diff --git a/lib/Camera.js b/lib/Camera.js new file mode 100644 index 0000000..00cd885 --- /dev/null +++ b/lib/Camera.js @@ -0,0 +1,55 @@ +var sylvester = require('sylvester'); +var util = require('util'); + + +// TODO: Extend to support roll/pitch in back projection +// TODO: Add support for front-facing camera +// TODO: Make image aspect ratio configurable + +// AR Drone 2.0 Bottom Camera Intrinsic Matrix +// https://github.com/tum-vision/ardrone_autonomy/blob/master/calibrations/ardrone2_bottom/cal.ymli +var K_BOTTOM = $M([[686.994766, 0, 329.323208], + [0, 688.195055, 159.323007], + [0, 0, 1]]); + +module.exports = Camera; +function Camera(options) { + this._options = options || {}; + this._k = this._options.k || K_BOTTOM; + + // We need to compute the inverse of K to back-project 2D to 3D + this._invK = this._k.inverse(); +} + +/* + * Given (x,y) pixel coordinates (e.g. obtained from tag detection) + * Returns a (X,Y) coordinate in drone space. + */ +Camera.prototype.p2m = function(x, y, altitude) { + // From the SDK Documentation: + // X and Y coordinates of detected tag or oriented roundel #i inside the picture, + // with (0; 0) being the top-left corner, and (1000; 1000) the right-bottom corner regardless + // the picture resolution or the source camera. + // + // But our camera intrinsic is built for 640 x 360 pixel grid, so we must do some mapping. + var xratio = 640 / 1000; + var yratio = 360 / 1000; + + // Perform a simple back projection, we assume the drone is flat (no roll/pitch) + // for the moment. We ignore the drone translation and yaw since we want X,Y in the + // drone coordinate system. + var p = $V([x * xratio, y * yratio, 1]); + var P = this._invK.multiply(p).multiply(altitude); + + // X,Y are expressed in meters, in the drone coordinate system. + // Which is: + // <--- front-facing camera + // | + // / \------- X + // \_/ + // | + // | + // Y + return {x: P.e(1), y: P.e(2)}; +} + diff --git a/lib/EKF.js b/lib/EKF.js new file mode 100644 index 0000000..3d3107c --- /dev/null +++ b/lib/EKF.js @@ -0,0 +1,134 @@ +var sylvester = require('sylvester'); +var util = require('util'); + +var Matrix = sylvester.Matrix; +var Vector = sylvester.Vector; + +EKF.DELTA_T = 1 / 15; // In demo mode, 15 navdata per second + +module.exports = EKF; +function EKF(client, 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._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() { + return this._state; +} + +EKF.prototype.confidence = function() { + return this._sigma; +} + +EKF.prototype.predict = function(data) { + var pitch = data.demo.rotation.pitch.toRad() + , roll = data.demo.rotation.roll.toRad() + , yaw = normAngle(data.demo.rotation.yaw.toRad()) + , vx = data.demo.velocity.x / 1000 //We want m/s instead of mm/s + , vy = data.demo.velocity.y / 1000 + , vz = data.demo.velocity.z / 1000 + , alt = data.demo.altitude + , dt = this._delta_t + ; + + // We are not interested by the absolute yaw, but the yaw motion, + // so we need at least a prior value to get started. + if (this._last_yaw == null) { + this._last_yaw = yaw; + return; + } + + // Compute the odometry by integrating the motion over delta_t + var o = {dx: vx * dt, dy: vy * dt, dphi: yaw - this._last_yaw}; + this._last_yaw = yaw; + + // 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)); + + // Compute the G term (due to the Taylor approximation to linearize the function). + var G = $M( + [[1, 0, -1 * Math.sin(state.yaw) * o.dx - Math.cos(state.yaw) * o.dy], + [0, 1, Math.cos(state.yaw) * o.dx - Math.sin(state.yaw) * o.dy], + [0, 0, 1]] + ); + + // Compute the new sigma + this._sigma = G.multiply(this._sigma).multiply(G.transpose()).add(this._q); +} + /* + * measure.x: x-position of marker in drone's xy-coordinate system (independant of roll, pitch) + * measure.y: y-position of marker in drone's xy-coordinate system (independant of roll, pitch) + * measure.yaw: yaw rotation of marker, in drone's xy-coordinate system (independant of roll, pitch) + * + * pose.x: x-position of marker in world-coordinate system + * pose.y: y-position of marker in world-coordinate system + * pose.yaw: yaw-rotation of marker in world-coordinate system + */ +EKF.prototype.correct = function(measure, pose) { + // Compute expected measurement given our current state and the marker pose + var state = this._state; + var psi = state.yaw; + + // 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; + + console.log("%d,%d,%d \t %d,%d,%d", measure.x, measure.y, measure.yaw, z1, z2, z3); + + // Compute the error + var e1 = measure.x - z1; + var e2 = measure.y - z2; + var e3 = measure.yaw - z3; + + // Compute the H term + var H = $M([[ -Math.cos(psi), -Math.sin(psi), Math.sin(psi) * (state.x - pose.x) - Math.cos(psi) * (state.y - pose.y)], + [ Math.sin(psi), -Math.cos(psi), Math.cos(psi) * (state.x - pose.x) + Math.sin(psi) * (state.y - pose.y)], + [ 0, 0, -1]]); + + // 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); + state.x = state.x + c.e(1); + state.y = state.y + c.e(2); + state.yaw = state.yaw + c.e(3); + + this._sigma = Matrix.I(3).subtract(K.multiply(H)).multiply(this._sigma); +}; + +function normAngle(rad) { + while (rad > Math.PI) { rad -= 2 * Math.PI;} + while (rad < -Math.PI) { rad += 2 * Math.PI;} + return rad; +} + +/** Converts numeric degrees to radians */ +if (typeof(Number.prototype.toRad) === "undefined") { + Number.prototype.toRad = function() { + return this * Math.PI / 180; + } +} diff --git a/package.json b/package.json index 979bce1..cd37975 100644 --- a/package.json +++ b/package.json @@ -16,6 +16,7 @@ "pid" ], "dependencies": { + "sylvester": "0.0.21" }, "author": "Laurent Eschenauer ", "license": "MIT"