개요

사진

샘플 코드

function KalmanFilter(rate, pnoise, mnoise) {
  this.F = math.matrix([[1, rate], [0, 1]]);
  this.G = math.matrix([[math.square(rate) / 2], [rate]]);
  this.H = math.matrix([1, 0]);
  this.Q = math.multiply(math.multiply(this.G, math.transpose(this.G)), pnoise);
  this.R = mnoise;
  this.P = this.Q;
}
KalmanFilter.prototype.update = function(m) {
    if (!this.X)
  {
        this.X = math.matrix([[m], [0]]);
    }
    this.X = math.multiply(this.F, this.X);
    this.P = math.add(math.multiply(math.multiply(this.F, this.P), math.transpose(this.F)), this.Q); 
    var K = math.multiply(math.multiply(this.P, math.transpose(this.H)), math.inv(math.add(math.multiply(math.multiply(this.H, this.P), math.transpose(this.H)), this.R)));
    this.X = math.add(this.X, math.multiply(K, math.subtract(m, math.multiply(this.H, this.X))));
    this.P = math.multiply(math.subtract(math.eye(2), math.multiply(K, this.H)), this.P);
    return this.X._data[0];
}

var Agent = function() {
    this.err2 = 0;
  this.err = 0;
  var rate = 0.5;
  var pnoise = 0.1;
  var mnoise = 0.11;
  this.kf = new KalmanFilter(rate, pnoise, mnoise);
};
Agent.prototype.get_action = function(input, reward, done) {
  var setPoint = 0;
  var dt = 10;
  var Kp = 0.03;
  var Kd = 0.2;
  var input2;
    input2 = this.kf.update(input);
    var error = setPoint - input2;
    var u = Kp * error - (Kd * (input2 - this.err2)) / dt;
    this.err2 = this.err;
    this.err = input;
    return u;
}
function drawWorld(world, context) {
    for (var j = world.m_jointList; j; j = j.m_next) 
  {
        drawJoint(j, context);
    }
    for (var b = world.m_bodyList; b; b = b.m_next)
  {
        for (var s = b.GetShapeList(); s != null; s = s.GetNext()) 
    {
            drawShape(s, context);
        }
    }
}
function drawJoint(joint, context) {
    var b1 = joint.m_body1;
    var b2 = joint.m_body2;
    var x1 = b1.m_position;
    var x2 = b2.m_position;
    var p1 = joint.GetAnchor1();
    var p2 = joint.GetAnchor2();
    context.strokeStyle = '#00eeee';
    context.beginPath();
    switch (joint.m_type)
  {
    case b2Joint.e_distanceJoint:
        context.moveTo(p1.x, p1.y);
        context.lineTo(p2.x, p2.y);
    break;
    case b2Joint.e_pulleyJoint:
    break;
    default:
        if (b1 == world.m_groundBody)
    {
            context.moveTo(p1.x, p1.y);
            context.lineTo(x2.x, x2.y);
        }
        else if (b2 == world.m_groundBody)
    {
            context.moveTo(p1.x, p1.y);
            context.lineTo(x1.x, x1.y);
        }
        else
    {
            context.moveTo(x1.x, x1.y);
            context.lineTo(p1.x, p1.y);
            context.lineTo(x2.x, x2.y);
            context.lineTo(p2.x, p2.y);
        }
    break;
    }
    context.stroke();
}
function drawShape(shape, context) {
    context.strokeStyle = '#ff0000';
    context.beginPath();
    switch (shape.m_type) 
  {
    case b2Shape.e_circleShape:
    {
            var circle = shape;
            var pos = circle.m_position;
            var r = circle.m_radius;
            var segments = 16.0;
            var theta = 0.0;
            var dtheta = 2.0 * Math.PI / segments;
            context.moveTo(pos.x + r, pos.y);
            for (var i = 0; i < segments; i++)
      {
                var d = new b2Vec2(r * Math.cos(theta), r * Math.sin(theta));
                var v = b2Math.AddVV(pos, d);
                context.lineTo(v.x, v.y);
                theta += dtheta;
            }
            context.lineTo(pos.x + r, pos.y);
            context.moveTo(pos.x, pos.y);
            var ax = circle.m_R.col1;
            var pos2 = new b2Vec2(pos.x + r * ax.x, pos.y + r * ax.y);
            context.lineTo(pos2.x, pos2.y);
    }
    break;
    case b2Shape.e_polyShape:
    {
            var poly = shape;
            var tV = b2Math.AddVV(poly.m_position, b2Math.b2MulMV(poly.m_R, poly.m_vertices[0]));
            context.moveTo(tV.x, tV.y);
            for (var i = 0; i < poly.m_vertexCount; i++)
      {
                var v = b2Math.AddVV(poly.m_position, b2Math.b2MulMV(poly.m_R, poly.m_vertices[i]));
                context.lineTo(v.x, v.y);
            }
            context.lineTo(tV.x, tV.y);
    }
    break;
    }
    context.stroke();
}
var worldAABB = new b2AABB();   
worldAABB.minVertex.Set(-1000, -1000);
worldAABB.maxVertex.Set(1000, 1000);
var gravity = new b2Vec2(0, 300);
var doSleep = false;        
var world = new b2World(worldAABB, gravity, doSleep);
var wheelSd = new b2CircleDef();
wheelSd.density = 1.0;
wheelSd.radius = 40;
var wheelBd = new b2BodyDef();
wheelBd.AddShape(wheelSd);
wheelBd.position.Set(200, 290);
var wheel = world.CreateBody(wheelBd);
var carSd = new b2BoxDef();
carSd.density = 1.0;
carSd.extents.Set(20, 60);
var carBd = new b2BodyDef();
carBd.AddShape(carSd);
carBd.position.Set(200, 250);
var car = world.CreateBody(carBd);
var pinJd = new b2RevoluteJointDef();
pinJd.body1 = wheel;
pinJd.body2 = car;
pinJd.anchorPoint = wheel.GetCenterPosition();
pinJd.enableMotor = true;
pinJd.motorTorque = 100000000;
pinJd.motorSpeed = -0.9;
var frontPin = world.CreateJoint(pinJd);
var groundSd = new b2BoxDef();
groundSd.extents.Set(2000, 50);
groundSd.restitution = 0.2;
var groundBd = new b2BodyDef();
groundBd.AddShape(groundSd);
groundBd.position.Set(-500, 400);
var ground = world.CreateBody(groundBd);
Event.observe(window, 'load', function(e) {
  var context = document.querySelector('#c').getContext('2d');
    var timeStep = 1.0 / 30;
    var iteration = 1;
  var agent = new Agent();
    setInterval(function() {
        context.clearRect(0, 0, 400, 400);
        world.Step(timeStep, iteration);
        drawWorld(world, context);
    var observation = car.m_linearVelocity.x;
    var reward = 1.0;
    var done = false;
    var u = agent.get_action(observation, reward, done);
    frontPin.SetMotorSpeed(u);
    var str = "observation: " + observation + "<br> action: " + u;
    document.getElementById('helloWorld').innerHTML = str;
    }, timeStep);
});

아티팩트