Review of Factor Graphs for Robust GNSS

Applications
arXiv:2112.07794v2 [cs.RO] 15 Jun 2022

Shounak Das1 , Ryan Watson2 , and Jason Gross1

1
Department of Mechanical and Aerospace Engineering , West
Virginia University , Morgantown, WV 26505, USA
2
The Johns Hopkins University Applied Physics Laboratory,
Laurel, USA

1 What is a factor graph?

Probabilistic modelling is a very important tool for any kind of estimation prob-
lem. This led to the development of Graphical Models [1] which model known
and unknown variables of a system using vertices in a graph and define edges
between them to represent their relationship. Due to the inherent uncertainty in
modelling a system, these relations are probabilistic. A factor graph connects
many types of these graphical models like Markov Random Fields, Bayesian
Networks, Tanner Graphs [2]. The primary motivation of a factor graph is to
represent a global function of many variables as a product of local functions with
smaller subsets of variables. The factor graph is not a method but a framework
for modelling any system using its locality structure, that is each variable is
only dependent on a few other local variables and is independent of the others.
As explained in [3], it is this locality property that makes it useful in modelling
a variety of problems including mapping, visual-inertial odometry, motion plan-
ning, trajectory estimation and deep learning. The factor graph is defined as
a Bipartite graph which has two types of vertices, one is the variable (i.e., the
state vector) vertex which is to be estimated and another one is the factor vertex
which encodes the constraints (e.g., a set of GNSS observations) applied to the
variable vertices. An edge can only exist between a factor vertex and a variable
vertex. The factor vertices represent the local functions which depend on the
variable vertices with which it shares edges. A common estimation problem in
robotics uses the factor graph framework to estimate the unknown robot pose
along with other parameters depending on the problem. This is achieved by solv-
ing the Maximum-A-Posteriori (MAP) problem which maximizes the product
of factors that are probabilistic constraints between states and measurements.
[4] gives a rigorous mathematical description of how this maximization problem
is solved in the field of robotic perception.

1
 Factor graphs have been treated as an alternative framework for solving
estimation problems and have proven very effective for specific applications in-
cluding Simultaneous Localization and Mapping (SLAM). This framework has
attracted widespread use due to the flexibility of the approach and the ease of
implementation granted through the availability of open-source graph optimiza-
tion libraries like GTSAM [5], g2o [6], Ceres [7]. While the factor graph frame-
work has been shown to be beneficial for many applications, it should be noted
that the frame work can be used in an equivalent manner to (i.e., it is a gener-
alization of) existing state estimation implementations (e.g., the Kalman filter
and it many variants). To begin this comparison, we will note that the factor
graph ultimately encodes an objective function, which is solved through repeated
relinearization via a non-linear optimization routine (e.g., Gauss-Newton, Lev-
enberg–Marquardt). Previous work has demonstrated that iterating on the
measurement update of Extended Kalman Filter and relinearizing the system
models between iterations is equivalent to a Gauss-Newton optimization [8].
Which shows that, under a certain set of constraints, the factor graph operating
in batch mode is equivalent to a backward-smoothing Extended Kalman Filter
(EKF) [9] that re-linearizes system and observation models and iterates.

2 What are the advantages of using a factor

graph?
Factor graph optimization has some advantages over the standard non-iterated
Kalman filter which may be valuable for certain applications. First, like any
optimization problem, it uses multiple iterations to minimize a cost instead of
just one iteration for each state as in a standard Kalman filter. Second, it also
linearizes the nonlinear measurement model every iteration step for every state
unlike the single linearization performed by the standard Kalman filter. Factor
graphs have also been shown to better exploit the time correlation between
past and current epochs, which has been attributed to the batch nature of the
estimation method. In particular, when operating in a batch-mode, a factor
graph would be equivalent to a forward filter and backward smoother after
each measurement update. For GNSS/INS applications, these benefits have
been supported by experimental results in [10] where factor graphs have been
shown to perform better than an EKF in urban environments. It might appear
that with accumulation of new measurements over a longer time, the batch
estimation might lose real-time performance. A sliding window approach can
also be used similar to [11] to relieve computational cost.The window size has
been found to be crucial for good optimization results and can depend upon
environmental conditions [10]. Factor graphs achieve efficient computation by
utilizing the sparse nature of the Jacobian and information matrices. This helps
in fast matrix factorization and back-substitutions. Directly removing earlier
poses from the graph can lead to information loss. This can be avoided using
marginalization in the square-root information form which removes variables

