Localization via Montecarlo / Histogram / Bucket Method

https://jsfiddle.net/v9xemn86/3/ A JavaScript example of the basics of localization using the Histogram method. This provides a discrete, multi-modal tracking of position, finding he probability of each possible position being the correct one. This allows for multiple positions to have a high probability of being where we are now. However, it requires an impossible amount of memory when the number of positions increases to a real scale.

The code takes a single point sensor input and applies it to a pattern over multiple movements and readings. Note that the pattern can be a known map of the world, or a pattern we are looking for in the world, e.g. the dark door opening with light trim and dark walls. In an real application, it would be possible to take multiple readings without moving, by calling the sense method without the move. Uncertainty in movement is compensated, as is sensor reading. Note that the uncertainty in movement could be caused by physical movement, or by uncertainty in the range to the target causing different spacing between pixels taken from an image. The results are shown as a probability map where grey scale represents the likelihood that the robot is sensing the pattern. The disadvantage of the Histogram method is that it's need for memory scales exponentially with the number of position variables. Here we use only one (e.g. angle between the robot and target) but in real life localization requires far more dimensions.

Code: 

function print(msg) { //quick and dirty way to get data out.
	document.body.innerHTML+="<br>"+msg;
  }
function print_image(msg,v) { // print message and image from the vector 
	for (var i=0,s=""; i<v.length; i++) {
  	var c=Math.floor(v[i]*200+50); // expand to 0...255 and make integer
  	s+="<td style='background-color:rgb("+c+","+c+","+c+")'>&nbsp;&nbsp;</td>"
    } //the background of the cell displays the color.
  print("<table cellpadding=0><tr>"+s+"<td>"+msg+"</td></tr></table>")
}

try {
// This is the world as we have been told it exists... or...
// the pattern we are looking for in the world.
pattern = [1,1,0,1,0,1,1];
print_image("Starting pattern",pattern);

// This is what we remember about what we have seen. 
memory = new Array(pattern.length);
// It starts uniformly unsure. Each cell is equally likely, total is 1
for (var i=0;i<pattern.length;i++) memory[i]=1/pattern.length;
//print(memory); //should show uniform probability, total=1

function mod(i, m) { 	//circular modulus is
  var r = i % m;			//regular modulus
  if (r<0) r+=m;  		//with a wrap if negative
  return r;
	}

// Sensors arn't always totally accurate.
wHit = 0.9; //weight of a match between sensor and memory
wMiss = 0.1; //weight of a mismatch 

function sense(memory, pattern, input) {
	output = []; //new memory state
  var s=0;
  for (var i=0; i<memory.length;i++) {
  	var d = 1-Math.abs(pattern[i] - input); //compare pattern with input
    var p = memory[i] * (d * wHit + (1-d) * wMiss); //new probabilities
    output.push(p);
    s += p; //sum probabilities. 
		}
	for (var i=0;i<output.length;i++) output[i] /= s; //normalize
	return output;
	}
print_image("See a zero, or",sense(memory, pattern, 0))

// Robots don't alway move exactly as commanded. Wheels slip, etc...
wExactMove = 0.8; // probibility that we will move exactly 1 cell
wOvershoot = (1-wExactMove) / 2; // Remaining chance is half over...
wUndershoot = (1-wExactMove) / 2; // and half undershoot when moving.

function move(memory, dp) { //update memory based on delta position.
	output = []; //new memory state
  var s=0;	// sum of the probability of the input we expect in this cell
  var l=memory.length;
  for (var i=0; i<l;i++) { // each new cell is based on one before the move
  	s = memory[mod(i-dp,l)] * wExactMove; // cell we moved from 
  	s += memory[mod(i-dp+1,l)] * wUndershoot; // cell one short 
  	s += memory[mod(i-dp-1,l)] * wOvershoot; // cell one farther
  	output.push(s); 
	  }
  return output;
	}
//Note: We are just testing, so using pattern as memory.
print_image("Move right one",move(pattern,1));

events = [['right',1],['right',0],['right',1],['right',0],['left',1]];
for (var i=0; i<events.length; i++) {
	if ('left'==events[i][0]) { memory=move(memory,-1) }
	if ('right'==events[i][0]) { memory=move(memory,1) }
	memory=sense(memory,pattern,events[i][1]);
	print_image("Moved "+events[i][0]+" saw "+events[i][1], memory)
	}
}
catch(e) { print("ERROR: "+e.message);}

Code:

See also: