Factor Graph Optimization
& Sensor Fusion

01 The State Estimation Problem

Before any equations, let us understand what problem we are actually solving. A robot, autonomous vehicle, or any mobile platform needs to know where it is, how fast it is moving, and how it is oriented in the world. Together these constitute the state — a vector that fully describes the relevant aspects of the system at a point in time.

The state is never directly observable. Sensors give us noisy, partial, and sometimes inconsistent glimpses of reality. A GPS receiver outputs a position with meter-level uncertainty. An IMU measures acceleration and angular velocity but accumulates error over time. A camera sees the world but cannot directly tell you your latitude. The goal of state estimation is to combine all available sensor data — with their known noise characteristics — to compute the best possible estimate of the state.

What makes this hard?

Three fundamental difficulties arise:

• 1
  Nonlinearity. The relationship between state and sensor measurement is rarely linear. IMU integration involves multiplying velocity by time and rotating acceleration vectors. GNSS pseudorange depends on the Euclidean distance between satellite and receiver. These nonlinearities prevent closed-form solutions in general.
• 2
  Noise on a manifold. Orientation — which is part of the state — does not live in Euclidean space. Rotations belong to the special orthogonal group SO(3), a curved manifold. Standard addition does not apply; you cannot write R_new = R_old + δ. This requires special mathematical machinery.
• 3
  Scale. A practical GNSS/IMU system runs for minutes to hours. At 200 Hz IMU and 10 Hz GNSS, that is tens of thousands of state variables coupled by hundreds of thousands of measurements. Naïve matrix operations are computationally impossible; we need structure-exploiting algorithms.

Two estimation paradigms: filtering vs. smoothing

Historically, two philosophies have emerged for handling temporal state estimation:

The Extended Kalman Filter (EKF) has dominated robotics since the 1990s due to its simplicity and low memory footprint. It works by maintaining only the current state estimate and its covariance, updating them as new measurements arrive. The cost is that it discards all past information except through its summarized covariance — an approximation that loses accuracy over time, especially in nonlinear systems.

Factor graph smoothing keeps every state variable and every measurement in a graph simultaneously, then finds the global maximum a posteriori (MAP) estimate over the full trajectory. This is more accurate because: (a) future measurements can retroactively improve past estimates, (b) loop closures in SLAM are handled exactly, and (c) the global optimization is not prone to linearization errors that accumulate in the EKF. Modern implementations (iSAM2, GTSAM) make this computationally feasible by exploiting the sparse structure of the graph.

Why GNSS + IMU?

Together, GNSS and IMU are complementary: GNSS gives absolute position at low rate; IMU gives high-rate motion with bounded but growing error. A factor graph can fuse them optimally — the GNSS factors anchor the trajectory globally while the IMU preintegration factors provide tight high-frequency constraints between keyframes.

The full pipeline — preview

Part 1 of this guide builds all the mathematical machinery needed to understand the optimization block (Chapters 2–7). Part 2 fills in the sensor models and assembles the full pipeline with working code.

02 Probabilistic Foundations

Factor graph optimization is, at its core, Bayesian inference expressed as a least squares problem. To understand why optimization is the right tool, you need to understand the probabilistic reasoning underneath it.

The Bayesian framework

We model the unknown state as a random variable x and sensor measurements as random variables z. We have two pieces of information:

• 1
  Prior belief p(x): what we know about the state before seeing any measurements. For the first pose of a trajectory, this is often a delta function (known starting point) or a broad Gaussian (uncertain starting position).
• 2
  Likelihood p(z|x): the sensor model — given state x, how likely is measurement z? This encodes the noise model of the sensor. A GNSS receiver measuring position with 1 m standard deviation contributes a Gaussian likelihood.

Bayes' theorem combines these into the posterior:

The posterior encodes everything we know about x after seeing z. For multiple independent measurements z₁, …, z_n, the joint likelihood factorizes:

This is the fundamental equation that factor graphs compute. The graph structure makes the product over factors explicit and computationally exploitable.

Maximum A Posteriori (MAP) estimation

For continuous state spaces, we cannot enumerate all possibilities. Instead, we find the single most probable state — the mode of the posterior. This is the MAP estimate:

Taking the logarithm (which is monotone, so does not change the argmax) and negating (turning maximization into minimization):

This is a scalar function we can minimize with standard optimization tools. The log transformation is crucial: it turns products into sums, exponentials into quadratics, and multiplicative uncertainty into additive cost.

