Discrete Robot Localization

This example builds a grid-cell robot localization problem. The robot moves through three corridor cells, and the state variables are the possible cells at consecutive time steps. Motion and beacon observations form a chain, so the factor graph is tree-structured and forward-backward discrete belief propagation is exact.

Variable Nodes

Each position variable has the same three possible cell states:

using FactorGraph

cells = [:A, :B, :C]

variables = [
    DiscreteVariable(:x1, length(cells); label = "x1", states = cells),
    DiscreteVariable(:x2, length(cells); label = "x2", states = cells),
    DiscreteVariable(:x3, length(cells); label = "x3", states = cells),
    DiscreteVariable(:x4, length(cells); label = "x4", states = cells)
]

Prior Factor Node

The robot is believed to start near cell A:

prior = DiscreteFactor(:x1, [0.85, 0.10, 0.05]; label = "prior_x1", initialize = true)

Motion Factor Nodes

The motion model favors moving one cell to the right, while still allowing the robot to stay in place or slip:

motion = [
    0.20 0.75 0.05
    0.05 0.20 0.75
    0.02 0.18 0.80
]

motion12 = DiscreteFactor(:x1, :x2, motion; label = "motion12")
motion23 = DiscreteFactor(:x2, :x3, motion; label = "motion23")
motion34 = DiscreteFactor(:x3, :x4, motion; label = "motion34")

Rows follow the previous cell and columns follow the next cell.

Beacon Factor Nodes

Beacon observations are unary likelihood factors. A beacon near cell C is detected at time step 3, and another observation again favors cell C at time step 4:

beacon3 = DiscreteFactor(:x3, [0.05, 0.15, 0.80]; label = "beacon3")
beacon4 = DiscreteFactor(:x4, [0.03, 0.17, 0.80]; label = "beacon4")

Factor Graph Construction

Collect the factors and build a tree graph with a selected root:

factors = [prior, motion12, motion23, motion34, beacon3, beacon4]

graph = factorGraph(variables, factors; root = :x1)

The graph can be rendered as an SVG factor graph figure:

saveGraphFigure("../drl.svg", graph; layout = (columnSpacing = 120, rowSpacing = 90))

Running Belief Propagation

Run sum-product belief propagation on the tree:

inference = sumproduct(graph)

forwardBackward!(graph, inference)

Results

Inspect the posterior cell probabilities:

printMarginal(graph, inference)

Variable marginals (sum-product form):

Marginal for variable node "x1":
  probability = [0.8261052256238364, 0.11494644808955293, 0.05894832628661077]

Marginal for variable node "x2":
  probability = [0.04768022810366688, 0.7617809936202635, 0.19053877827606952]

Marginal for variable node "x3":
  probability = [0.0016831764592516471, 0.07577558232694633, 0.922541241213802]

Marginal for variable node "x4":
  probability = [0.0010617437359046814, 0.04734964369219023, 0.9515886125719051]

Adding a New Time Step

In an online localization problem, the robot later moves again and a new position state is needed. Add the new variable, connect it with the latest motion model, add a new beacon observation, and continue from the current inference state:

addVariable!(graph, inference, :x5, length(cells); label = "x5", states = cells)
addFactor!(graph, inference, :x4, :x5, motion; label = "motion45")
addFactor!(graph, inference, :x5, [0.02, 0.18, 0.80]; label = "beacon5")

forwardStep!(graph, inference; variable = :x5, factor = "beacon5")
forwardStep!(graph, inference; variable = :x5, factor = "motion45")
backward!(graph, inference)

marginals!(graph, inference)

The previous messages and marginals are kept as a warm start. Use the inference-aware addVariable! and addFactor! forms for this warm start; graph-only topology changes require a fresh inference object.

The updated graph is still a tree. A full forwardBackward! sweep can be used instead when all messages in the updated tree should be recomputed.

printMarginal(graph, inference; variable = :x5)

Marginal for variable node "x5" (sum-product form):
  probability = [0.0006445639312052108, 0.0487329844192641, 0.9506224516495306]