Discrete Bayes Filter

This section is adapted from Kalman and Bayesian Filters in Python by Roger Labbe. The Kalman filter belongs to a family of filters called Bayesian filters. Textbooks often present them at an abstract level, starting from Bayes’ formula, which leaves you to build the intuition yourself. This section builds that intuition with a concrete problem. You track a dog as it walks along a hallway, using a noisy sensor that can tell a door from a wall. The approach follows Dieter Fox and Sebastian Thrun, who used a robot in a hallway [1, 2, 3]. You describe what you know about the dog’s position as a probability distribution over a small set of hallway positions. Before you use a measurement, this distribution is the prior. After you fold in the measurement, it is the posterior. The filter repeats two steps. The prediction step moves the belief forward in time by convolving it with a model of how the dog moves, and this spreads the probability out. The update step multiplies the prior by the likelihood of the measurement and normalizes the result. That is Bayes’ theorem, and it sharpens the belief. Because the belief is a full distribution, it can hold several hypotheses at once. A multimodal belief says the dog is probably at one of a few doors, without committing to any of them yet. The figure shows the cycle. You start from initial conditions

x_0

and predict where the dog will be. A measurement

z_k

then corrects that prediction. Each pass around the loop yields a state estimate

\hat{x}_k

Tracking a dog

Suppose your workplace is dog friendly, and dogs sometimes wander out of offices and down the halls. A sonar sensor on a dog’s collar emits a signal and listens for the echo. From the echo it can tell whether the dog is in front of an open doorway or a wall. It also senses when the dog walks and reports the direction. It sends an update once a second. You want to track a dog named Simon. The sensor reports “door”, “hall”, “hall”, and so on. How can you turn that stream into a position? To keep the problem small, assume the hallway has only 10 positions, numbered 0 to 9, where 1 is to the right of 0. Assume also that the hallway is circular: moving right from position 9 takes you to position 0. When you begin listening, you have no reason to believe Simon is at any particular position. Each of the 10 positions is equally likely, so each has probability 1/10. You store this belief in an array.

belief = np.array([1 / 10] * 10)
print(belief)

[0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1]

In Bayesian statistics this is called a prior: the probability before you incorporate measurements or other information. More completely, it is the prior probability distribution. A probability distribution lists every possible event and the probability of each. It always sums to 1, because one of the events had to happen. Bayesian statistics treats probability as a belief about a single event. If you flip a fair coin many times, half the flips land heads. That statement is about frequencies, which is the view of frequentist statistics. Now flip the coin once and cover it. The coin is either heads or tails, and you do not know which. A Bayesian says the strength of your belief that this flip is heads is 50%. In this section “belief” always means a measure of the strength of your knowledge, not a claim held without evidence. Bayesian reasoning also uses past information. If it rains 4 days in every 100, you could say the chance of rain tomorrow is 1/25. Weather prediction does not work that way. If it is raining today and the storm front has stalled, rain tomorrow is likely. When this section talks about the probability of something, it means the probability that a specific thing is true given past events. Now build a map of the hallway. Put the first two doors close together and a third door further away. Use 1 for a door and 0 for a wall.

hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])

The first reading from the sensor is “door”. For the moment, assume the sensor is always right. Simon is in front of a door, but which one? You have no reason to prefer the first, second, or third door, so you assign probability 1/3 to each.

belief = np.array(
    [1 / 3, 1 / 3, 0, 0, 0, 0, 0, 0, 1 / 3, 0]
)

This is a categorical distribution: a discrete distribution over

n

outcomes. It is also a multimodal distribution, because it has more than one peak. It does not say Simon is in three places at once. It says your knowledge has narrowed to one of three places, with a 1/3 chance for each of doors 0, 1, and 8. Your knowledge improved in two ways. You ruled out most positions, and your belief in the remaining ones rose from 10% to 33%. As knowledge improves, the probabilities move toward 100%. The mode of a set of numbers is the value that occurs most often. In

\{1, 2, 2, 2, 3, 3, 4\}

the mode is 2. The set

\{1, 2, 2, 2, 3, 3, 4, 4, 4\}

has two modes, 2 and 4. The first set is unimodal and the second is multimodal. A bar chart like the one above is also called a histogram. You do not need to type the belief by hand. Doors are 1 and walls are 0, so you can scale the hallway map.

belief = hallway * (1 / 3)
print(belief)

[0.33333333 0.33333333 0.         0.         0.
 0.         0.         0.         0.33333333 0.        ]

Extracting information from sensor readings

Set the code aside for a moment. Suppose the sensor reports this sequence:

door
move right
door

