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+")'> </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:
©2024 These pages are served without commercial sponsorship. (No popup ads, etc...).Bandwidth abuse increases hosting cost forcing sponsorship or shutdown. This server aggressively defends against automated copying for any reason including offline viewing, duplication, etc... Please respect this requirement and DO NOT RIP THIS SITE. Questions? <A HREF="http://linistepper.com/techref/method/histogramfilter_localization.htm"> Multi-Modal Localization via Montecarlo / Histogram / Bucket Method</A> |
Did you find what you needed? |