目的

パーティクルフィルタを勉強したので、JavaScriptで動かしてみる.
といっても最新のJavaScriptに明るいわけではないので、実装が雑なのはお許しを...

めちゃくちゃ単純に実装するのが今回の目標.

参考

下記のサイトと動画を参考に実装しています.

できたもの

長方形の位置を予測するパーティクルフィルタ.
4つのランドマークからの距離を取得して、尤度を計算.
赤、緑、青、黄色のランドマークは動かないものとして、
長方形だけは動かせられる.

予測→更新(重み)→リサンプリング
を繰り返し.

尤度の計算は4つのランドマークからの距離から計算.
システムモデルは前回の位置に移動量とノイズを加えたのみ

やりたいことはできてそう.
思ったよりもパラメータの調整が必要だった...

実装

コードはやや汚いけど、このまま載せておく

HTML

particle_filter.html


<html>
  <head>
    <script src="https://unpkg.com/konva@8.2.3/konva.min.js"></script>
    <meta charset="utf-8" />
    <title>Simple ParticleFilter Demo</title>
    <style>
      body {
        margin: 0;
        padding: 0;
        overflow: hidden;
        background-color: #f0f0f0;
      }
    </style>
  </head>
  <body>
    <div id="container"></div>
  </body>
</html>

JavaScript