Can you tell where Simon is? Yes. Only one starting point in this hallway produces that sequence: the left end. So Simon is now in front of the second door. If he had started at the second or third door, moving right would have put him in front of a wall, and the sensor would have said “wall”. The same reasoning rules out every other start. Your belief is:

belief = np.array(
    [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
)

This hallway and sequence were chosen to give an exact answer quickly. Real problems are rarely so clean. Still, notice the pattern. The first reading gave only a 1/3 probability per door, but one move and one more reading pinned Simon down. In a long hallway with many doors, a handful of readings and moves would either locate him or narrow him to a few positions. That works whenever a sequence of readings matches only one or a few starting points. Real sensors add a complication, which the next part handles.

Noisy sensors

Perfect sensors are rare. The sensor might miss a door if Simon sits in front of it to scratch himself, or misread if he is not facing down the hall. So when you read “door” you cannot give each door probability 1/3. You have to give each door a little less, and each wall a little more than zero, something like

[.31, .31, .01, .01, .01, .01, .01, .01, .31, .01]

If the sensor is noisy, every reading is in doubt. How can you conclude anything? The answer again is probability. You already hold a probabilistic belief about the dog’s position. Now you fold the sensor’s uncertainty into it. Suppose the reading is “door”, and tests show the sensor is 3 times more likely to be right than wrong. Then you scale the belief by 3 wherever there is a door. The result no longer sums to 1, but you fix that in a moment. The measurement is called z. The letters

z

and

y

are the usual names for a measurement in the literature.

def update_belief(hall, belief, z, correct_scale):
    for i, val in enumerate(hall):
        if val == z:
            belief[i] *= correct_scale


belief = np.array([0.1] * 10)
reading = 1  # 1 is 'door'
update_belief(hallway, belief, z=reading, correct_scale=3.0)
print("belief:", belief)
print("sum =", sum(belief))

belief: [0.3 0.3 0.1 0.1 0.1 0.1 0.1 0.1 0.3 0.1]
sum = 1.6000000000000003

This is not a probability distribution, because it does not sum to 1. The doors do have a value (0.3) that is 3 times the walls (0.1), which is right. All that remains is to normalize: divide each element by the sum of all elements.

belief / sum(belief)

array([0.1875, 0.1875, 0.0625, 0.0625, 0.0625, 0.0625,
       0.0625, 0.0625, 0.1875, 0.0625])

The normalization step is a function called normalize. “3 times as likely to be right as wrong” is an awkward way to state accuracy. It is more natural to give the probability that the sensor is correct and derive the scale factor from it:

\text{scale} = \frac{p_{\text{correct}}}{p_{\text{incorrect}}} = \frac{p_{\text{correct}}}{1 - p_{\text{correct}}}

The for loop can also go. Comparing the hallway with a value gives a boolean mask:

hallway == 1

array([ True,  True, False, False, False, False, False,
       False,  True, False])

Indexing an array with a boolean mask selects only the elements where the mask is True. So the loop becomes

belief[hall == z] *= scale

and only the elements that equal z are multiplied by scale. Here is the improved version:

from filterpy.discrete_bayes import normalize


def scaled_update(hall, belief, z, z_prob):
    scale = z_prob / (1.0 - z_prob)
    belief[hall == z] *= scale
    normalize(belief)


belief = np.array([0.1] * 10)
scaled_update(hallway, belief, z=1, z_prob=0.75)

print("sum =", sum(belief))
print("probability of door =", belief[0])
print("probability of wall =", belief[2])

sum = 1.0
probability of door = 0.1875
probability of wall = 0.06249999999999999

The sum is now 1, and a door is still three times as probable as a wall. This matches intuition. The probability of a door must be below 1/3, and the probability of a wall must be above zero. You also have no information yet that separates one door from another, so all doors share one value and all walls share another. This result is the posterior, short for posterior probability distribution: the distribution after incorporating the measurement. The prior is the distribution before the measurement. The term for the scaling is the likelihood. When you computed belief[hall == z] *= scale, you computed how likely each position is given the measurement. A likelihood is not a probability distribution, because it need not sum to 1. Together these give

\text{posterior} = \frac{\text{likelihood} \times \text{prior}}{\text{normalization}}

When you talk about a filter’s output, the state after prediction is the prior or prediction. The state after the update is the posterior or estimated state. Most of the literature uses these terms, so they are worth learning well. scaled_update() performs exactly this computation. Recast into that form, it reads:

def scaled_update(hall, belief, z, z_prob):
    scale = z_prob / (1.0 - z_prob)
    likelihood = np.ones(len(hall))
    likelihood[hall == z] *= scale
    return normalize(likelihood * belief)

This function still knows about the hallway and how to match a measurement against it. A general update should not. So move the likelihood out of the function and let the caller compute it. The general update is one line:

def update(likelihood, prior):
    return normalize(likelihood * prior)

How you compute the likelihood depends on the problem. The sensor might return a number between 0 and 1 rather than a hard door or wall. It might be a camera that reports a blob shape, which you match to a door with some probability. It might be sonar that returns a distance. Each case needs its own likelihood, but the update stays the same. Here is the previous example in general form:

from filterpy.discrete_bayes import update


def lh_hallway(hall, z, z_prob):
    """compute likelihood that a measurement matches
    positions in the hallway."""

    try:
        scale = z_prob / (1.0 - z_prob)
    except ZeroDivisionError:
        scale = 1e8

    likelihood = np.ones(len(hall))
    likelihood[hall == z] *= scale
    return likelihood


belief = np.array([0.1] * 10)
likelihood = lh_hallway(hallway, z=1, z_prob=0.75)
update(likelihood, belief)

array([0.1875, 0.1875, 0.0625, 0.0625, 0.0625, 0.0625,
       0.0625, 0.0625, 0.1875, 0.0625])

Incorporating movement

With perfect sensors, a few readings and one move gave an exact answer. Can you get an exact answer with noisy sensors? No. Even if every reading matches a complicated hallway map perfectly, there is a tiny chance that every reading was wrong. In practice most readings are right, and you can get close to 100% sure, but never all the way. Start with the simple case. The movement sensor is perfect, and it reports that the dog moved one space to the right. How should the belief change? Shift every value one space to the right. If you were 50% sure Simon was at position 3, then after he moves right you are 50% sure he is at position 4. The hallway is circular, so the shift wraps around with modular arithmetic.

def perfect_predict(belief, move):
    """move the position by `move` spaces, where positive is
    to the right, and negative is to the left
    """
    n = len(belief)
    result = np.zeros(n)
    for i in range(n):
        result[i] = belief[(i - move) % n]
    return result


belief_before = np.array(
    [0.35, 0.1, 0.2, 0.3, 0, 0, 0, 0, 0, 0.05]
)
belief_after = perfect_predict(belief_before, 1)

Every value moved one position to the right, and the last value wrapped around to the start. If Simon keeps walking and you only predict, with no new measurements, the distribution keeps its shape and only slides along the hallway. The figure shows the belief over the first several steps.

belief = np.array(
    [0.35, 0.1, 0.2, 0.3, 0, 0, 0, 0, 0, 0.05]
)
perfect_beliefs = [belief]

for _ in range(20):
    # Simon takes one step to the right
    belief = perfect_predict(belief, 1)
    perfect_beliefs.append(belief)

Terminology

The system is what you are trying to model or filter. Here the system is the dog. The state is the system’s current configuration or value. In this section the state is the dog’s position. You rarely know the true state, so a filter produces an estimated state. In practice people often call that “the state”, so watch the context. One cycle of prediction and update is called the state or system evolution, short for time evolution [7]. Another name is system propagation. It describes how the state changes over time. For filters, time usually moves in discrete steps, such as 1 second. For the dog tracker, the state evolution is the dog’s position after one step of time. You model the system’s behavior with the process model. Here the process model says the dog moves one or more positions each time step. That is not a very accurate model of dogs. The error in the model is the system error or process error. The prediction is your new prior. Time has moved forward, and you predicted without the benefit of a measurement. Here is an example. The dog is at 17 m. The time step is 2 seconds, and the dog moves at 15 m/s. Where will he be in two seconds?

\begin{aligned} \bar x &= 17 + (15 \cdot 2) \\ &= 47 \end{aligned}

A bar over a variable marks it as a prior (a prediction). You can write the process model as

\bar x_{k+1} = f_x(\bullet) + x_k

Here

x_k

is the current position or state. If the dog is at 17 m then

x_k = 17

. The function

f_x(\bullet)

is the state propagation function for

x

. It describes how much

x_k

changes over one time step. In the example it computes

15 \cdot 2

, so you would define it as

f_x(v_x, t) = v_x t

Adding uncertainty to the prediction

perfect_predict() assumes perfect movement measurements, but every sensor has noise. What if the sensor said the dog moved one space, but he really moved two, or none? Model it. Assume the movement measurement is right 80% of the time, overshoots by one position 10% of the time, and undershoots by one position 10% of the time. If the sensor reports a move of 4 spaces to the right, the dog moved 4 spaces with probability 0.8, 3 spaces with probability 0.1, and 5 spaces with probability 0.1. Each entry of the new belief now combines three cases. Suppose the reported move is 2 and you are certain the dog started at position 3. Then he is at 5 with probability 0.8, and at 4 or 6 with probability 0.1 each.

def predict_move(belief, move, p_under, p_correct, p_over):
    n = len(belief)
    prior = np.zeros(n)
    for i in range(n):
        prior[i] = (
            belief[(i - move) % n] * p_correct
            + belief[(i - move - 1) % n] * p_over
            + belief[(i - move + 1) % n] * p_under
        )
    return prior


belief = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
prior = predict_move(belief, 2, 0.1, 0.8, 0.1)

That matches the reasoning. Now try a belief that is not certain.

belief = [0, 0, 0.4, 0.6, 0, 0, 0, 0, 0, 0]
prior = predict_move(belief, 2, 0.1, 0.8, 0.1)
prior

array([0.  , 0.  , 0.  , 0.04, 0.38, 0.52, 0.06, 0.  ,
       0.  , 0.  ])

You can still check these numbers by hand. The 0.04 at position 3 comes from the 0.4 belief undershooting by one. The 0.38 at position 4 has two sources: the 0.4 belief moving exactly 2 (

0.4 \times 0.8

), and the 0.6 belief undershooting (

0.6 \times 0.1

). Overshooting plays no role at position 4, because an overshoot from either 0.4 or 0.6 lands past it. Much of what follows depends on this step, so work a few examples of your own until it is clear. Compare the belief before and after. You started with 0.4 and 0.6 in two positions. After the prediction the peak is lower and the probability is spread over more positions. This is not a quirk of the example. It always happens in the prediction step. With a noisy movement sensor you lose some information on every prediction. Predict forever and you end up knowing nothing: the probability spreads evenly over the whole hallway. Here are 100 predictions from a start you are certain of.

belief = np.array([1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
predict_beliefs = []

for i in range(100):
    belief = predict_move(belief, 1, 0.1, 0.8, 0.1)
    predict_beliefs.append(belief)

print("final belief:")
print(belief)

final belief:
[0.10407069 0.10329322 0.10125784 0.09874205 0.09670682
 0.09592945 0.09670682 0.09874205 0.10125784 0.10329322]

After 100 predictions almost all information is gone, even though you started 100% sure that Simon was at position 0. After 50 steps a fair amount is left. By 200 steps essentially nothing is.

Generalizing with convolution

So far the movement error is at most one position. It could be two, three, or more. You want one method that handles any error model. That method is convolution. Convolution modifies one function with another. Here it modifies a probability distribution with the error function of the movement sensor. predict_move() is already a convolution, under another name. Formally, convolution is

(f \ast g) (t) = \int_0^t \!f(\tau) \, g(t-\tau) \, \mathrm{d}\tau

where

f \ast g

means “convolve

f

with

g

”. It does not mean multiply. The integral is for continuous functions. For discrete ones, replace the integral with a sum and the parentheses with brackets:

(f \ast g) [t] = \sum\limits_{\tau=0}^t \!f[\tau] \, g[t-\tau]

predict_move() computes exactly this sum of products. The idea is to slide an array called the kernel across the belief, multiply neighboring cells by the kernel values, and sum. The earlier example used 0.8 for moving the right amount, 0.1 for undershooting, and 0.1 for overshooting. As a kernel that is [0.1, 0.8, 0.1]. The code below loops over each element of the belief, multiplies its neighborhood by the kernel, and sums. The belief is named pdf to stress that it is a probability distribution.

def predict_move_convolution(pdf, offset, kernel):
    N = len(pdf)
    kN = len(kernel)
    width = int((kN - 1) / 2)

    prior = np.zeros(N)
    for i in range(N):
        for k in range(kN):
            index = (i + (width - k) - offset) % N
            prior[i] += pdf[index] * kernel[k]
    return prior

This shows the algorithm, but the double loop is slow. A faster form first shifts the belief by offset and then does a circular (“wrap”) convolution with the kernel:

convolve(np.roll(pdf, offset), kernel, mode='wrap')

That one line is the predict function used from here on.

from filterpy.discrete_bayes import predict

belief = [
05,
05,
05,
05,
55,
05,
05,
05,
05,
05,
]
prior = predict(belief, offset=1, kernel=[0.1, 0.8, 0.1])

Only the middle values change. Positions 4 and 6 should each be

(0.1 \times 0.05) + (0.8 \times 0.05) + (0.1 \times 0.55) = 0.1

and position 5 should be

(0.1 \times 0.05) + (0.8 \times 0.55) + (0.1 \times 0.05) = 0.45

Next, check that it shifts correctly for moves larger than one and for an asymmetric kernel.

prior = predict(
    belief, offset=3, kernel=[0.05, 0.05, 0.6, 0.2, 0.1]
)

The peak moved by 3 positions, and the kernel gives more weight to overshooting than to undershooting, so the result looks right. Keep in mind what this step does. You predict where the dog moves, and you convolve the probabilities to get the prior. Without probabilities you would use the equation from earlier:

\bar x_{k+1} = x_k + f_{\mathbf x}(\bullet)

The prior is the current position plus the distance moved. The dog was at 10 m, he moved 5 m, so he is now at 15 m. With probabilities, the equation becomes

\bar{ \mathbf x}_{k+1} = \mathbf x_k \ast f_{\mathbf x}(\bullet)

You convolve the current probabilistic position with a probabilistic estimate of how far the dog moved. The idea is the same; the math is slightly different. The bold

\mathbf x

marks an array of numbers.

Integrating measurements and movement updates

Losing information in every prediction might suggest the filter soon knows nothing. But each prediction is followed by an update that folds in a measurement, and the update improves your knowledge. The update’s output feeds the next prediction, which degrades certainty. That goes into the next update, which raises certainty again. Think about it intuitively. You track a dog that is sitting still. Each prediction says he does not move, and your filter quickly converges on his position. Then the kitchen microwave turns on and he races off. You do not know this, so your next prediction still says he is in the same spot. The measurements disagree. As you fold them in, your belief smears along the hallway toward the kitchen. Each cycle, your belief that he is sitting still shrinks, and your belief that he is heading for the kitchen grows. That is the intuition. Now check the math. You already have the predict and update steps. Feed each one’s output into the other and you have a dog tracker. The measurements below are what you would see if the dog started at position 0 and moved right one position per step. As in a real application, you start with no knowledge, so every position gets equal probability.

hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])
prior = np.array([0.1] * 10)
likelihood = lh_hallway(hallway, z=1, z_prob=0.75)
posterior = update(likelihood, prior)

After the first update, each door has a high probability and each wall a low one.

kernel = (0.1, 0.8, 0.1)
prior = predict(posterior, 1, kernel)

The prediction shifted the probabilities to the right and smeared them a little. Now apply the next measurement.

likelihood = lh_hallway(hallway, z=1, z_prob=0.75)
posterior = update(likelihood, prior)

Note the tall bar at position 1. It matches the true story: start at position 0, sense a door, move one to the right, and sense another door. No other position makes this pair of observations as likely. Next, predict again and then sense a wall.

prior = predict(posterior, 1, kernel)
likelihood = lh_hallway(hallway, z=0, z_prob=0.75)
posterior = update(likelihood, prior)

Position 2 now stands out at about 35%. That is more than twice any other bar, and about 4 points higher than the tallest bar in the previous figure (about 31%). Here is one more cycle.

prior = predict(posterior, 1, kernel)
likelihood = lh_hallway(hallway, z=0, z_prob=0.75)
posterior = update(likelihood, prior)

One issue was skipped. The predict step assumed a motion sensor. The microwave story, though, assumed you did not know the dog had started running, and said your belief that he is running would grow over time. No code here does that. How do you detect or estimate changes in the process model when you do not measure them directly? This section sets that problem aside. Solving it needs more mathematics than this section builds, so from here on assume a sensor that reports movement.

The discrete Bayes algorithm

The algorithm is the predict and update cycle drawn at the start of this section. The filter equations are:

\begin{aligned} \bar {\mathbf x} &= \mathbf x \ast f_{\mathbf x}(\bullet)\, \, &\text{Predict Step} \\ \mathbf x &= \|\mathcal L \cdot \bar{\mathbf x}\|\, \, &\text{Update Step}\end{aligned}

\mathcal L

is the usual symbol for the likelihood function. The

\|\cdot\|

notation means normalize: you scale the product of the likelihood and the prior so that

\mathbf x

is a probability distribution that sums to 1. In words:

Initialize your belief in the state.
Predict. Use the system behavior to predict the state at the next time step, and widen the belief to account for the uncertainty in that prediction.
Update. Get a measurement and a belief about its accuracy. Compute how likely each state is given the measurement, and multiply the prior by this likelihood.
Repeat steps 2 and 3.

The one-dimensional Kalman filter uses exactly this algorithm. Only the details of the computation differ. Algorithms of this form are sometimes called predictor correctors: you make a prediction, then correct it. The function below runs the filter over a list of measurements and keeps every prior and posterior, so you can look at any step. In the figures, the light gray bars mark the doors, the orange bars are priors, the blue bars are posteriors, and the green line is where Simon really is. The green line is not an output of the filter. You know it only because the movement is simulated.

def discrete_bayes_sim(
    prior, kernel, measurements, z_prob, hallway
):
    posterior = np.array(prior, dtype=float)
    priors, posteriors = [], []
    for i, z in enumerate(measurements):
        prior = predict(posterior, 1, kernel)
        priors.append(prior)

        likelihood = lh_hallway(hallway, z, z_prob)
        posterior = update(likelihood, prior)
        posteriors.append(posterior)
    return priors, posteriors

Run it with perfect measurements, as if the dog walks around the hallway from position 0.

kernel = (0.1, 0.8, 0.1)
z_prob = 1.0
hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])