2
from the Bayes net derived from the factor graph using elimination algorithm.
Due to the sparseness in the graph, incremental QR factorization can be also
achieved efficiently [5]. Beyond fixed-lag smoothing, the isam2 formulation [12]
achieves real-time performance by converting the factor graph to the Bayes tree
[13] when a new constraint is added. This is a more accurate incremental and
smoothing method for highly nonlinear measurement models. The vertices of
Bayes tree represent cliques in the Bayes net that is obtained from the factor
graph during factorization. Only states contained within the same cliques as
the states in the new constraint and their predecessors in the Bayes tree need
to be updated. Watson and Gross [14] used isam2 in a GNSS factor graph to
show improved positioning performance than a traditional EKF-Precise Point
Positioning (PPP) method. Wen et al. [15] applied factor graph optimization to
the problem of both GNSS and GNSS-Real-time Kinematic (RTK) positioning
and shows better performance than an EKF.

3 What does a factor graph applied to GNSS

look like?
A detailed description of creating factors with GNSS observations is presented
in [14]. The states commonly estimated in the GNSS factor graph are the re-
ceiver position, tropospheric delay, carrier phase bias, and the receiver clock
bias. A visual representation of the GNSS factor graph is provided in Fig. 1,
where ψ represents any probabilistic constraint that might exist between the
states and the measurements. In this specific implementation, ψ p encodes the
prior belief on each state, which depends on the specific data set and environ-
mental characteristics. ψ b is a motion constraint between two consecutive states
along the trajectory which could, for example, incorporate motion data from an
Inertial Measurement Unit (IMU) or wheel odometry. A common example of ψ b
used in GNSS/IMU navigation is the factor which uses IMU-preintegration [16]
to calculate displacement between the two factor graph locations with multiple
IMU measurements integrated between them. Finally, ψ m are measurement
constraint between a state and the measurements that were perceived from that
state, for example GNSS pseudorange or carrier-phase measurements. To find
the MAP estimate for the GNSS factor graph, we could find the set of states
that maximize the product of factors. However, in practice, this optimization
problem can be greatly simplified by employing the Gaussian noise assumption,
which enables the conversion of the problem from maximizing the product of
the factors to a non-linear least squares problem where each component of the
sum is a Mahalanobis cost, which represents sum of squares of the normalized
residuals, as provided in Eq. 1, where f (∗) a mapping between states at dif-
ference epochs and h(∗) is a mapping from the state space to the observation
space.

3
 Figure 1: GNSS factor graph example.

 
I J K
X 2
X 2 X 2
X̂ = argmin  kψip kΣ + ψjb Λ
+ kψkm kΞ 
x
i=1 j=1 k=1
 
XI J
X K
X
2 2 2
= argmin  kxo − xi kΣ + kxj − fj (xj−1 )kΛ + kyk − hk (xk )kΞ 
x
i=1 j=1 k=1
(1)

4 What are methods for robust estimation using

factor graphs in GNSS?
As mentioned, the factor graph framework also makes it easy to add existing and
new robust estimation methods which can help reduce localization error during
spoofing attacks or large noise from multipath or atmospheric effects. The
discussion below enumerates some of these robust methods applied to GNSS.
Sunderhauf et al. [17] defined Switch Constraints (SC), which is a lifted
optimization [18] methodology, that defines an observation weighting function
Ψ() that is a function of switch variables s, which is estimated in conjunction
with the state parameters of interest. The SC method was initially developed
for robust loop closure detection in SLAM and then extended to GNSS for
multipath mitigation [19]. When utilizing switch constraints, the pseudorange
factor cost is expressed as a scaled version of the Mahalanobis cost between the
predicted and actual measurement
2 2
eswitch
k Σk
= kΨ (sk ) · (yk − hk (xk ))kΣk , (2)

where the function Ψ is a linear function of the switch variable. Prior factors are
added for each of these switch variables to stop the optimization from making all
sk to zero. A transition factor can also be added to model the change between
sk−1 and sk if the same satellite is observed at the next time step. These

4
switch functions help in automatically de-weighting erroneous measurements
(e.g., suspected multipath measurements) and are seen to perform better than
computationally expensive ray tracing methods [19].
An extension of SC was derived in [20] called Dynamic Covariance Scaling
(DCS) where the switch variables are taken out of the optimization method
and calculated separately using the residual, current measurement uncertainty
and a prior switch uncertainty. After calculating sk , the information matrix
associated with the GNSS observation factor is scaled by Ψ(sk )2 .
Max-mixtures (MM) [21] was also developed to tackle false loop closures
using a Gaussian Mixture Model (GMM) but instead of the sum operator which
is unsuitable for maximum likelihood when multi-modal uncertainty model is
utilized, the objective function is converted to use the max operator, as shown
in Eq. 3.

