目的
パーティクルフィルタを勉強したので、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);