# measurements with no noise
zs = [hallway[i % len(hallway)] for i in range(50)]

priors, posteriors = discrete_bayes_sim(
    np.array([0.1] * 10), kernel, zs, z_prob, hallway
)

In each row the prediction (left) shifts the belief and lowers its certainty. The update (right) keeps the position and raises the certainty by folding in the measurement. Here the measurements are perfect, which is what z_prob = 1.0 means. The next part looks at imperfect ones. Note also how sharp the estimate becomes in front of a door, and how it softens in the middle of the hallway. There are only a few doors, so sensing a door says a lot about the position. A long stretch of wall says much less.

The effect of bad sensor data

So far every measurement was correct. A filter should also reject bad measurements. Does this one? To keep the figures readable, change the hallway to mostly alternating doors and walls, and run 6 correct measurements:

hallway = np.array([1, 0, 1, 0, 0] * 2)
kernel = (0.1, 0.8, 0.1)
prior = np.array([0.1] * 10)
zs = [1, 0, 1, 0, 0, 1]
z_prob = 0.75
priors, posteriors = discrete_bayes_sim(
    prior, kernel, zs, z_prob, hallway
)

The filter has identified the two likely starting points, 0 and 5, because both match the sequence door, wall, door, wall, wall. Now inject a bad measurement. The next reading should be 0, but the sensor returns 1:

measurements = [1, 0, 1, 0, 0, 1, 1]
priors, posteriors = discrete_bayes_sim(
    prior, kernel, measurements, z_prob, hallway
)

That single bad reading did real damage to your knowledge. Now continue with a run of correct measurements.

prior = np.array([0.1] * 10)
measurements = [1, 0, 1, 0, 0, 1, 1, 1, 0, 0]
step_posteriors = []
for i, m in enumerate(measurements):
    likelihood = lh_hallway(hallway, z=m, z_prob=0.75)
    posterior = update(likelihood, prior)
    step_posteriors.append(posterior)
    prior = predict(posterior, 1, kernel)

The filter quickly recovered from the bad reading and converged on the most likely positions of the dog.

Drawbacks and limitations

The examples are simple, but the filter is complete and robust. If you need a multimodal, discrete filter, this one works in real systems. It is still not used often, because it has several limitations. The first is scaling. The dog problem tracks one variable, the position. Most interesting problems track several variables in a large space. You would at least want the dog’s

(x, y)

coordinates, and probably his velocity

(\dot{x}, \dot{y})

as well. In more dimensions the array becomes a grid that stores a probability at each discrete location. Every update() and predict() touches every cell, so a four-variable problem costs

O(n^4)

per time step. Realistic filters can track 10 or more variables. The second is that the filter is discrete, while the world is continuous. A 100 m hallway needs 10,000 positions for 1 cm resolution, so every predict and update works on 10,000 probabilities. It gets exponentially worse with more dimensions. A 100 m by 100 m courtyard needs 100,000,000 cells for 1 cm resolution. The third is that the filter is multimodal. In the last example you ended up with strong beliefs that the dog was at one of two positions. That is not always a problem. Particle filters are multimodal too, and are often chosen for that reason. But imagine a car navigation system telling you it is 40% sure you are on D Street and 30% sure you are on Willow Avenue. The fourth is that it needs a measurement of the change in state. You need a motion sensor to know how far the dog moved. There are ways around this, but they are beyond this section. Even so, for a small problem that fits, this filter is a good choice. It is easy to implement, debug, and understand.

