Bayes' formula for updating probabilistic map

Question

I'm trying to get a mobile robot to map an arena based on what it can see from a camera. I've created a map, and managed to get the robot to identify items placed in the arena and give an estimated location, however, as I'm only using an RGB camera the resulting numbers can vary slightly ever frame due to noise, or change in lighting, etc. What am now trying to do is create a probability map using Bayes' formula to give a better map of the arena.

Bayes' Formula

 P(i | x) =  (p(i)p(x|i))/(sum(p(j)(p(x|j))

This is what I've got so far. All points on the map are initialised to 0.5.

// Gets the Likely hood of the event being correct
// Para 1 = Is the object likely to be at that location
// Para 2 = is the sensor saying it's at that location
private double getProbabilityNum(bool world, bool sensor)
{

    if (world && sensor)
    {
    // number to test the function works
        return 0.6;
    }
    else if (world && !sensor)
    {
    // number to test the function works
        return 0.4;
    }
    else if (!world && sensor)
    {
    // number to test the function works
        return 0.2;
    }
    else //if (!world && !sensor)
    {
    // number to test the function works
        return 0.8;
    }
}


// A function to update the map's probability of an object being at location (x,y)
// Para 3 = does the sensor pick up the an object at (x,y)
public double probabilisticMap(int x,int y,bool sensor)
{
    // gets the current likelihood from the map (prior Probability)
    double  mapProb = get(x,y);
    //decide if object is at location (x,y)
    bool world = (mapProb < threshold);
     //Bayes' formula to update the probability
     double newProb =
(getProbabilityNum(world, sensor) * mapProb) / ((getProbabilityNum(world, sensor) * mapProb) + (getProbabilityNum(!world, sensor) * (1 - mapProb)));

        // update the location on the map
        set(x,y,newProb);
        // return the probability as well
        return newProb;
    }

It does work, but the numbers seem to jump rapidly, and then flicker when they are at the top, it also errors if the numbers drop too near to zero. Anyone have any idea why this might be happening? I think it's something to do with the way the equations is coded, but I'm not too sure. (I found this, but I don't quite understand it, so I'm not sure of it's relevents, but it seems to be talking about the same thing

Thanks in Advance.

Answer 1

Use log-likelihoods when doing numerical computations involving probabilities.

Consider

P(i | x) = (p(i)p(x|i))/(sum(p(j)(p(x|j)).

Because x is fixed, the denominator, p(x), is a constant. Thus

P(i | x) ~ p(i)p(x|i)

where ~ denotes "is proportional to."

The log-likelihood function is just the log of this. That is,

L(i | x) = log(p(i)) + log(p(x|i)).