Gaussian distributions and their log-likelihood

In almost all sensor fusion problems, we assume Gaussian noise. This is motivated by the Central Limit Theorem (many small independent errors sum to Gaussian), mathematical tractability, and practical effectiveness. For a multivariate Gaussian with mean μ and covariance Σ:

Taking the negative log:

The expression ‖e‖²_Σ = e^T Σ⁻¹ e is called the Mahalanobis distance squared. It generalizes Euclidean distance by scaling each dimension by its uncertainty — a 1 cm error in a 1 mm-precision sensor costs 100× more than in a 10 cm-precision sensor.

The least squares connection

Now substitute Gaussian noise models into Equation (2.3). Each measurement factor p(z_i | x) = 𝒩(h_i(x), Σ_i) where h_i is the measurement function (how the state maps to expected measurement). The negative log-likelihood of this factor is:

Summing over all factors, the MAP problem becomes:

This is nonlinear weighted least squares. Every factor graph problem with Gaussian noise reduces to this form. The prior factor contributes ½ ‖x − μ₀‖²_Ω₀ and each measurement factor contributes ½ ‖z_i − h_i(x)‖²_Ωi.

Whitening: reducing to standard least squares

Any weighted problem can be converted to standard (unweighted) least squares via the whitening transformation. If Ω = L^TL (upper Cholesky), then:

Defining the whitened residual r̃ = L r, the problem becomes standard least squares ½ ‖r̃‖². Whitening is not just a mathematical trick — it has practical importance: the square root factor L (not Ω itself) is what iSAM2 maintains and updates incrementally. Working with L instead of Ω improves numerical stability and enables efficient rank-one updates.

Maximum Likelihood vs MAP

When there is no prior (p(x) = const, or equivalently Ω₀ → 0), MAP reduces to Maximum Likelihood Estimation (MLE): just minimize the measurement residuals. In practice for sensor fusion, we always include at least a prior on the first state (to fix the reference frame) and priors on IMU biases (from calibration). Pure MLE on a GNSS/IMU system would produce an under-constrained problem in the orientation space.

03 Graphical Models

We established that the MAP problem involves a product of factors. How we represent and organize those factors determines how efficiently we can solve the problem. Graphical models provide the representation; graph structure exposes the sparsity that makes optimization tractable.

Why graphs?

The joint distribution over all state variables and measurements is typically intractable to reason about directly. A vehicle navigating for 10 minutes at 10 Hz has 6000 pose states. The full covariance matrix would be 36,000 × 36,000 — over a billion entries, impossible to store or invert. But in reality, state x₅ only directly constrains states x₄ and x₆ (through the motion model). The GNSS measurement at time 5 constrains only x₅. The joint distribution has sparse conditional independence structure, and graphs make this structure explicit.

Bayesian Networks (Directed Graphical Models)

A Bayesian Network (BN) is a directed acyclic graph (DAG) where each node represents a random variable and each directed edge encodes a conditional dependency. The joint distribution factorizes as:

For a robot motion model with states x_0:T and IMU controls u_1:T:

This is the standard state-space model underlying the Kalman filter. Bayesian networks are powerful for representing causal, temporal structure.

Markov Random Fields (Undirected)

Some constraints are symmetric — a distance between two poses is the same whether you measure it from pose A to pose B or B to A. Undirected graphical models (Markov Random Fields / MRFs) handle these naturally. The joint distribution is a product of potential functions over cliques:

where Z is the partition function (normalizing constant). MRFs are natural for loop-closure constraints in SLAM, where closing a loop creates a symmetric constraint between two poses.

Factor Graphs — the best of both worlds

Factor graphs are bipartite graphs with two types of nodes:

• ○
  Variable nodes (circles): represent the unknowns — poses, velocities, biases. These are what we optimize over.
• ■
  Factor nodes (squares): represent constraints between variables — measurement factors, motion factors, prior factors. Each factor encodes a probability distribution involving its connected variables.

Every BN and MRF can be converted to a factor graph. The power of factor graphs is that they make the factorization structure of the joint distribution visually and computationally explicit. The joint probability is:

Conditional independence from graph structure

The most powerful property of factor graphs is that the graph structure directly encodes conditional independence. Two variables x_i and x_j are conditionally independent given a set of variables S if all paths between them in the factor graph pass through S. This is the graphical model analog of the Markov property, and it is what allows local computations (variable elimination, belief propagation) to produce globally correct results.

