Robot Localization on a Chain

This example builds a one-dimensional robot localization problem. The robot moves along a line, and the state variables are robot positions at consecutive time steps. Odometry measurements connect neighboring positions, so the factor graph is a chain. This tree structure is a natural fit for forward-backward Gaussian belief propagation.

Variable Nodes

The state variables are robot positions at four time steps. Each position is modeled as a scalar variable:

using FactorGraph

variables = [
    GaussianVariable(:x1, 1),
    GaussianVariable(:x2, 1),
    GaussianVariable(:x3, 1),
    GaussianVariable(:x4, 1)
]

Prior Factor Node

The initial position is measured or known approximately. This is represented as a unary prior factor:

prior = GaussianFactor(:x1, 0.0, 1.0, 0.01)

Odometry Factor Nodes

Odometry measurements connect consecutive robot positions. For example, a measured displacement between time steps 1 and 2 is represented as a factor connected to the first two position variables:

odom12 = GaussianFactor(:x1, :x2, 1.05, [-1.0 1.0], 0.04)
odom23 = GaussianFactor(:x2, :x3, 0.95, [-1.0 1.0], 0.04)
odom34 = GaussianFactor(:x3, :x4, 1.10, [-1.0 1.0], 0.04)

These odometry factors form a chain, so the graph is tree-structured.

Landmark Factor Node

Suppose the robot also receives a position measurement at the final time step, for example from a landmark or GPS-like sensor:

landmark = GaussianFactor(:x4, 3.05, 1.0, 0.02)

This factor does not create a cycle because it is unary.

Factor Graph Construction

Collect the factors and build the graph with a selected root for forward-backward Gaussian belief propagation:

factors = [prior, odom12, odom23, odom34, landmark]

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

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

saveGraphFigure("../rl.svg", graph; layout = (columnSpacing = 120, rowSpacing = 100))

Running Belief Propagation

Run canonical-form Gaussian belief propagation on the tree:

inference = canonical(graph)

forwardBackward!(graph, inference)

Results

Print the resulting Gaussian marginals:

printMarginal(graph, inference)

Variable marginals (canonical form):

Marginal for variable node "x1":
  mean = [-0.0033333333333333045]
  covariance = [0.009333333333333332]
  information = [-0.3571428571428541]
  precision = [107.14285714285714]

Marginal for variable node "x2":
  mean = [1.0333333333333332]
  covariance = [0.033333333333333326]
  information = [31.0]
  precision = [30.0]

Marginal for variable node "x3":
  mean = [1.9700000000000002]
  covariance = [0.036000000000000004]
  information = [54.72222222222222]
  precision = [27.77777777777778]

Marginal for variable node "x4":
  mean = [3.056666666666666]
  covariance = [0.017333333333333333]
  information = [176.34615384615384]
  precision = [57.6923076923077]

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 node, connect it with the latest odometry measurement, and continue from the current inference state:

addVariable!(graph, inference, :x5, 1)
addFactor!(graph, inference, :x4, :x5, 0.90, [-1.0 1.0], 0.04; label = "odom45")

forwardStep!(graph, inference; variable = :x5, factor = "odom45")
backward!(graph, inference)
marginals!(graph, inference)

The previous messages and marginals are kept as a warm start. The new factor extends the chain, so the updated graph is still a tree. Use the inference-aware addVariable! and addFactor! forms for this warm start; graph-only topology changes require a fresh inference object.

The old messages remain useful because the old part of the chain did not change. Here, only the new edge is advanced manually before the backward pass updates the messages toward the new terminal state. A full forwardBackward! sweep can be used instead when all messages in the updated tree should be recomputed.

printMarginal(graph, inference)

Variable marginals (canonical form):

Marginal for variable node "x1":
  mean = [-0.0033333333333333045]
  covariance = [0.009333333333333332]
  information = [-0.3571428571428541]
  precision = [107.14285714285714]

Marginal for variable node "x2":
  mean = [1.0333333333333332]
  covariance = [0.033333333333333326]
  information = [31.0]
  precision = [30.0]

Marginal for variable node "x3":
  mean = [1.9700000000000002]
  covariance = [0.036000000000000004]
  information = [54.72222222222222]
  precision = [27.77777777777778]

Marginal for variable node "x4":
  mean = [3.056666613684445]
  covariance = [0.017333333032888892]
  information = [176.34615384615384]
  precision = [57.6923086923077]

Marginal for variable node "x5":
  mean = [3.9566666666666657]
  covariance = [0.05733333333333332]
  information = [69.01162790697674]
  precision = [17.44186046511628]

Validation

Compare the current Gaussian belief propagation result with the centralized weighted least-squares solution:

reference = solveWLS(graph)
maxMeanError(graph, inference, reference)

5.298222216509885e-8