particle_filter.js

      var particleNum = 100;
      var defaultRadius = 10;
      var animationInterval = 2000;
      var controlSigma = 200;
      let radiusScaler = 20.0;
      let observationNoise = 5;

      

      function writeMessage(message) {
        text.text(message);
      }
      var width = window.innerWidth;
      var height = window.innerHeight;

      var stage = new Konva.Stage({
        container: 'container',
        width: width,
        height: height,
      });

      var layer = new Konva.Layer();

      var text = new Konva.Text({
        x: 10,
        y: 10,
        fontFamily: 'Calibri',
        fontSize: 24,
        text: '',
        fill: 'black',
      });


      
      var circles =[];
      for(var i = 0; i < particleNum; i++){
	  circles[i] =
	      new Konva.Circle({
		  x:  stage.width() * Math.random(),
		  y:  stage.height() * Math.random(),
		  radius: defaultRadius,
		  fill: 'red',
		  stroke: 'rgb(250, 200, 200)',
		  strokeWidth: 2,
	      });
      }

      var lastx=0,lasty=0;
      var speedx=0,speedy=0;

      function calculate4Length(centerx, centery){
	  let lengths = [];
	  for(let i = 0; i < 4; i++){
	      let diffx = centerx - hexagons[i].x();
	      let diffy = centery - hexagons[i].y();
	      lengths[i] = Math.sqrt(diffx * diffx + diffy * diffy);
	  }

	  return lengths
      }
      
      function getObservation(){
	  let boxCX = box.x() + box.width()/2;
	  let boxCY = box.y() + box.height()/2;
	  let lengths = calculate4Length(boxCX, boxCY);

	  for(let i = 0; i < 4; i++){
	      lengths[i] += observationNoise * (Math.random()-0.5)
	      if(lengths[i] < 0){
		  lengths[i] = 0;
	      }
	  }
	  
	  return lengths;
      }

      let likelihoodCoeff = 0.03;
      function likelihood(cx, cy, observation){
	  let lengths = calculate4Length(cx, cy);
	  
	  let diffSum = 0;
	  for(var i = 0; i < 4; i++){
	      let diff = lengths[i] - observation[i];
	      diffSum += diff*diff;
	  }

	  diffSum = Math.sqrt(diffSum);

	  //Very Simple Likelihood Function
	  //Calculate each particle center to 4 landmarks,
	  //squared diff of observation and predicted lengths and finally get exponential value.
	  return Math.exp(-diffSum * likelihoodCoeff);
      }

      
      function predict(){
	  writeMessage('Predict');
	  
	  //Predict with System Model
	  for(var i = 0; i < particleNum; i++){
	      //Very Simple Model : just add latest speed and noise
	      circles[i].x( circles[i].x() + speedx + controlSigma * (Math.random()-0.5));
	      circles[i].y( circles[i].y() + speedy + controlSigma * (Math.random()-0.5));
	  }
	  
	  setTimeout(update, animationInterval);
      }

      var weights = [];
      function update(){
	  //Get Observation
	  let observation  = getObservation();
	  writeMessage('Update');

	  //Calculate Likelihood and Weights
	  weights = [];
	  for(var i = 0; i < particleNum; i++){
	      weights[i] = likelihood(circles[i].x(), circles[i].y(), observation);
	  }

	  let sumWeight = weights.reduce((sum, element) => sum + element, 0);

	  //Resize particles size with wieghts
	  for(var i = 0; i < particleNum; i++){
	      weights[i] = 1.0 * weights[i] / sumWeight;
	      circles[i].radius(Math.min(circles[i].radius() * radiusScaler * weights[i], 80));
	  }

	  setTimeout(resample, animationInterval);
      }

      function resample(){

	  //Resampling the particle position
	  let w = 0.0;
	  let windex = 0;

	  let backupCircleXYs = [];
	  for(var i = 0; i < particleNum; i++){
	      backupCircleXYs[i] = {x:circles[i].x(), y:circles[i].y()};
	  }
	  
	  for(var i = 0; i < particleNum; i++){
	      let unitsize = 1.0 * (i+1) / particleNum;
	      if( unitsize <= w ){
		  
	      }else{
		  //until w become bigger than unitsize
		  for(var j = windex+1; j < particleNum; j++){
		      w += weights[j];

		      if( unitsize <= w){
			  windex = j;
			  break;
		      }
		  }
	      }
	      
	      circles[i].x(backupCircleXYs[windex].x);
	      circles[i].y(backupCircleXYs[windex].y);
	  }
	  
	  //Restore particles size
	  writeMessage('Resample');
	  for(var i = 0; i < particleNum; i++){
	      circles[i].radius(defaultRadius);
	  }

	  setTimeout(predict, animationInterval);
      }
      setTimeout(predict, animationInterval);

      var lines = [];
      var hexagons = [];
      var colors = ["red", "green", "blue", "orange"];

      var box = new Konva.Rect({
        x: 20,
        y: 100,
        offset: [50, 25],
        width: 100,
        height: 50,
        fill: '#00D2FF',
        stroke: 'black',
        strokeWidth: 4,
        draggable: true,
      });

      box.on('dragmove', function () {
	  for(var j = 0; j < 4; j++){	  
	      lines[j].points([box.x()+box.width()/2, box.y()+box.height()/2, hexagons[j].x(), hexagons[j].y()]);
	  }

	  //Calculate Velocity
	  let currentx = box.x();
	  let currenty = box.y();
	  speedx = currentx - lastx;
	  speedy = currenty - lasty;

	  lastx = currentx;
	  lasty = currenty;
      });


      for(var j = 0; j < 4; j++){	  
	  hexagons[j] = new Konva.RegularPolygon({
	      x:  stage.width() * (j%2+1)/3,
	      y:  stage.height() * (Math.floor(j/2)+1)/3,
              sides: 6,
              radius: 50,
              fill: colors[j],
              stroke: "white",
              strokeWidth: 2,
	      shadowColor: 'black',
              shadowBlur: 0,
              shadowOffset: { x: 10, y: 10 },
              shadowOpacity: 0.5,
	  });


	  // dashed line
	  lines[j] = new Konva.Line({
              points: [box.x()+box.width()/2, box.y()+box.height()/2, hexagons[j].x(), hexagons[j].y()],
              stroke: 'green',
              strokeWidth: 2,
              lineJoin: 'round'
	  });

	  layer.add(hexagons[j]);
	  layer.add(lines[j]);
      }

     
      layer.add(text);
      layer.add(box);
      for(var i = 0; i < particleNum; i++){
	  layer.add(circles[i]);
      }      

      // add the layer to the stage
      stage.add(layer);