Tracking and control

So far you have passively tracked an object that moves on its own. Now consider a closely related problem. You are automating a warehouse and want robots to collect the items in each order. A simple design puts the robots on a track. You send a robot a destination and it drives there. But tracks and motors are imperfect. Wheels slip, and a robot rarely stops exactly where you commanded. There are several robots, and you need to know where each one is so they do not collide. So you add sensors. Perhaps you mount magnets along the track every few feet and use a Hall sensor to count the magnets the robot passes. If it counts 10 magnets, it should be at the 10th magnet. It might miss a magnet or count one twice, so you allow for some error. Counting magnets is very similar to sensing doors, so the hallway filter applies directly. There is one more source of information. You know the commands you send to the robot. Say that once a second you command it to move left 1 unit, move right 1 unit, or stay put. If you command “move left 1 unit”, you expect it to be 1 unit to the left a second later. (This ignores acceleration.) Wheels and motors are imperfect, so it might move 0.9 units, or 1.2. The dog filter assumed the dog keeps moving the way he was moving. That is a weak assumption for a dog. Robots are far more predictable. Instead of guessing the motion, you pass the commanded movement to predict(), together with a kernel that describes how likely each actual movement is.

Simulating the train

You need a simulated, imperfect train. When commanded to move, it sometimes makes a small mistake, and its sensor sometimes returns a wrong value.

