EKF vs factor graphs; loosely vs tightly coupled

1. Engineering problem

Fusing sensors for localisation can be done by a recursive filter (EKF), which updates only the current state, or by optimisation over a window or a whole trajectory (factor graph, smoother), which can revise earlier states. It can also be done on the outputs of other estimators (loosely coupled) or on raw measurements (tightly coupled).

2. When is this needed?

• Choosing a localisation architecture.
• When loop closures or late GPS fixes should correct the past trajectory.
• When picking between packages (robot_localization, GTSAM-based, LIO packages).

3. Selection and design decisions

robot_localization is an EKF-based package; LIO-SAM’s README describes a factor graph in imuPreintegration.cpp that optimises IMU and lidar odometry factors and estimates IMU bias, reset periodically, plus a second graph for mapping; FAST-LIO is a tightly coupled iterated EKF fusing LiDAR feature points with IMU data (READMEs read 2026-11-06). Loosely coupled fusion treats another estimator’s pose or twist as a measurement (as in robot_localization’s odometry inputs); tightly coupled fusion uses the raw sensor measurements inside one estimator.

4. Advantages and disadvantages

• Filter vs smoother: compute and simplicity against revisable accuracy.
• Loose vs tight: modularity against accuracy.
• Many systems combine them: a tightly coupled front end and a pose-graph back end.

5. Requirements and constraints

• The compute budget decides between a filter and a window or full smoother.
• Whether loop closures or delayed measurements must correct the past trajectory.
• Measurement correlations are handled (do not feed an estimator its own output).

6. Architecture

EKF: x(t-1) ─▶ predict ─▶ update(z) ─▶ x(t)         (past is frozen)
Smoother: all x(0..T) ◀─ odometry factors + GPS/loop factors (solved together)

7. Implementation

1. List the measurements and when they arrive (late or delayed ones).
2. Decide whether past states must be revised.
3. If yes, use a smoother or a pose graph; if no, an EKF may suffice.
4. Decide loose or tight from the available estimators and the required accuracy.
5. Validate on logged data with a reference (see the evaluation guide).

import numpy as np
n = 5
odo = np.full(n - 1, 1.1)                    # biased odometry: true step is 1.0
gps_i, gps = 4, 4.0                          # absolute position at the last step
s_odo, s_gps = 0.1, 0.05
# filter view: at step 2 the estimate was the dead-reckoned value; later GPS cannot change it
x = [0.0]
for u in odo:
    x.append(x[-1] + u)
w_o, w_g = 1 / s_odo ** 2, 1 / s_gps ** 2
# smoother: solve all states together (x0 fixed at 0)
A, b, w = [], [], []
for i in range(n - 1):
    row = np.zeros(n); row[i + 1], row[i] = 1, -1; A.append(row); b.append(odo[i]); w.append(w_o)
row = np.zeros(n); row[gps_i] = 1; A.append(row); b.append(gps); w.append(w_g)
row = np.zeros(n); row[0] = 1; A.append(row); b.append(0.0); w.append(1e6)
A, b, W = np.array(A), np.array(b), np.diag(w)
sm = np.linalg.solve(A.T @ W @ A, A.T @ W @ b)
true = np.arange(n, dtype=float)
print("true         ", true)
print("dead-reckoned", np.round(x, 2))
print("smoother     ", np.round(sm, 2))
print("error at step 2: dead-reckoned %.2f m, smoother %.2f m" % (abs(x[2] - 2), abs(sm[2] - 2)))

Printed (checked by the site’s tests):

true          [0. 1. 2. 3. 4.]
dead-reckoned [0.  1.1 2.2 3.3 4.4]
smoother      [-0.    1.01  2.01  3.02  4.02]
error at step 2: dead-reckoned 0.20 m, smoother 0.01 m

Reading it: biased odometry (1.1 m per step) dead-reckons to 4.4 m at step 4, and the state at step 2 is frozen at 2.2 m (0.20 m error); a late absolute fix of 4.0 m at step 4 lets the smoother distribute the correction over the whole trajectory, bringing step 2 to 2.01 m (0.01 m error). The scenario is a one-dimensional illustration.

8. Real examples

• Outdoor robot with intermittent RTK GNSS: a smoother revises the trajectory when the fix returns; an EKF only corrects from then on.

9. Failure scenarios

• Expecting an EKF to fix the earlier map after a loop closure.
• Feeding a SLAM pose and the same sensor into the filter (double counting).
• A full smoother with no windowing, which falls behind real time.

10. Troubleshooting

1. If loop closures do not improve the past, the architecture is a filter.
2. If compute runs out, window or marginalise the smoother.
3. If fused results are over-confident, check for double counting.

11. Validation

• The example prints the lines shown (executed by the site tests).
• The architecture meets the compute budget and the accuracy requirement on logged data.

12. Engineering checklist

• Late-data needs identified
• Compute budget known
• Loose or tight chosen
• No double counting
• Validated on logs

13. Further references

• robot_localization documentation (ros2 branch): state_estimation_nodes.rst and params/ekf.yaml, read 2026-11-06.
• LIO-SAM, FAST-LIO READMEs: LIO-SAM, FAST-LIO, read 2026-11-06.

Related on FixTheRobot