In a GNSS/IMU fusion graph, state x₅ is conditionally independent of state x₂ given x₄ — because the only path from x₂ to x₅ passes through x₃ and x₄. This means when we update x₅ based on a new GNSS measurement, we only need to process the local neighborhood of the graph, not reprocess everything from the beginning. This is the key insight that makes incremental smoothing (iSAM2) efficient.

04 The Factor Graph Formalism

With the probabilistic motivation and graphical model foundation in place, we can now define factor graphs precisely and show how they encode the GNSS/IMU state estimation problem.

Formal definition

A factor graph G = (V, F, E) consists of:

• V
  Variable nodes V = {x₁, …, x_n}: the unknowns we want to estimate. In GNSS/IMU fusion, these are navigation states — pose, velocity, IMU biases at each keyframe.
• F
  Factor nodes F = {f₁, …, f_m}: the probabilistic constraints. Each factor f_i is a function of a subset of variables X_i ⊆ V. It encodes the probability distribution of a measurement or a prior.
• E
  Edges E ⊆ V × F: undirected edges connecting each factor to its variable nodes. A factor f_i is connected to exactly those variables X_i it depends on. There are no edges between variable nodes or between factor nodes (bipartite).

The graph represents the factorized joint distribution of Equation (3.1). Finding the MAP estimate means finding the variable assignment that maximizes this joint, or equivalently minimizes the sum of negative log-factors.

Factor types in sensor fusion

The objective function

For Gaussian factors, the MAP problem (Equation 2.6) with factor graph structure becomes:

Each term is a factor's contribution to the total cost. Minimizing F over all state variables simultaneously yields the global MAP estimate — the trajectory that best explains all measurements given the noise models.

The information matrix and sparsity

When we linearize F around a current estimate (Chapter 5), the resulting Hessian H has exactly the same sparsity pattern as the factor graph connectivity. If two variables x_i and x_j are not connected to any common factor, the corresponding H_ij block is zero. This is why factor graph optimization is tractable at scale: for a chain-structured GNSS/IMU graph (no loop closures), H is block-tridiagonal — only O(n) nonzero blocks — and can be solved in O(n) time via forward/backward substitution.

A minimal working example in Python (GTSAM)

Before diving into the optimization algorithms, here is a complete minimal factor graph: 1D robot position estimated from two noisy measurements. This makes the abstract formalism concrete.

import gtsam
import numpy as np

# --- Define variable keys ---
# GTSAM uses integer keys to identify variables
X0 = gtsam.symbol('x', 0)   # position at time 0
X1 = gtsam.symbol('x', 1)   # position at time 1

# --- Create an empty nonlinear factor graph ---
graph = gtsam.NonlinearFactorGraph()

# --- Prior factor: x0 ~ N(0.0, sigma=0.5) ---
prior_noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.5)
graph.add(gtsam.PriorFactorDouble(X0, 0.0, prior_noise))

# --- Between factor: x1 - x0 ~ N(1.0, sigma=0.1) (odometry) ---
odom_noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.1)
graph.add(gtsam.BetweenFactorDouble(X0, X1, 1.0, odom_noise))

# --- Unary measurement: x1 ~ N(1.2, sigma=0.3) (GPS) ---
gps_noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.3)
graph.add(gtsam.PriorFactorDouble(X1, 1.2, gps_noise))

# --- Initial values (linearization point) ---
init = gtsam.Values()
init.insert(X0, 0.0)
init.insert(X1, 1.0)

# --- Optimize with Gauss-Newton ---
params = gtsam.GaussNewtonParams()
params.setRelativeErrorTol(1e-6)
optimizer = gtsam.GaussNewtonOptimizer(graph, init, params)
result = optimizer.optimize()

# --- Extract result ---
print(f"x0 = {result.atDouble(X0):.4f}")   # ≈ 0.000
print(f"x1 = {result.atDouble(X1):.4f}")   # ≈ 1.057 (weighted avg of 1.0+0.0 and 1.2)

# --- Marginal covariances ---
marginals = gtsam.Marginals(graph, result)
print(f"cov(x1) = {marginals.marginalCovariance(X1)[0,0]:.4f}")  # ≈ 0.0082
Python · GTSAM

05 Nonlinear Least Squares

The objective function F(x) in Equation (4.1) is nonlinear because the measurement functions h_i(x) are nonlinear (rotation matrices, trigonometric functions, etc.). We cannot solve for the minimum analytically. We iteratively linearize and solve linear systems — this is the foundation of all factor graph solvers.

Residuals and the cost function

For each factor i, define the residual:

The total cost is:

For a Gaussian measurement factor, r_i = 0 when the measurement is perfectly predicted. For a GNSS factor, r_GNSS = p_measured − R_body→world · p_state measures how much the current trajectory state disagrees with the GNSS fix.

Jacobians and linearization

To linearize, expand r_i around the current estimate x₀ using a first-order Taylor expansion:

Substituting into (5.1) and minimizing over δ:

where:

The Gauss-Newton method

Setting the gradient of the quadratic approximation to zero gives the normal equations:

The "⊕" notation is intentional: for variables living on manifolds (like rotations), the update is not simple addition but a retraction (Chapter 7). For Euclidean variables, ⊕ is just addition.

The full Gauss-Newton iteration:

def gauss_newton(factors, x0, max_iter=50, tol=1e-8):
    x = x0.copy()
    for iteration in range(max_iter):
        # Step 1: Evaluate all residuals and Jacobians at current x
        H = np.zeros((n, n))          # Hessian accumulator
        b = np.zeros(n)               # Gradient accumulator
        cost = 0.0

        for factor in factors:
            r, J = factor.evaluate(x)  # residual and Jacobian
            Omega = factor.information_matrix()
            H += J.T @ Omega @ J       # accumulate Hessian
            b += J.T @ Omega @ r       # accumulate gradient
            cost += 0.5 * r @ Omega @ r

        # Step 2: Solve normal equations H·δ = -b
        delta = np.linalg.solve(H, -b)

        # Step 3: Update state (retract on manifold)
        x = retract(x, delta)

        # Step 4: Check convergence
        if np.linalg.norm(delta) < tol:
            break

    return x, cost
Python · Pseudocode

Levenberg-Marquardt (LM)

Gauss-Newton can diverge when the linear approximation is poor — this happens far from the minimum (early iterations) or in regions of high curvature. The Levenberg-Marquardt algorithm adds a damping term that interpolates between Gauss-Newton and gradient descent:

The damping parameter λ is adapted automatically:

• ↑
  If the step increased cost (step was bad): increase λ → move toward gradient descent (smaller, safer step).
• ↓
  If the step decreased cost (step was good): decrease λ → move toward Gauss-Newton (larger, faster step).

LM has a beautiful trust-region interpretation: λ implicitly controls the radius of a ball around the current point within which the quadratic model is trusted. When things go wrong, shrink the ball; when they go right, expand it. This makes LM robust to poor initialization and widely used in robotics (Ceres Solver, GTSAM both use LM variants).

Convergence criteria

Three standard checks determine convergence. All three should pass before declaring success:

Typical values: ε₁ = 10⁻⁸, ε₂ = 10⁻⁵, ε₃ = 10⁻⁶. For sensor fusion with well-conditioned problems, convergence in 5–15 Gauss-Newton iterations is typical. The cost of one iteration is dominated by solving the linear system, which we address next.

Analytic vs automatic differentiation

Computing Jacobians J_i = ∂r_i/∂x has two practical approaches:

For performance-critical sensor fusion (real-time SLAM), analytic Jacobians are standard. For prototyping and research, GTSAM's Expression and Ceres's automatic differentiation are excellent. Numerical Jacobians are invaluable for checking your analytic implementation: if the analytic and numerical Jacobians disagree by more than ~10⁻⁵, your analytic derivation has a bug.

06 Solving the Linear System

Each Gauss-Newton iteration reduces to solving the linear system H·δ = −b, where H is a large sparse symmetric positive definite (SPD) matrix. The efficiency of this solve — exploiting the sparsity from the factor graph structure — is what makes large-scale sensor fusion tractable.

The sparsity of H

H = ∑_i J_i^T Ω_i J_i inherits the sparsity of the Jacobians. Block H_jk is nonzero only when variables x_j and x_k appear together in at least one factor. For a sequential GNSS/IMU trajectory with no loop closures, H is block-tridiagonal (each state only directly coupled to its neighbors). This sparsity is the key property that separates factor graph backends from brute-force matrix inversion.

Cholesky factorization

For an SPD matrix H, the standard solver is Cholesky factorization: H = L L^T where L is lower triangular. Then:

where nnz is the number of nonzero entries in L. For a tridiagonal H of size n×n, nnz = O(n) and the entire solve is O(n). Compare this to O(n³) for dense Cholesky — the difference for a 10,000-state trajectory is roughly 10¹² vs 10⁴ operations.

Variable ordering and fill-in

When Cholesky-factoring a sparse H, the triangular factor L can be denser than H — this is called fill-in. The amount of fill-in depends critically on the variable ordering (the order in which variables appear in the vector x, which determines the row/column order in H).

Consider a factor graph with a loop closure: state x₀ and state x₁₀₀ are connected by a factor. This creates H_0,100 ≠ 0, and Cholesky propagates this coupling to fill all blocks H_i,j for 0 ≤ i,j ≤ 100. The optimal ordering (nested dissection, COLAMD, AMD) minimizes fill-in by clever reordering. In practice, GTSAM and g2o use COLAMD or AMD automatically — the programmer rarely needs to think about this.

QR factorization and the square root information form

An alternative to Cholesky is to factor the Jacobian matrix J directly (before forming H = J^TΩJ):

The upper triangular R is the square root information matrix (also called the R-factor or ℛ). Solving the normal equations becomes R^T R δ = −b, equivalent to Cholesky but numerically more stable (H's condition number is the square of J's condition number — QR avoids this squaring). GTSAM internally maintains R and updates it incrementally.

iSAM: Incremental Smoothing and Mapping

In online sensor fusion, new states and factors are added continuously. Rerunning Cholesky from scratch every time is wasteful — most of the existing factorization is still valid. iSAM (Kaess et al., 2008) exploits this by maintaining the Cholesky factor and updating it incrementally using rank-one (or rank-k) updates when new factors arrive.

The cost of adding a new factor that involves m variables is O(m·nnz_affected) rather than O(n·nnz_total). For a chain-structured problem, this is O(1) amortized per new factor — the same cost as the EKF, but with globally consistent estimates.

iSAM2 and the Bayes tree

iSAM had a limitation: when a non-leaf variable is affected by a new measurement (e.g., an old pose receives a loop closure), the entire subtree below it in the elimination tree must be refactored. iSAM2 (Kaess et al., 2012) solves this with the Bayes tree — a data structure that organizes the factorized Bayes net into cliques, enabling truly local updates.

When a new measurement arrives, iSAM2:

1. 1
   Identifies the affected cliques — those in the subtree rooted at the clique containing the new variables.
2. 2
   Removes the affected cliques from the Bayes tree (converting them back to factors).
3. 3
   Adds the new factor and re-eliminates only the affected variables (a local Cholesky on a small subproblem).
4. 4
   Updates the Bayes tree with the new cliques and propagates back-substitution (fluid relinearization) only through affected variables.

For a chain graph (no loop closures), only one clique is ever affected per new measurement — O(1) work. For loop closures, multiple cliques may need re-elimination, but the tree structure keeps this local. In practice, iSAM2 achieves near-constant time per factor addition for most SLAM/fusion problems.

Library comparison

For GNSS/IMU fusion, GTSAM is the standard choice: it has prebuilt IMU preintegration factors, a Python API, active development, and iSAM2 for real-time use. The examples in Part 2 use GTSAM.

07 Lie Groups & Manifold Optimization

Everything in Chapters 2–6 assumed variables live in Euclidean space ℝⁿ, where adding a correction δ to a state x is well-defined. Rotations break this assumption. This chapter explains why, and provides the mathematical tools to fix it.

The problem with representing rotations

A 3D rotation has exactly 3 degrees of freedom (pitch, roll, yaw). Yet every common representation has problems:

For optimization, the rotation matrix is actually the best internal representation — it is closed under composition (R₁R₂ is always a rotation matrix) and easy to differentiate. The challenge is that we cannot perform unconstrained optimization over the 9 entries of R — we must ensure the result stays in SO(3). The solution is optimization on manifolds.

Groups and Lie groups

A group (G, ∘) is a set G with a binary operation ∘ satisfying: (1) closure (a∘b ∈ G), (2) associativity, (3) identity element e, (4) inverse for every element. The set of 3D rotations with composition forms the group SO(3).

A Lie group is a group that is simultaneously a smooth manifold — you can do calculus on it. The group operation and inverse are smooth (differentiable) maps. This is what enables the connection to optimization: we can define derivatives, tangent spaces, and exponential maps.