import random


class Train(object):
    def __init__(
        self, track_len, kernel=[1.0], sensor_accuracy=0.9
    ):
        self.track_len = track_len
        self.pos = 0
        self.kernel = kernel
        self.sensor_accuracy = sensor_accuracy

    def move(self, distance=1):
        """move in the specified direction
        with some small chance of error"""

        self.pos += distance
        # insert random movement error according to kernel
        r = random.random()
        s = 0
        offset = -(len(self.kernel) - 1) / 2
        for k in self.kernel:
            s += k
            if r <= s:
                break
            offset += 1
        self.pos = int((self.pos + offset) % self.track_len)
        return self.pos

    def sense(self):
        pos = self.pos
        # insert random sensor error
        if random.random() > self.sensor_accuracy:
            if random.random() > 0.5:
                pos += 1
            else:
                pos -= 1
        return pos

Now write the filter as a function, so you can run it under different assumptions. The robot always starts at the beginning of the track. The track has 10 units. Think of it as a much longer track, say 10,000 units, with the magnet pattern repeating every 10 units. A length of 10 is easier to plot and inspect. The sensor also needs its own likelihood. A reading

m

comes from the true position

m

with probability equal to the sensor accuracy, and otherwise from one of the two neighbors

m \pm 1

, each with half the remaining probability. Every other position cannot produce that reading, so its likelihood is zero. The door-and-wall likelihood of the hallway would instead give every nonmatching position the same small weight.

