Localization

by pomber

HTML

<script src="http://colorjizz.googlecode.com/svn/trunk/Javascript/ColorJizz.js"></script>
<div class="world">
    <table class="board">
    </table>
</div>

<div class="localization">
    <table class="board">
    </table>    
</div>
<a href="http://www.udacity-forums.com/cs373/questions/5548/2d-localization-demo" style="margin:20px">Go to forum thread</a>

CSS

table.board {
    margin: 5px;
    border-collapse: separate;
    border-spacing: 2px;
    border: 5px solid #333;
}

.board td {
    height: 50px; 
    width: 50px; 
    text-align: center;
    vertical-align: middle;
}

.world .board td {
    font-size: 40px;
}

.localization .board td {
    color: white;
}

td.black {
    background-color: #555;
    color: white;
}

td.white{
    background-color: #BBB;
    color: black;    
}

JavaScript

/************* World **************/

var rowCount = 3;
var colCount = 8;
var currentRow = null;
var currentCol = null;

var pMove = 0.95;
var pStall = 1 - pMove;

var pSensorRight = 0.95;
var pSensorFail = 1 - pSensorRight;

function moveRobot(rowDelta, colDelta) {
    if (Math.random() < pMove) {
        currentRow = displaceRow(currentRow, rowDelta);
        currentCol = displaceCol(currentCol, colDelta);
    }
}

function senseWorld() {
    var currentCellColor = cellColor(currentRow, currentCol);
    var otherColor = currentCellColor == "black" ? "white" : "black";
    
    if (Math.random() < pSensorRight) {
        return currentCellColor;
    } else {
        return otherColor;
    }
}

/********** Localization **********/

var belief = null;

function createInitialBelief() {
    var maximumConfusion = 1 / (rowCount * colCount);    
    belief = new Array(rowCount);
    for (var row = 0; row < rowCount; row++) {
        belief[row] = new Array(colCount);
        for (var col = 0; col < colCount; col++) {
            belief[row][col] = maximumConfusion;
        }
    }    
}

function updateBelief(color) {
    var sum = 0;
    for (var row = 0; row < rowCount; row++) {
        for (var col = 0; col < colCount; col++) {
            belief[row][col] *= cellColor(row, col) == color ? pSensorRight : pSensorFail;
            sum += belief[row][col];
        }
    }
    
    for (var row = 0; row < rowCount; row++) {
        for (var col = 0; col < colCount; col++) {
            belief[row][col] /= sum;
        }
    }
}

function moveBelief(rowDelta, colDelta) {
    var newBelief = new Array(rowCount);
    for (var row = 0; row < rowCount; row++) {
        newBelief[row] = new Array(colCount);
        for (var col = 0; col < colCount; col++) {
            newBelief[row][col] = cellNewBelief(row, col, rowDelta, colDelta);
        }
    }
    
    belief = newBelief;
}

function cellNewBelief(row, col, rowDelta, colDelta) {
    var previousRow = displaceRow(row,...