p (yi | x) = max wk N µk , Λ−1

k (3)
k

The benefits of SC, DSC, and MM have been evaluated in [22, 23] for GNSS
factor graph applications with real world data. Both of these studies showed
the substantial positioning improvement that can be granted via the utilization
of robust estimation techniques when conducting optimization with degraded
GNSS observations.
To extend upon the max-mixtures work, Watson et al. [24] proposed to learn
the GMM during run-time based upon clustering of the observation residuals.
Initially, this work was implemented in a batch framework; however, it was
later extended to work incrementally [25], through an efficient methodology for
incrementally merging GMMs.
M-estimators [26] have also been recently tested within the GNSS framework
in batch form [27] and found to perform better than non-robust estimators. M-
estimators assume a loss function that is different from the squared loss function.
The squared loss function is highly sensitive to outliers since it grows aggressively
for larger values of residuals. Thus, a group of loss functions were introduced
which grow less aggressively than the squared loss function. The Huber cost
function [28], as provided in Eq. 4 is one such function.
(
z 2 /2 |z| ≤ ∆
ρ(z) = 2
(4)
∆|z| − ∆ /2 |z| > ∆
When the objective function is modified to utilize a m-estimator, the optimiza-
tion problem takes a form as depicted in Eq. 5.
X  ri (x) 
X̂ = arg min ρ , (5)
x
i
σ

where ri (x) is the residual for each measurement and σ is the scale parameter.
Increasing the ∆ parameter makes this function closer to the squared loss
function. Equation 5 can be solved iteratively with weighted least squares

5
method [27, 26]. Selecting a suitable ∆ parameter is not straightforward, since
it depends on the measurement noise statistics. Agamennoni et al. [29] uses the
fact that some M-estimators like Huber, Cauchy, Laplace have a correspond-
ing elliptical distribution to estimate the ∆ and the states in an Expectation
Maximization (EM) framework. Barron [30] jointly optimizes for the states and
the parameters for computer vision applications. A factor graph gives greater
flexibility in the M-estimator application since it can help in de-weighting not
only the current measurements but also changing the weights of the past mea-
surements. It also can help is totally removing some past measurements if it is
found to be an outlier later whereas in the Kalman filter, the contribution of
past measurements cannot be changed in a real-time manner. Most graph op-
timization libraries also have built in functionality to use robust cost functions
which is also helpful.
Finally, the robust estimation technique derived by Yang et al. [31] unites
the two well-known ideas from computer vision, the Black-Rangarajan Duality
[32] and Graduated Non-Convexity [33] to iteratively solve the point cloud regis-
tration problem using robust cost functions. According to the Black-Rangarajan
Duality, equation 5 can be re-written as
X
wi ri2 (x) + Φρ (wi ) ,

X̂, w = arg min (6)
x,wi ∈[0,1] i
th
where wi is the weight for the i measurement and Φρ is a penalty term which
depends on the weight and the robust cost ρ. Graduated Non-Convexity(GNC)
is a method to minimize a non-convex function f without facing the problem of
local minima. The idea is to replace the function f with a surrogate function
fµ whose convexity is controlled by the parameter µ. The optimization starts
off with a convex form of fµ and µ is changed iteratively such that the non-
convexity increases. By uniting these two methods, [31] solved two problems in
equation 5, 1) avoiding local minima while optimizing a robust cost function 2)
convert equation 5 to a weighted least squares problem which is always easier
to solve. The similarity of this problem to GNSS should be obvious to the
reader. [34] shows importance of applying this robust estimation technique in
GNSS factor graph in mitigating multipath effects in urban canyons. The batch
nature of most of these robust estimation methods makes it suitable for use in
a factor graph rather than an EKF.

5 What are potential uses of factor graphs for

the GNSS and radio-navigation community?
The EKF has been the preferred choice for GNSS based state estimation due to
their simplicity, computational efficiency, and the fact that GNSS observation
models are well modeled by linear approximations and are often well charac-
terized by Gaussian errors. Despite this, there may be situations in which the
radio-navigation community can benefit from factor graph optimization.

6
 For one, recent work has emphasized the potential use and benefits of signals
