Continuing my journey through the implementation of factor graph-based state estimation, we first explored how a classic Extended Kalman Filter (EKF) update is not a black-box matrix equation, but fundamentally a two-factor graph solve via Non-linear Least Squares (NLS).
When we look under the hood of a tracking or localization filter, we are ultimately trying to minimize a stacked residual vector. Every measurement adds a local error term, and optimizing the state means solving:
The Underlying Machinery
To actually solve this optimization problem, we dug into the numerical machinery required to handle real-world robot trajectories. We implemented the Gauss-Newton method to iteratively solve the normal equations:
Because real sensor data is rarely clean, we paired this with Levenberg-Marquardt damping to handle poor initializations, alongside robust M-estimators (like Huber Kernels) to dynamically downweight outlier measurements. This ensures the solver does not diverge when a sensor provides bad data.
The Information Matrix as a Topology
From there, we realized a beautiful mathematical equivalence: the information matrix Λ provides the perfect data structure to store our graph topology.
Because each local factor only references a few variables, the resulting Jacobian J is incredibly tall and mostly filled with zeros. When we form the information matrix:
it naturally inherits this sparse, block-tridiagonal structure. Variables that do not share a measurement factor remain uncorrelated in the matrix, meaning the graph topology and the linear algebra structure are exactly the same thing.
Bringing It Together: A Batch Optimization Engine
With all these pieces snapping together, we can now assemble a complete batch optimization engine from scratch. Instead of hand-writing giant, error-prone Jacobians, we can simply declare our variables and local factors, letting the graph automatically handle the bookkeeping and matrix assembly.
This framework allows us to explore state estimation that is structurally more resilient to severe non-linearities than classic, localized Kalman-based filters.
See the Code in Action
The video below illustrates this exact engine optimizing a toy 2D robot trajectory, fusing noisy wheel odometry with sparse, absolute GPS fixes. You can see the block-tridiagonal structure form in real time as the path converges.
For more details, including theory and code, check out the notebook below:
https://github.com/carlos-argueta/factor-graphs-from-scratch/blob/main/modules/03-factor-graphs-from-scratch/lesson.ipynb
Leave a Reply