def lh_train(track_len, m, sensor_accuracy):
    """likelihood of each true position given the reading m:
    the sensor reports the true position with probability
    sensor_accuracy, otherwise one of its two neighbors"""
    likelihood = np.zeros(track_len)
    likelihood[m % track_len] = sensor_accuracy
    likelihood[(m - 1) % track_len] += (1 - sensor_accuracy) / 2
    likelihood[(m + 1) % track_len] += (1 - sensor_accuracy) / 2
    return likelihood


def train_filter(
    iterations,
    kernel,
    sensor_accuracy,
    move_distance,
    do_print=True,
):
    track = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9])
    prior = np.array([0.9] + [0.01] * 9)
    posterior = prior[:]
    normalize(prior)

    robot = Train(len(track), kernel, sensor_accuracy)
    for i in range(iterations):
        # move the robot and
        robot.move(distance=move_distance)

        # perform prediction
        prior = predict(posterior, move_distance, kernel)

        #  and update the filter
        m = robot.sense()
        likelihood = lh_train(len(track), m, sensor_accuracy)
        posterior = update(likelihood, prior)
        index = np.argmax(posterior)

        if do_print:
            conf = posterior[index] * 100
            print(f"time {i}: pos {robot.pos}, sensed {m}")
            print(
                f"  estimate {index}, confidence {conf:.2f}%"
            )

    if do_print:
        print()
        print("final position is", robot.pos)
        index = np.argmax(posterior)
        conf = posterior[index] * 100
        print(f"estimate {index}, confidence {conf:.2f}%")
    return posterior

First run it with no sensor or movement error. If the code is correct, it should locate the robot exactly. The printout traces the predict and update cycle step by step.

random.seed(3)
np.set_printoptions(
    precision=2, suppress=True, linewidth=60
)
posterior = train_filter(
    4,
    kernel=[1.0],
    sensor_accuracy=0.999,
    move_distance=4,
    do_print=True,
)

time 0: pos 4, sensed 4
  estimate 4, confidence 100.00%
time 1: pos 8, sensed 8
  estimate 8, confidence 100.00%
time 2: pos 2, sensed 2
  estimate 2, confidence 100.00%
time 3: pos 6, sensed 6
  estimate 6, confidence 100.00%

final position is 6
estimate 6, confidence 100.00%

The filter tracked the robot perfectly. Now add errors.

random.seed(5)
posterior = train_filter(
    4,
    kernel=[0.1, 0.8, 0.1],
    sensor_accuracy=0.9,
    move_distance=4,
    do_print=True,
)

time 0: pos 4, sensed 4
  estimate 4, confidence 98.50%
time 1: pos 8, sensed 9
  estimate 9, confidence 70.42%