of opportunity (SOP) in radio-navigation applications. SOP may include as cell
phone signals [35, 36, 37] and Low Earth Orbiting satellites. Use of SOPs often
includes a need to solve for an unknown or very uncertain transmitter location
and clock offsets. Therefore, this class of problems shares many parallels with
SLAM and therefore may enjoy similar benefits from the use of a factor graph
as recognized for visual or LIDAR based pose-graph SLAM.
Next, as discussed in the context of GNSS, many robust estimation tech-
niques have been developed for use in factor graphs. For use of GNSS urban
environments that is prone to multipath errors, the use of these robust factor
graphs may be beneficial. For example, the winning solution of the Google
SmartPhone decimeter challenge, which included a variety of datasets collected
different environmental settings, was indeed a factor graph implementation [38].
Finally, factor graph adoption may be beneficial simply because the factor
graph framework has become the standard state estimation paradigm within the
robotics and autonomy communities (i.e., pretty much every sensor modality,
other than GNSS, utilizes – and has shown the benefit of – the factor graph
framework). The adoption of a GNSS factor graph may enable more seam-
less integration between GNSS and other sensor modalities and integration of
multiple information sources is a well-recognized key to any critical navigation
system.

References
[1] D. Koller and N. Friedman, Probabilistic graphical models: principles and
techniques. MIT press, 2009.
[2] B. J. Frey, F. R. Kschischang, H.-A. Loeliger, and N. Wiberg, “Factor
graphs and algorithms,” in Proceedings of the Annual Allerton Conference
on Communication Control and Computing, vol. 35. Citeseer, 1997, pp.
666–680.
[3] F. Dellaert, “Factor graphs: Exploiting structure in robotics,” Annual Re-
view of Control, Robotics, and Autonomous Systems, vol. 4, pp. 141–166,
2021.
[4] F. Dellaert, M. Kaess et al., “Factor graphs for robot perception,” 2017.
[5] F. Dellaert, “Factor graphs and gtsam: A hands-on introduction,” Georgia
Institute of Technology, Tech. Rep., 2012.

[6] G. Grisetti, R. Kümmerle, H. Strasdat, and K. Konolige, “g2o: A general

framework for (hyper) graph optimization,” in Proceedings of the IEEE
International Conference on Robotics and Automation (ICRA), Shanghai,
China, 2011, pp. 9–13.
[7] S. Agarwal, K. Mierle, and Others, “Ceres solver,” http://ceres-solver.org.

7
 [8] B. M. Bell and F. W. Cathey, “The iterated kalman filter update as a
gauss-newton method,” IEEE Transactions on Automatic Control, vol. 38,
no. 2, pp. 294–297, 1993.
[9] M. L. Psiaki, “Backward-smoothing extended kalman filter,” Journal of
guidance, control, and dynamics, vol. 28, no. 5, pp. 885–894, 2005.

[10] W. Wen, T. Pfeifer, X. Bai, and L.-T. Hsu, “Factor graph optimization
for gnss/ins integration: A comparison with the extended kalman filter,”
NAVIGATION, Journal of the Institute of Navigation, vol. 68, no. 2, pp.
315–331, 2021.

[11] D. Wilbers, C. Merfels, and C. Stachniss, “Localization with sliding win-

dow factor graphs on third-party maps for automated driving,” in 2019
International Conference on Robotics and Automation (ICRA). IEEE,
2019, pp. 5951–5957.
[12] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert,
“isam2: Incremental smoothing and mapping using the bayes tree,” The
International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235,
2012.
[13] M. Kaess, V. Ila, R. Roberts, and F. Dellaert, “The bayes tree: An algo-
rithmic foundation for probabilistic robot mapping,” in Algorithmic Foun-
dations of Robotics IX. Springer, 2010, pp. 157–173.

[14] R. M. Watson and J. N. Gross, “Evaluation of kinematic precise point

positioning convergence with an incremental graph optimizer,” in 2018
IEEE/ION Position, Location and Navigation Symposium (PLANS), 2018,
pp. 589–596.

[15] W. Wen and L.-T. Hsu, “Towards robust gnss positioning and
real-time kinematic using factor graph optimization,” arXiv preprint
arXiv:2106.01594, 2021.
[16] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “Imu preintegration
on manifold for efficient visual-inertial maximum-a-posteriori estimation.”
Georgia Institute of Technology, 2015.
[17] N. Sünderhauf and P. Protzel, “Switchable constraints for robust pose
graph slam,” in 2012 IEEE/RSJ International Conference on Intelligent
Robots and Systems. IEEE, 2012, pp. 1879–1884.
[18] C. Zach and G. Bourmaud, “Iterated lifting for robust cost optimization,”
in BMVC, 2017.
[19] N. Sünderhauf, M. Obst, G. Wanielik, and P. Protzel, “Multipath mitiga-
tion in gnss-based localization using robust optimization,” in 2012 IEEE
Intelligent Vehicles Symposium. IEEE, 2012, pp. 784–789.

