Bayes filter

What it computes

A Bayes filter tracks a belief: a probability for every place the robot could be. Here the corridor is cut into $N = 23$ cells of 5 cm, so the belief is 23 numbers that sum to 1. Each step has two parts.

Predict with the motion. Told to move $m$ cells, the rover really moves $m-1$, $m$ or $m+1$ with the probabilities in KERNEL, so

$$\bar b(j) = \sum_i p(j \mid i, m)\, b(i).$$

The end walls stop it: probability that would leave the corridor stays in the end cell.

Update with the reading $z$. The floor sensor says "marker" with probability $p_{hit}$ over a marker and $p_{false}$ elsewhere. Weigh each cell by how likely $z$ is there, then normalise:

$$b(j) = \frac{p(z \mid j)\, \bar b(j)}{\sum_k p(z \mid k)\, \bar b(k)}$$

# predict
new = zeros(N)
for i in range(N):
    for d, p in zip((-1, 0, 1), KERNEL):
        j = clamp(i + move + d, 0, N - 1)      # the walls stop the rover
        new[j] += p * belief[i]
# update
for j in range(N):
    if saw_marker: like[j] = P_HIT if MARKS[j] else P_FALSE
    else:          like[j] = 1 - P_HIT if MARKS[j] else 1 - P_FALSE
belief = like * new / sum(like * new)

Why it works

The start is unknown, so the belief starts flat. The first "marker" raises every marker cell at once: the belief is multimodal, one bump per place that fits. Predict slides and blurs the bumps; update shrinks those whose readings don't fit. The blue strips are 10, 15, 5 and 20 cm long, so soon only one fits.

Every filter in this set is this loop. The Kalman filter runs it on a Gaussian belief (always one bump), the particle filter on a belief made of samples.

The program

It senses, then drives 14 cells forward and 14 back, one wheel-measured cell at a time: predict after each move, update after each reading. The belief is drawn above the corridor and in the Images tab.

Your task

Implement predict(belief, move) and update(belief, saw_marker); each returns the new belief. The grader tests both on fixed beliefs, including next to the walls. Live, the peak must end within one cell of the rover with at least half the probability there.