time 2: pos 3, sensed 3
  estimate 3, confidence 96.58%
time 3: pos 7, sensed 8
  estimate 8, confidence 70.21%

final position is 7
estimate 8, confidence 70.21%

The sensor misread at times 1 and 3. Each time the estimate moves one position off and the confidence drops to about 70%. The correct reading at time 2 brings the estimate back, with about 97% confidence. Next, run a long simulation and see how the filter responds to errors.

long_runs = {}
for i in range(4):
    random.seed(3)
    long_runs[148 + i] = train_filter(
        148 + i,
        kernel=[0.1, 0.8, 0.1],
        sensor_accuracy=0.8,
        move_distance=4,
        do_print=False,
    )

At iteration 148 the confidence is lower (about 87%), because recent errors left probability on the neighboring positions. A few iterations later the filter has corrected itself and is about 96% confident again.

Bayes’ theorem and the total probability theorem

You built the math in this section by reasoning about the information available at each moment. Along the way you found Bayes’ theorem and the total probability theorem. Bayes’ theorem tells you how to compute the probability of an event given earlier information. The update() function computes

\text{posterior} = \frac{\text{likelihood}\times \text{prior}}{\text{normalization factor}}

That is Bayes’ theorem. Every Bayesian filter is an expression of it. A more formal treatment adds notation, but the idea stays this simple:

\text{updated knowledge} = \big\|\text{likelihood of new knowledge}\times \text{prior knowledge} \big\|

where

\| \cdot\|

means normalize. You reached this by reasoning about a dog in a hallway, yet the same equation applies to a wide range of filtering problems, including the one-dimensional Kalman filter. The predict() step computes the total probability of several possible events. In statistics this is the total probability theorem [6]. For now, the key point is that Bayes’ theorem is a rule for combining new information with existing information.

Summary

The code is short, yet it is a working Bayesian filter. You started with no information and extracted position from noisy sensors. The sensors in this section are quite noisy (most real sensors are more than 80% accurate), and the filter still converged quickly on the most likely position. The predict step always degrades your knowledge. Adding a measurement, even a noisy one, improves it, and the cycle converges. The one-dimensional Kalman filter uses different math but the same logic. It also forms estimates from measurements and a process model using Bayesian reasoning. If this section is clear to you, the Kalman filter will be too. The core idea carries over directly: multiply probabilities when you measure, and shift probabilities when you predict, and the estimate converges.

References

D. Fox, W. Burgard, and S. Thrun. “Monte Carlo localization: Efficient position estimation for mobile robots.” Journal of Artificial Intelligence Research, 1999. http://www.cs.cmu.edu/afs/cs/project/jair/pub/volume11/fox99a-html/jair-localize.html
Dieter Fox et al. “Bayesian Filters for Location Estimation.” IEEE Pervasive Computing, September 2003. http://swarmlab.unimaas.nl/wp-content/uploads/2012/07/fox2003bayesian.pdf
Sebastian Thrun. “Artificial Intelligence for Robotics.” https://www.udacity.com/course/cs373
Khan Academy. “Introduction to the Convolution.” https://www.khanacademy.org/math/differential-equations/laplace-transform/convolution-integral/v/introduction-to-the-convolution
Wikipedia. “Convolution.” http://en.wikipedia.org/wiki/Convolution
Wikipedia. “Law of total probability.” http://en.wikipedia.org/wiki/Law_of_total_probability
Wikipedia. “Time evolution.” https://en.wikipedia.org/wiki/Time_evolution

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

Discrete Bayes Filter

Tracking a dog

Extracting information from sensor readings

Noisy sensors

Incorporating movement

Terminology

Adding uncertainty to the prediction

Generalizing with convolution

Integrating measurements and movement updates

The discrete Bayes algorithm

The effect of bad sensor data

Drawbacks and limitations

Tracking and control

Simulating the train

Bayes’ theorem and the total probability theorem

Summary

References

​Tracking a dog

​Extracting information from sensor readings

​Noisy sensors

​Incorporating movement

​Terminology

​Adding uncertainty to the prediction

​Generalizing with convolution

​Integrating measurements and movement updates

​The discrete Bayes algorithm

​The effect of bad sensor data

​Drawbacks and limitations

​Tracking and control

​Simulating the train

​Bayes’ theorem and the total probability theorem

​Summary

​References

Tracking a dog

Extracting information from sensor readings

Noisy sensors

Incorporating movement

Terminology

Adding uncertainty to the prediction

Generalizing with convolution

Integrating measurements and movement updates

The discrete Bayes algorithm

The effect of bad sensor data

Drawbacks and limitations

Tracking and control

Simulating the train

Bayes’ theorem and the total probability theorem

Summary

References