A real-time estimator must decide what information to retain from past states. Derive marginalization with the Schur complement, then compare a filter, a sliding window, and a current-state solve. Identify which states can be relinearized and which correlations survive.
0 · Warm-up, from memory
From lesson 5. You'll see it again in section 3, applied to a whole state.
From memory: write the preintegration covariance recursion and its starting value.
- Sandwiched: \(\mathbf{A}\Sigma\mathbf{A}^{\top}\)
- Plus fresh noise, \(\mathbf{B}\Sigma_{\eta}\mathbf{B}^{\top}\)
A filter runs exactly this shape on its whole state: \(\mathbf{P} \leftarrow \mathbf{F}\mathbf{P}\mathbf{F}^{\top} + \mathbf{G}\mathbf{Q}\mathbf{G}^{\top}\).
1 · The question every estimator answers
Strasdat, Montiel & Davison frame the choice in two sentences: "Filtering methods marginalise out past poses and summarise the information gained over time with a probability distribution. Keyframe methods retain the optimisation approach of global bundle adjustment, but computationally must select only a small number of past frames to process."Strasdat, Montiel & Davison, Visual SLAM: Why Filter?, Image and Vision Computing, 2012 (authors' PDF), abstract. Both are ways of forgetting. The difference is how, and that's where marginalization comes in.
2 · Marginalization
Write the normal equations in information form, split into a block \(m\) to remove and a block \(k\) to keep:
Assume a symmetric positive definite information matrix and a linear Gaussian model (or a fixed local linearization). Solve the top row for \(\mathbf{x}_m\) and substitute it into the bottom row. That's the Schur complement:
Solving \(\mathbf{H}'\mathbf{x}_k = \mathbf{b}'\) gives exactly the same \(\mathbf{x}_k\) as the full system.
\(\mathbf{H}'\) is the prior the survivors carry: glass-core calls it "the EXACT summary of every factor that touched the
marginalized variable".marginalization.hpp,
lines 9–57, which solves against \(\mathbf{H}_{mm}\) with LDLT rather than inverting it. Compare three ways of getting rid of \(\mathbf{x}_m\):
- Full solve: solve for both blocks and read off \(\mathbf{x}_k\). The reference answer.
- Marginalize \(m\): the Schur complement gives the same \(\mathbf{x}_k\). Exact.
- Hold \(\mathbf{x}_m\) fixed and ignore its uncertainty: solve \(\mathbf{H}_{kk}\mathbf{x}_k = \mathbf{b}_k - \mathbf{H}_{km}\mathbf{x}_m\). Generally the wrong \(\mathbf{x}_k\), and with information \(\mathbf{H}_{kk}\), which exceeds \(\mathbf{H}'\) by the subtracted term: overconfident.
glass-core's test checks exactly these two claims: the Schur solve must match the full solve,
and "hold-fixed (the freeze)" must not.test_marginalization.cpp,
lines 43–67: the naive solve must differ, "or the information the dropped block carried
forward was zero". One cost to know: \(\mathbf{H}_{km}\mathbf{H}_{mm}^{-1}\mathbf{H}_{mk}\) couples everything the removed variable
touched, so the prior can become dense. In a nonlinear problem this summary is exact only for the chosen linearization; discarded nonlinear factors cannot subsequently be relinearized.
H_marg = Hkk - Hmk.transpose() * Hmm_inv_Hmk;
b_marg = bk - Hmk.transpose() * Hmm_inv_bm;3 · Four answers to the question
| Design | Keeps | Re-linearises | The past |
|---|---|---|---|
| Filter (EKF, iterated EKF: FAST-LIO2) | current state + its covariance | the current state only | marginalized every step |
| Window, marginalize (VINS-Mono) | last \(N\) states | all \(N\), every solve | oldest turned into a prior |
| Window, drop (simple bounded window) | last \(N\) states | retained states | oldest dropped; its information is lost |
| Current-state factor solve (one-step optimizer) | the new state \(\mathbf{x}_j\) | \(\mathbf{x}_j\) only | \(\mathbf{x}_i\) held fixed; its covariance carried |
The filter. Propagate \(\mathbf{P}\leftarrow\mathbf{F}\mathbf{P}\mathbf{F}^{\top}+\mathbf{G}\mathbf{Q}\mathbf{G}^{\top}\) across the IMU interval, the warm-up's shape, then fold in the measurement. The past is marginalized as you go, which is why a filter is cheap. It can never go back and re-linearise an old state. FAST-LIO2 is "a highly efficient tightly-coupled iterated Kalman filter", which re-linearises only the current state, a few times per update.Xu et al., FAST-LIO2, abstract.
The window. A fixed-lag optimizer retains recent states and relinearizes them. When the window is full, it can marginalize the oldest state into a prior, as in VINS-Mono,Qin, Li & Shen, VINS-Mono, IEEE T-RO 2018, Marginalization subsection. or simply drop its measurements. The latter bounds cost but discards information. Fixing an uncertain state as an anchor changes the problem again: it treats that estimate as exact.
Implementation example: a window that drops old keyframes
The linked glass-vio revision uses pop_front() when its window is full.
That is an example of the drop policy, not a property of every windowed estimator.
See the implementation.
The current-state factor solve. A factor is a term in an objective, not a mutually exclusive alternative to a filter or smoother. glass-lio optimizes the new state and gravity, inflates the IMU covariance by the previous state's uncertainty, and carries selected covariance blocks. Not revisiting old states does not by itself distinguish it from a filter. The important question is which joint distribution and correlations its prior represents.
Extracting covariance without inventing information
Let \(H_{\rm total}=H_{\rm prior}+H_{\rm data}\) include all four residual blocks. For the resulting nonsingular local Gaussian approximation,
Here \(b\) labels bias variables and \(o\) all other variables. The block \(H_{bb}^{-1}\) is the covariance conditional on the other variables being fixed, not their marginal covariance, and whenever the blocks are coupled it is smaller than the marginal.
An earlier implementation added \(P_b^{-1}\) to \(H_{bb}\) even though that block already contained the bias prior, counting the prior twice. Even with no new information, that would shrink the variance on every update. The revised implementation solves \(H_{\rm total}P=I\) once and extracts the nav and bias blocks from \(P\), including their coupling to gravity in this solve.
Eigen::Matrix<double, kTightDim, kTightDim> posterior;
if (r.posteriorCovariance(posterior)) {
nav_cov_ = posterior.topLeftCorner<kNavDim, kNavDim>();
bias_cov_ = posterior.block<6, 6>(kIdxBg, kIdxBg);
}Read the current covariance extraction and caller; older commit-pinned links elsewhere document the historical implementation.
This is still a local covariance estimate at the last linearization and robust weights, not a complete Bayesian filter. The estimator carries selected blocks, uses a fixed gravity prior, and separately approximates previous-state and bias uncertainty; cross-factor correlations are not all preserved. Correct block extraction fixes these two algebraic errors, not every possible source of inconsistency.
4 · Two shapes of marginalization: a chain and a landmark
Section 2 proved marginalization exact. Two slightly bigger shapes show the property that decides whether it is cheap: what happens to the sparsity of \(\mathbf{H}\).
A chain stays a chain
Take poses linked like a trajectory: a prior on \(x_0\), then odometry factors \(x_0\!\to\!x_1\) and \(x_1\!\to\!x_2\). The information matrix is tridiagonal. Marginalize \(x_0\) and only the \(x_1\) entry changes; the \(x_2\) row is untouched. What is added is a new prior on \(x_1\), and it has a physical reading: its variance is the prior's variance plus the odometry's variance. Marginalizing the start of a chain is just combining uncertainties in series. The chain stays a chain, and the cost stays constant however long the trajectory gets. A filter is exactly this, applied every step.
A landmark fills in
Now marginalize a landmark \(l\) observed from several poses. The landmark's row connects it to every pose that saw it, and the Schur complement adds \(-\mathbf{H}_{xl}\mathbf{H}_{ll}^{-1}\mathbf{H}_{lx}\) to the pose block: a dense term over all of them.
Every one of those poses is now coupled to every other, including pairs that never shared a factor. This is fill-in: the removed variable's connections become a dense clique among its neighbours. Marginalize many landmarks from a window of keyframes and the keyframes' prior becomes one dense block, which is why VINS-Mono's marginalization is its most expensive and most delicate step, and why window design has to account for both computation and the resulting prior.
Try it: why do windowed estimators usually marginalize landmarks before the oldest keyframe?
A landmark seen only by the oldest keyframe can be marginalized with the keyframe at little cost. But landmarks seen by several keyframes cause fill-in among all of them. Removing them first, or dropping their measurements entirely, keeps the resulting prior on the survivors small and structured. The choice of what to marginalize, and in which order, is a sparsity decision as much as a statistical one.
5 · Previous-state uncertainty in a factor
One way to account for an uncertain starting state in an IMU factor is to inflate its covariance: \(\Sigma_{\text{eff}} = \Sigma_{\text{pre}} + \mathbf{J}_i\mathbf{P}_i\mathbf{J}_i^\top\). The Schur complement explains what that line really is. Take the simplest version: \(x_j = x_i + d\), with measurement noise variance \(\sigma^2\) and a prior on \(x_i\) with variance \(p\). The joint information on \([x_i;\ x_j]\) and its marginal on \(x_j\) are
The marginal information is \(1/(p+\sigma^2)\): the variances add. That is the inflation, derived. Adding \(\mathbf{J}_i\mathbf{P}_i\mathbf{J}_i^\top\) to the factor's covariance is marginalizing a Gaussian \(\mathbf{x}_i\) out of this one factor. In the glass-lio case study, this is a one-factor marginalization of \(\mathbf{x}_i\); the estimator then carries forward selected covariance blocks.
What it would take to become a full filter
Compare that with the filter row of section 3. Three things separate the two:
- Propagating the full state covariance across the interval,
\(\mathbf{P}\leftarrow\mathbf{F}\mathbf{P}\mathbf{F}^\top + \mathbf{G}\mathbf{Q}\mathbf{G}^\top\). glass-core already computes the
state-transition Jacobian \(\mathbf{F}\) (
imuStateTransition); what a filter needs is a caller that applies it every scan to the whole state. - Keeping every cross-correlation. glass-lio carries a 15×15 navigation covariance, including position–bias correlations, but omits correlations with gravity and the complete joint history. A full-state filter would propagate the complete current-state covariance through the prediction and update.
- Iterating the update's linearization, as FAST-LIO2's iterated EKF does, which glass-lio's Gauss-Newton loop already approximates.
None of these requires a new mathematical tool. Each is the same Schur complement from section 2, applied more completely. The difference between glass-lio and a filter is how much of the joint distribution it chooses to keep, not a different theory.
6 · Why a wrong marginalization injects information
A marginalization prior is exact only for the chosen linearization. In a nonlinear estimator, combining it carelessly with later linearizations can make the resulting uncertainty too small.
When a state is marginalized, its prior \(\mathbf{H}'\) is frozen with Jacobians evaluated at the estimate held at that moment. The surviving states keep being re-linearized at newer, better estimates. Now the problem contains Jacobians of the same variables evaluated at two different points. For a linear problem that does not matter. For rotations it does: some directions that should be unobservable, such as the global yaw of the whole trajectory, which no local measurement can fix, get a little information from the mismatch between the two linearizations. The estimator starts to believe it knows something nothing measured. It becomes overconfident, and an overconfident estimator weighs new measurements too lightly and drifts while reporting a small covariance.
The standard remedy is first-estimate Jacobians (FEJ): evaluate every Jacobian that touches a marginalized state at that state's first estimate, so all factors agree on the linearization point and the unobservable directions stay unobservable. Filter-based visual–inertial systems also use FEJ to preserve the correct unobservable directions. It is a consistency technique, not a guarantee that every covariance is calibrated.
The consistency test
An estimator is consistent if its errors, measured against ground truth, are about as large as its own covariance claims. The usual check is the normalised estimation error squared (NEES): average \(\mathbf{e}^\top\mathbf{P}^{-1}\mathbf{e}\) over many runs and compare it with the state dimension. Far above it means overconfident, which is the signature of injected information. It is a useful statistical test for this class of overconfidence, which a trajectory plot alone may miss.
7 · So which?
There is no universal winner. A filter keeps a compact current distribution and cannot relinearize past states. A sliding window can refine recent states at a bounded computational cost, but it must decide how to remove old information. A current-state solve can be simpler while approximating some past correlations. Choose based on the sensor model, available compute, and which uncertainties the application must preserve. Dropping measurements loses information; fixing an uncertain estimate can invent certainty. Fixing a coordinate gauge to define the reference frame is a separate operation.
Explain the idea
A filter summarizes past measurements in the current state distribution and cannot relinearize old states. A sliding window retains recent states and can refine them together; marginalizing its oldest state preserves a local linearized prior but may create dense couplings. A current-state solve can also account for an uncertain previous state within a factor. Dropping measurements loses information, while fixing an uncertain estimate as exact can make the result overconfident.
8 · Practice
From memory, no scrolling. The last question comes from lesson 3.
What does a filter keep about the past?
"Filtering methods marginalise out past poses and summarise the information gained over time with a probability distribution." In this filtering model, past information is summarized by the current state's mean and covariance.
Marginalize \(\mathbf{x}_m\) when it is coupled to \(\mathbf{x}_k\). Compared with \(\mathbf{H}_{kk}\), the survivors' information \(\mathbf{H}'\) is…
\(\mathbf{H}' = \mathbf{H}_{kk} - \mathbf{H}_{km}\mathbf{H}_{mm}^{-1}\mathbf{H}_{mk}\). Reading \(\mathbf{H}_{kk}\) alone is what you'd get by ignoring the coupling, which overstates the information.
Why is holding an old state fixed worse than marginalizing it?
Fixing ignores the old state's uncertainty and its coupling to the survivors, so the answer is biased and overconfident: it keeps \(\mathbf{H}_{kk}\) where the truth is the smaller \(\mathbf{H}'\). glass-core's test calls it "the freeze".
A fixed-size window removes its oldest state and all measurements touching it, without making a marginalization prior. What happens?
Dropping removes those measurements. Marginalization instead summarizes their local information in a prior on the surviving states. Treating the removed estimate as exact can create overconfidence.
From lesson 3. Why does naive IMU integration have to be redone whenever the solver moves \(\mathbf{R}_i\)?
Each sample is rotated into the world by \(\mathbf{R}_k = \mathbf{R}_i\,\Delta\mathbf{R}_{ik}\). That's the same "re-linearise the past" cost this lesson is about, and preintegration is how a window avoids paying it per sample.
In a 1-D chain, \(x_0\) has a prior of information \(p\) and an odometry factor of information \(q\) to \(x_1\). Marginalizing \(x_0\) leaves a prior on \(x_1\) of information…
In series, variances add, so the resulting information is smaller than either. Information adds only for measurements of the same variable, in parallel.
A landmark seen from three poses is marginalized. What happens to the poses' information matrix?
The Schur complement adds \(-\mathbf{H}_{xl}\mathbf{H}_{ll}^{-1}\mathbf{H}_{lx}\), a dense block over every pose that saw the landmark. Pairs that never shared a factor now share one. That density is what makes window marginalization expensive.
A factor adds \(\mathbf{J}_i\mathbf{P}_i\mathbf{J}_i^\top\) to its covariance to account for an uncertain starting state. In the language of this lesson, that is…
For \(x_j = x_i + d\) with prior variance \(p\) and noise \(\sigma^2\), the Schur complement gives a marginal variance of exactly \(p + \sigma^2\): the inflation. glass-lio marginalizes \(\mathbf{x}_i\) correctly for this factor and carries forward selected covariance blocks.
From memory: write the Schur-complement marginalization of \(m\) from \([m;\ k]\), both \(\mathbf{H}'\) and \(\mathbf{b}'\). Then say in one line each what a filter, a sliding window, and a current-state solve do with the past.
- \(\mathbf{H}_{mm}^{-1}\) sandwiched between the coupling blocks
- The same correction applied to \(\mathbf{b}\)
- Filter: marginalizes every step, keeps the current state and \(\mathbf{P}\)
- Window: keeps \(N\) states; marginalizes or drops the oldest when full
- Current-state solve: optimizes the newest state and can account for the previous state's uncertainty in its factors
Missed something? Try this box again after a night's sleep, before rereading anything.