8
[20] P. Agarwal, G. D. Tipaldi, L. Spinello, C. Stachniss, and W. Burgard, “Ro-
bust map optimization using dynamic covariance scaling,” in 2013 IEEE
International Conference on Robotics and Automation. Ieee, 2013, pp.
62–69.
[21] E. Olson and P. Agarwal, “Inference on networks of mixtures for robust
robot mapping,” The International Journal of Robotics Research, vol. 32,
no. 7, pp. 826–840, 2013.
[22] T. Pfeifer, P. Weissig, S. Lange, and P. Protzel, “Robust factor graph
optimization-a comparison for sensor fusion applications,” in 2016 IEEE
21st International Conference on Emerging Technologies and Factory Au-
tomation (ETFA). IEEE, 2016, pp. 1–4.
[23] R. M. Watson and J. N. Gross, “Robust navigation in gnss degraded envi-
ronment using graph optimization,” in Proceedings of the 30th International
Technical Meeting of the Satellite Division of The Institute of Navigation
(ION GNSS+ 2017), 2017, pp. 2906–2918.
[24] R. M. Watson, J. N. Gross, C. N. Taylor, and R. C. Leishman, “Enabling
robust state estimation through measurement error covariance adaptation,”
IEEE Transactions on Aerospace and Electronic Systems, vol. 56, no. 3, pp.
2026–2040, 2019.
[25] ——, “Robust incremental state estimation through covariance adapta-
tion,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 3737–3744,
2020.
[26] M. Bosse, G. Agamennoni, I. Gilitschenski et al., Robust estimation and
applications in robotics.
[27] O. G. Crespillo, A. Andreetti, and A. Grosch, “Design and evaluation of
robust m-estimators for gnss positioning in urban environments,” in Pro-
ceedings of the 2020 International Technical Meeting of The Institute of
Navigation, San Diego, CA, USA, 2020, pp. 21–24.
[28] P. J. Huber, “Robust estimation of a location parameter,” in Breakthroughs
in statistics. Springer, 1992, pp. 492–518.
[29] G. Agamennoni, P. Furgale, and R. Siegwart, “Self-tuning m-estimators,”
in 2015 IEEE International Conference on Robotics and Automation
(ICRA). IEEE, 2015, pp. 4628–4635.
[30] J. T. Barron, “A general and adaptive robust loss function,” in Proceedings
of the IEEE/CVF Conference on Computer Vision and Pattern Recogni-
tion, 2019, pp. 4331–4339.
[31] H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated non-
convexity for robust spatial perception: From non-minimal solvers to global
outlier rejection,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp.
1127–1134, 2020.

9
[32] M. J. Black and A. Rangarajan, “On the unification of line processes,
outlier rejection, and robust statistics with applications in early vision,”
International journal of computer vision, vol. 19, no. 1, pp. 57–91, 1996.
[33] A. Blake and A. Zisserman, Visual reconstruction. MIT press, 1987.

[34] W. Wen, G. Zhang, and L.-T. Hsu, “Gnss outlier mitigation via
graduated non-convexity factor graph optimization,” arXiv preprint
arXiv:2109.00667, 2021.
[35] K. Shamaei and Z. M. Kassas, “Receiver design and time of arrival estima-
tion for opportunistic localization with 5g signals,” IEEE Transactions on
Wireless Communications, 2021.
[36] ——, “LTE receiver design and multipath analysis for navigation in ur-
ban environments,” NAVIGATION, Journal of the Institute of Navigation,
vol. 65, no. 4, pp. 655–675, 2018.
[37] J. J. Morales, J. Khalife, U. S. Cruz, and Z. M. Kassas, “Orbit modeling
for simultaneous tracking and navigation using leo satellite signals,” in
Proceedings of the 32nd International Technical Meeting of the Satellite
Division of The Institute of Navigation (ION GNSS+ 2019), 2019, pp.
2090–2099.
[38] T. Suzuki, “First place award winner of the smartphone decimeter chal-
lenge: Global optimization of position and velocity by factor graph opti-
mization,” in Proceedings of the 34th International Technical Meeting of
the Satellite Division of The Institute of Navigation (ION GNSS+ 2021),
2021, pp. 2974 – 2985.

10