The special orthogonal group SO(3)

SO(3) is a 3-dimensional manifold embedded in ℝ⁹. It looks locally like ℝ³ near any point, but globally it is curved — think of the surface of a sphere, which locally looks flat but globally is not.

The Lie algebra so(3)

The Lie algebra g of a Lie group G is the tangent space at the identity element. For SO(3), the identity is I (the 3×3 identity matrix), and the tangent space is the set of skew-symmetric matrices:

Every element of so(3) has the form:

The hat operator (∧): φ ∈ ℝ³ → [φ]_× ∈ so(3) converts a 3D vector to a skew-symmetric matrix. The vee operator (∨) is its inverse: [φ]_×^∨ = φ.

The exponential map: Rodrigues' rotation formula

The exponential map takes a Lie algebra element (tangent vector at identity) to a Lie group element (a rotation):

The inverse, the logarithmic map, maps a rotation back to its rotation vector:

These formulas are used constantly in factor graph optimization: the IMU preintegration integrates angular velocity by repeatedly applying Exp, and the optimizer computes linearization Jacobians using derivatives of Exp and Log.

import numpy as np

def hat(phi):
    """R³ → so(3): skew-symmetric matrix from rotation vector."""
    return np.array([[       0, -phi[2],  phi[1]],
                     [ phi[2],       0, -phi[0]],
                     [-phi[1],  phi[0],       0]])

def Exp(phi):
    """so(3) → SO(3): Rodrigues' rotation formula."""
    theta = np.linalg.norm(phi)
    if theta < 1e-8:
        return np.eye(3) + hat(phi)   # first-order approx for small angles
    K = hat(phi / theta)
    return np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * K @ K

def Log(R):
    """SO(3) → so(3): matrix logarithm."""
    theta = np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1))
    if theta < 1e-8:
        return (R - R.T) / 2           # first-order approx
    return (theta / (2 * np.sin(theta))) * (R - R.T)

def vee(Phi):
    """so(3) → R³: extract rotation vector from skew-symmetric matrix."""
    return np.array([Phi[2,1], Phi[0,2], Phi[1,0]])

# Example: 90-degree rotation about z-axis
phi = np.array([0, 0, np.pi/2])
R = Exp(phi)
phi_recovered = vee(Log(R))
print(phi_recovered)  # [0, 0, 1.5708] ✓
Python · SO(3)

The special Euclidean group SE(3)

A full rigid body pose (position + orientation) is an element of SE(3):

The 4×4 matrix T is the homogeneous transformation matrix. It acts on homogeneous points p = [x;y;z;1] as T·p = [Rp+t; 1]. The Lie algebra is se(3) — 4×4 matrices of the form [[φ]_×, ρ; 0, 0] where ρ ∈ ℝ³. SE(3) has 6 degrees of freedom (3 rotation + 3 translation), corresponding to the 6 DoF state of a rigid body.

Perturbation model: how to update rotations in optimization

In Gauss-Newton, we compute a correction δ ∈ ℝ⁶ (for SE(3)) and need to apply it to the current estimate T. The right perturbation model is:

This is the retraction — the operation that takes a current point on the manifold and a tangent vector, and produces a new point on the manifold. It is what the optimizer uses instead of naive addition. The choice of left vs right perturbation affects the form of the Jacobians but not the final result; GTSAM uses right perturbation for its Pose3 type.

Jacobians on manifolds

When a factor residual r_i depends on a rotation R, the Jacobian ∂r_i/∂R is not well-defined in the usual sense (R is constrained). Instead, we compute the perturbation Jacobian:

The 3-column Jacobian J_i,R is what enters the normal equations. It accounts for the curved geometry of SO(3) and produces numerically stable updates that always stay on the rotation manifold. GTSAM computes these Jacobians automatically for its built-in types (Rot3, Pose3) and provides a framework for user-defined types.

The full state manifold for GNSS/IMU

The navigation state at keyframe k is:

where R_k is orientation, p_k is position, v_k is velocity, b^a_k is accelerometer bias, and b^g_k is gyroscope bias. The total dimension per keyframe is 3+3+3+3+3 = 15 parameters (but only 3+3+3+3+3 = 15 degrees of freedom since SO(3) has 3 DoF). The full trajectory over K keyframes has 15K variables. A typical 1-minute drive at 10 Hz keyframes has 600×15 = 9,000 optimization variables — well within the reach of modern sparse solvers.