Initial tag detection and EKF implementation
Esse commit está contido em:
Laurent Eschenauer
@@ -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);
|
||||
|
||||
@@ -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)};
|
||||
}
|
||||
|
||||
+134
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -16,6 +16,7 @@
|
||||
"pid"
|
||||
],
|
||||
"dependencies": {
|
||||
"sylvester": "0.0.21"
|
||||
},
|
||||
"author": "Laurent Eschenauer <laurent@eschenauer.be>",
|
||||
"license": "MIT"
|
||||
|
||||
Referência em uma Nova Issue
Bloquear um usuário