Awesome-WAMs
GitHub
PAPER REPORTENAll readings ↗

State Estimation for Robotics

English reading report: Method, equations, original figures, experiments and reproducibility.

Authors: Timothy D. Barfoot

Source: Cambridge University Press (Book, 2nd edition) · ref-b116da337363048621f5 ↗ · Catalog record

Reading: 444 / 558 · 5 original figures & tables · ~20 min ·

1. Paper overview

In one sentence: Gaussian structure makes estimation exact and sparse, while rigid-body pose estimation preserves geometry through local updates whose approximations must remain explicit. identityrevisionlg-setupsmoothingsparse-gpalignment-setuppose-ekf

At a glanceWhat to know
Research problem
Source description

A robot knows inputs and receives noisy observations but does not directly know its state. Estimation combines these with motion and observation models and prior uncertainty. The selected pose applications assume known point geometry or relative-pose measurements. Their target is an estimated trajectory and its uncertainty, rather than choosing or executing actions. lg-setupalignment-setupgraph

Core mechanism
Source description

The pedagogical contribution is a unified derivation: batch MAP and Bayesian conditioning agree in the linear-Gaussian case, and Cholesky/RTS recursions compute that same batch estimate efficiently. gaussian-equivalencesmoothing

A key reported resultSix-point alignment with a negative-determinant cross-moment matrix: C = diag(-1, -1, 1), det(C) = 1; reported minimum J = 4.

Feasible rotation and reported least-squares objective J. Analytical Example 8.1: source points at ±3, ±2 and ±1 along respective coordinate axes, paired to their negatives. No dataset split or repeated-trial evaluation.

The simplified square-root rule produces diag(-1, -1, -1); determinant -1 makes it inadmissible as a rotation. The worked example demonstrates the determinant constraint. It is not an empirical accuracy result or robot-execution test; no statistical uncertainty is reported. alignment-examplerotation-svdrevision

Reading caution
Source description

Gaussian exactness depends on linear models and noise assumptions. Ensemble unbiasedness does not promise zero error in an individual run. Treating noisy sensor readings as deterministic inputs without adjusting uncertainty can invalidate confidence estimates. lg-setupgaussian-equivalenceensemble

Core contributions

  • Source description

    The pedagogical contribution is a unified derivation: batch MAP and Bayesian conditioning agree in the linear-Gaussian case, and Cholesky/RTS recursions compute that same batch estimate efficiently. gaussian-equivalencesmoothing

  • Source description

    The pose chapter connects three alignment parameterizations to inference on rigid transformations, including rotation uniqueness conditions and the distinction between exact linear solves and nonlinear iteration. alignment-setuprotation-svdpose-batch

Figure 3.2. Observability asks whether any state change can leave every constraint unchanged. Original paper, p. 66 ↗

Excerpt from the authors’ paper; cropped without altering the figure or table.

How to read it. Read each cart as a state estimate at one time, rather than a separate robot. The springs labeled J_v represent initial-state or motion penalties; J_y marks measurement penalties. In the top row, the first cart is attached to the wall through the initial-state term. The middle row removes that spring but keeps measurement connections to the wall, so the chain is still anchored. The bottom row retains only connections between successive carts. Imagine translating that entire row together: no spring changes length. This is the visual counterpart of checking whether the normal equations determine a unique solution. batchobservabilitygaussian-equivalence

What it supports. The useful diagnostic is the existence of an unconstrained motion of the whole system. Relative information can fix the spacing between states while leaving their absolute position unknown. Suitable measurements can restore uniqueness even without initial-state knowledge; an optimizer alone cannot create the missing anchor.

Where the evidence stops. This is a one-dimensional conceptual example, not a measured ablation. The general time-invariant test uses an observability matrix; the drawing does not establish observability for an arbitrary higher-dimensional motion or measurement model.

2. Motivation

2.1 The problem and the proposed response

Source description

A robot knows inputs and receives noisy observations but does not directly know its state. Estimation combines these with motion and observation models and prior uncertainty. The selected pose applications assume known point geometry or relative-pose measurements. Their target is an estimated trajectory and its uncertainty, rather than choosing or executing actions. lg-setupalignment-setupgraph

2.2 What this reading follows

How can a robot combine imperfect motion knowledge with measurements without becoming unjustifiably certain? These selected chapters answer by connecting probability, least squares and rigid-body geometry. Chapter 3 turns one Gaussian trajectory problem into batch, smoothing and filtering algorithms, and then relates it to continuous-time Gaussian processes. Chapter 8 asks how the same reasoning changes when the unknown is a rotation or a pose. The five original diagrams below explain the assumptions behind those computations. This is a reading of the corrected first-edition author version compiled December 10, 2022, limited to Chapters 3 and 8; the catalog’s 2024 second edition was not reviewed. identityrevisionlg-setupsmoothingsparse-gpalignment-setuppose-ekf

3. Research context

We place the paper in the collection through its world–action interface. The catalog labels and the reading’s assessment are shown separately.

Catalog dimensionRecorded classification
Major categoryFoundational work
ArchitectureNot applicable
Prediction paradigmNot applicable
QuadrantNot applicable

3.1 Evidence-based assessment

Supports the recorded classification

Reader analysis

The foundational/state-estimation classification fits. Architecture, prediction paradigm and quadrant are not applicable: these chapters derive estimators, accept control inputs and return state or pose beliefs. They provide no learned architecture jointly predicting future observations and actions. This assessment is scoped to Chapters 3 and 8. lg-setupalignment-setuppose-ekfgraph

This is the collection’s architectural analysis, not a new related-work survey. Benchmark comparisons and their protocols appear in Section 6.

4. Problem formulation

4.1 Inputs and outputs

InputsOutputs
  • Initial state/pose and covariance; known motion and observation models; inputs and timestamped measurements with noise covariances.
  • For alignment/tracking, corresponding points in stationary and vehicle frames; for pose graphs, relative transformations with covariances and a fixed reference pose.
  • State estimates and Gaussian uncertainty for the linear problem; pose estimates and local uncertainty for EKF tracking.
  • An estimated rigid transformation for alignment, or graph poses relative to pose 0.

4.2 Equations and their role

J(x)=12(z−Hx)TW−1(z−Hx),(HTW−1H)x^=HTW−1zJ(\mathbf{x})=\tfrac12(\mathbf{z}-\mathbf{H}\mathbf{x})^{\mathsf T}\mathbf{W}^{-1}(\mathbf{z}-\mathbf{H}\mathbf{x}),\qquad (\mathbf{H}^{\mathsf T}\mathbf{W}^{-1}\mathbf{H})\hat{\mathbf{x}}=\mathbf{H}^{\mathsf T}\mathbf{W}^{-1}\mathbf{z}
x stacks states; z stacks the initial mean, inputs and observations; H encodes linear relationships; W stacks covariances. The normal matrix is the posterior precision. Solve the system without explicitly inverting that matrix. batchgaussian-equivalence
xˇk=Ak−1x^k−1+vk,Kk=PˇkCkT(CkPˇkCkT+Rk)−1,x^k=xˇk+Kk(yk−Ckxˇk)\check{\mathbf{x}}_k=\mathbf{A}_{k-1}\hat{\mathbf{x}}_{k-1}+\mathbf{v}_k,\quad \mathbf{K}_k=\check{\mathbf{P}}_k\mathbf{C}_k^{\mathsf T}(\mathbf{C}_k\check{\mathbf{P}}_k\mathbf{C}_k^{\mathsf T}+\mathbf{R}_k)^{-1},\quad \hat{\mathbf{x}}_k=\check{\mathbf{x}}_k+\mathbf{K}_k(\mathbf{y}_k-\mathbf{C}_k\check{\mathbf{x}}_k)
Checks denote predictions and hats corrected estimates. A is the transition matrix, v the input, C the observation matrix, y the measurement, P state covariance and R measurement-noise covariance. K is the gain, distinct from the terminal time index K. The residual is the innovation. lg-setupkalman
W=UDVT,C=Udiag⁡(1,1,det⁡Udet⁡V)VT,r=p−CTy\mathbf{W}=\mathbf{U}\mathbf{D}\mathbf{V}^{\mathsf T},\quad \mathbf{C}=\mathbf{U}\operatorname{diag}(1,1,\det\mathbf{U}\det\mathbf{V})\mathbf{V}^{\mathsf T},\quad \mathbf{r}=\mathbf{p}-\mathbf{C}^{\mathsf T}\mathbf{y}
In this separate alignment notation, W is the centered point cross-moment, not batch noise covariance; U and V are orthogonal SVD factors and D contains ordered singular values. C is rotation, r stationary-frame translation, and p and y without indices are weighted centroids. Uniqueness requires the stated conditions. rotation-svd
ϵk=Kk(yk−yˇk),T^k=exp⁡(ϵk∧)Tˇk\boldsymbol{\epsilon}_k=\mathbf{K}_k(\mathbf{y}_k-\check{\mathbf{y}}_k),\qquad \hat{\mathbf{T}}_k=\exp(\boldsymbol{\epsilon}_k^{\wedge})\check{\mathbf{T}}_k
For the pose EKF, T is a rigid transformation and epsilon its local correction. The wedge/exponential maps that correction into a left-multiplying transformation. The predicted measurement uses the predicted pose. Covariance describes six local coordinates, not sixteen independent matrix entries. pose-inputspose-ekf

5. Method in detail

5.1 1. Recover the same Gaussian estimate in different computational orders

Source description

Start with the trajectory, not the filter recursion. Each motion relation contributes a residual between neighboring states, while each observation contributes a residual at one state. The initial distribution supplies another constraint. Weighting these residuals by inverse covariance gives the batch objective. Its quadratic form is why solving the normal equations returns both the MAP estimate and the mean of the Gaussian posterior. The precision matrix has a narrow block structure because the motion model connects consecutive times. Sparse Cholesky exploits that structure directly; RTS expresses the same computation as forward and backward recursions. The forward part alone is a filter, using only information available at the current time. Thus computational equivalence applies to batch and smoothing with the same information, while filtering deliberately changes the information available for earlier states. lg-setupbatchgaussian-equivalencesmoothing

Figure 3.5. Prediction carries uncertainty forward; correction weights the new measurement innovation. Original paper, p. 83 ↗

Excerpt from the authors’ paper; cropped without altering the figure or table.

How to read it. Start with the leftmost solid density and the old-estimate cart. The rightward motion arrow is labeled with the input and process uncertainty. It produces the dashed prediction, whose mean and covariance carry check accents. Next follow the two hollow arrows toward the new solid density: the prediction and current measurement jointly determine the corrected estimate. Hats identify corrected quantities. In the equations, the measurement enters through the innovation, y_k minus C_k times the predicted state. The Kalman gain translates that discrepancy into a state correction. This scalar drawing places densities on one axis; the matrix equations supply the observation mapping needed in the general case. lg-setupkalmansmoothing

What it supports. The filter repeatedly summarizes past information in a mean and covariance, predicts with the motion model, and incorporates the latest observation. The drawing explains why a measurement is not simply substituted for the current estimate. It contributes according to its uncertainty relative to the prediction.

Where the evidence stops. The caption calls the prediction model the “measurement model,” conflicting with Eq. (3.120b), which uses the motion model. The explanation follows the equation. Increasing prediction width is schematic: general covariance propagation is A times P times A-transpose plus Q, not a universal increase relative to the previous covariance.

5.2 2. Keep the rotation feasible before judging its alignment cost

Source description

The alignment chapter first removes translation through weighted centroids, leaving a rotation problem. It is tempting to treat orthogonality as the only constraint, but a valid rotation also needs determinant positive one. Example 8.1 makes that distinction concrete. Its six points occupy opposite faces along three axes, and each point is paired to its negative. The simplified square-root expression returns a matrix with negative ones on all three diagonal entries: its determinant is negative one, so it fails the rotation constraint. The SVD solution inserts the determinant correction and returns diagonal entries negative one, negative one and positive one. This rotates by pi about the third axis, makes four residuals zero and yields the reported minimum cost J = 4. The example establishes an algebraic failure case, without claiming sensor-data performance. rotation-svdalignment-examplerevision

Figure 8.2. Point correspondences become compact statistics before the rigid alignment is solved. Original paper, p. 317 ↗

Excerpt from the authors’ paper; cropped without altering the figure or table.

How to read it. Follow the arrows from left to right. The starting pairs y_j and p_j already contain correspondences between the two clouds. The next block collects total weight w, weighted centroids y and p, and the quaternion moment matrix W. Those statistics determine translation r and rotation quaternion q. The final cloud illustrates applying the resulting pose change to bring paired points together. Read W in this figure using the quaternion derivation in Eqs. (8.11)–(8.12). The later rotation-matrix derivation reuses the letter for a different, three-dimensional cross-moment matrix. The diagram describes the alignment calculation after pairing has been established. alignment-setupquaternion-flowrotation-svdalignment-example

What it supports. The computation separates a geometric problem into centering and constrained rotation. It explains how many point pairs can contribute through a compact set of moments. The book supplies quaternion, rotation-matrix and iterative transformation-matrix approaches, with scalar weights central to the non-iterative solutions.

Where the evidence stops. The schematic’s overlapping endpoints are illustrative, not an accuracy measurement. Noisy or incompatible pairs need not align exactly, and degenerate statistics can yield nonunique rotations. Neither the figure nor the closed-form derivation solves unrestricted correspondence selection.

5.3 3. Separate a valid pose update from an exact inference guarantee

Reader analysis

Reader interpretation: geometric validity and statistical exactness answer different questions. The pose EKF stores a transformation in SE(3), represents uncertainty in six local coordinates and applies its correction by left multiplication through the exponential map. Batch tracking similarly linearizes a trajectory, solves for increments and applies them on the group. This preserves a valid pose representation, but it does not make the original nonlinear objective quadratic. The source’s simplified BCH expansion makes that approximation explicit. Pose-graph relaxation adds another distinction: initialization from a spanning tree is followed by optimization using the graph’s measurements. Reducing the graph to junctions and reconstructing chains can organize computation, yet the book labels that nonlinear shortcut approximate. A reproduction should therefore examine convergence and residuals separately from checking whether every stored transformation remains geometrically valid. pose-inputspose-ekfpose-batchgraphgraph-initializationgraph-sparsity

Figure 8.3. One fixed pose anchors a network of uncertain relative-pose constraints. Original paper, p. 346 ↗

Excerpt from the authors’ paper; cropped without altering the figure or table.

How to read it. Begin at the black triangle labeled T_0, fixed. Each remaining triangle represents another reference frame whose pose must be estimated relative to that anchor. The labeled pair of poses T_l and T_k has a relative transformation measurement and covariance. Follow the upper loop: uncertainty means the measured transformations around it need not compose to identity. The leaf on the right has a different connectivity role, since it terminates a branch. The relaxation problem uses all available relative measurements to find mutually consistent pose variables. Its logarithmic residual compares a measured relative transform with the relative transform implied by those variables. graph

What it supports. Fixing pose 0 specifies the coordinate reference, while covariance weighting determines how conflicting measurements influence the estimate. This is an inference problem over a pose graph. It explains why simply compounding one sequence of noisy transforms cannot reconcile all routes around a closed loop.

Where the evidence stops. The sketch supplies graph structure, not metric ground truth or a navigation result. Optimized relative transforms are consistent with estimated poses, but individual measurement residuals can remain nonzero. The source explicitly connects those residuals to the usefulness of retaining the SE(3) Jacobian.

5.4 Training and inference

During training

Reader analysis

These chapters specify model-based inference, not network training. Models, covariances, correspondences and initialization are supplied to estimators; no learned backbone, training dataset or training schedule is proposed here. lg-setupalignment-setuppose-inputs

During inference

Reader analysis

Filtering uses available data causally; fixed-interval smoothing uses later measurements to revise earlier states. Optimization iterations change an estimate, not the physical robot. Controller output appears as a known model input. smoothinglg-setup

Source description

A spanning tree initializes graph poses, preferably a shallow one. Measurements outside that tree are excluded only during initialization. Gauss-Newton then uses graph constraints and updates poses until a convergence criterion is met. graph-initializationgraph

5.5 Implementation flow

  1. Build the estimation objective

    Combine initial-state, adjacent-state motion and measurement penalties. Inverse-covariance weighting records confidence. Stacking the residuals produces a quadratic system whose solution is the Gaussian posterior mean and mode. lg-setupbatchgaussian-equivalence

  2. Check what observations constrain

    Invertibility matters before optimization. Figure 3.2 shows why relative motion alone cannot anchor a one-dimensional chain. Suitable absolute measurements can remove that ambiguity even without an initial-state anchor. observabilitygaussian-equivalence

  3. Exploit time structure

    Chronological ordering gives a block-tridiagonal normal matrix. Cholesky and RTS smoothing use forward/backward passes. Filtering stops at the current information boundary: predict with the motion input, then correct using the new measurement innovation. smoothingkalman

  4. Extend the prior to continuous time

    For the specified linear stochastic differential-equation class, the GP inverse kernel is exactly sparse. Matching prior terms give discrete estimates equal to the continuous-time construction at measurement times; intermediate queries combine neighboring states. The stated O(K+J) scaling holds at fixed state dimension, with J query times. sparse-gp

  5. Align corresponding geometry

    Use scalar-weighted centroids to eliminate translation, then solve for constrained rotation. The quaternion route is an eigenproblem; the rotation-matrix route uses SVD with a determinant correction. An iterative transformation-matrix alternative is also developed. Correspondences are inputs to these alignment solvers. alignment-setupquaternion-flowrotation-svd

  6. Track and reconcile poses

    Tracking combines velocity inputs and point observations. EKF corrections and batch increments operate locally, then update the pose by left multiplication on SE(3). Pose graphs replace point observations with relative-pose constraints, anchor pose 0 and reconcile inconsistent loops iteratively. pose-inputspose-ekfpose-batchgraph

6. Experiments & results

This selected-chapter reading follows how noisy measurements become state and pose estimates with uncertainty. Chapter 3 connects Gaussian least squares, exact smoothing, causal filtering and a special sparse continuous-time GP construction. Chapter 8 extends estimation to rigid-body geometry through alignment, tracking and pose-graph relaxation. The common lesson is to respect information structure and geometric constraints. These are foundational estimation tools, not a learned action-generating system. The reviewed source is the corrected first edition compiled in 2022, not the cataloged 2024 second edition.

Source and visual limitations
Reader analysis

The supplied chapters are mathematical exposition with worked examples and conceptual diagrams. They provide no original quantitative benchmark table, learned-model architecture, or measured experimental ablation. The edition therefore uses five original method/diagnostic diagrams; Figure 8.5 occupies the ablation section as an explicitly conceptual sparsity diagnostic. Example 8.1 is preserved as an analytical result, not converted into an invented table or benchmark. No empirical featured result is selected. These limitations describe Chapters 3 and 8 of this corrected first-edition source, not the unreviewed remainder or the cataloged second edition. alignment-setupalignment-examplesparse-gpgraph-sparsity

6.1 Read the original evidence

No results visual is included in this edition. The source-specific visual limitations are recorded above.

6.2 Results and evaluation conditions

Task & protocolReported resultComparison & interpretation
Six-point alignment with a negative-determinant cross-moment matrix

Analytical Example 8.1: source points at ±3, ±2 and ±1 along respective coordinate axes, paired to their negatives. No dataset split or repeated-trial evaluation.

C = diag(-1, -1, 1), det(C) = 1; reported minimum J = 4.

Feasible rotation and reported least-squares objective J

The simplified square-root rule produces diag(-1, -1, -1); determinant -1 makes it inadmissible as a rotation.

The worked example demonstrates the determinant constraint. It is not an empirical accuracy result or robot-execution test; no statistical uncertainty is reported. alignment-examplerotation-svdrevision

Exact solution of a linear-Gaussian trajectory

Chapter 3 chain-structured problem with N state coordinates and K+1 states.

O(N^3(K+1)) for the stated sparse solution; Cholesky and RTS smoothing implement the batch estimate without approximation.

Analytical computational scaling and solution equivalence

The corrected statement retains cubic dependence on N; linear growth refers to trajectory length at fixed N.

This is structural complexity, not a measured speedup on specified hardware. It does not establish the same cost for arbitrary pose graphs. smoothingrevision

6.3 Ablations and diagnostic examples

Read component removals and qualitative examples within their stated evaluation conditions.

Figure 8.5. A conceptual diagnostic: solve junctions first, then recover local chains. Original paper, p. 350 ↗

Excerpt from the authors’ paper; cropped without altering the figure or table.

How to read it. Compare this graph with Figure 8.3. In the source’s terminology, a “closed triangle” is a three-sided junction symbol, while an “open triangle” is a chevron-shaped symbol on a local chain; closed does not mean black-filled. Dotted segments pass through nodes with only one or two edges. A constrained chain connects two junctions, so its combined measurement contributes to the reduced graph. A cantilevered chain attaches at one end. After solving junction poses, the procedure fixes them and recovers the remaining nodes. Constrained chains need their own relaxation; cantilevered chains can be recovered by compounding outward. graphgraph-sparsitysmoothing

What it supports. The algorithm uses connectivity to organize computation. For a sequentially ordered constrained chain, each linearized solve has a block-tridiagonal matrix and cost linear in chain length. This motivates a two-phase calculation, while the text also permits a sparse solver on the complete graph without explicit chain bookkeeping.

Where the evidence stops. This is a structural diagnostic, not an empirical ablation or measured speedup. Footnote 12 explicitly calls the nonlinear two-phase reduction an approximation. It should not inherit the exact-equivalence guarantee of Chapter 3’s Gaussian smoother.

7. Analysis & limitations

7.1 What the evidence leaves open

Source description

Gaussian exactness depends on linear models and noise assumptions. Ensemble unbiasedness does not promise zero error in an individual run. Treating noisy sensor readings as deterministic inputs without adjusting uncertainty can invalidate confidence estimates. lg-setupgaussian-equivalenceensemble

Source description

Alignment uniqueness requires positive determinant, negative determinant with a distinct smallest singular value, or rank two of the cross-moment matrix. Other cases admit infinitely many minimizing rotations. Scalar-weight results do not establish the same closed form for anisotropic matrix weights. rotation-svdalignment-setup

Source description

Pose inference uses local linearization. The batch derivation simplifies BCH, and graph residuals can remain nonzero at convergence. Two-phase graph reduction is an approximation for a nonlinear system, unlike exact Gaussian smoothing. pose-batchgraphgraph-sparsity

7.2 Questions for discussion

  1. Which uncertainty and observability assumptions would survive when an estimated state is supplied to a learned controller?
  2. When does exact elimination in a Gaussian chain cease to justify an analogous nonlinear pose-graph shortcut?

8. Reproducibility audit

8.1 Requirements and known gaps

Source description

Implementation needs models, timestamped inputs/measurements, initial uncertainty, and noise covariances. Pose work additionally needs consistent frame conventions, correspondences and exponential-map operations. The graph algorithm leaves the numerical convergence threshold unspecified in this selected text. lg-setupalignment-setuppose-inputsgraph

Reader analysis

Reader-proposed checks: compare batch, sparse Cholesky and RTS means on identical synthetic Gaussian data; separately reproduce Example 8.1’s determinant failure and corrected rotation. These test algebra and geometry without claiming a complete robot-system reproduction. batchsmoothingalignment-example

8.2 Proposed reproduction checks

The following checks are proposals motivated by the paper. They have not been run as part of this reading.

Check 1: Gaussian equivalence with a causal control

Reader-proposed, not run: generate a scalar instance of Eq. (3.1) with A = C = 1, K = 50, v_k = 0, initial mean 0, initial variance 1, Q = 0.1 and R = 0.4. Record the seed and reuse exactly the same observations. Compare batch normal-equation, sparse Cholesky and RTS trajectory means, with a proposed maximum absolute difference tolerance of 1e-9 in double precision. Apply the time-zero measurement correction consistently. As a control, compare each Kalman-filter estimate only with the terminal state of the corresponding batch prefix. Agreement of full smoothers and prefix estimates is the predicted outcome; requiring a filter to match every full-interval smoothed state would test the wrong information set. lg-setupbatchsmoothingkalman

Check 2: Reproduce the determinant counterexample and a valid-rotation control

Reader-proposed, not run: use the six point pairs in Example 8.1 and evaluate Eq. (8.21) with unit scalar weights. Compare the simplified square-root rule with the determinant-corrected SVD rule. Check orthogonality, determinant, per-point residuals and total objective separately. The source predicts determinant -1 for the simplified result and determinant +1 with J = 4 for the corrected rotation. Then keep the same source points but generate target points with a known proper rotation as a control. Both methods should recover that valid transform in this full-rank, noiseless control. Predeclare a numerical tolerance, such as 1e-10; failure on either case would expose a convention, transpose or determinant-handling error. alignment-setuprotation-svdalignment-example

8.3 Reading coverage

Visual audit: The title and revision pages, all pages cited for retained equations, numerical examples, assumptions, inference procedures and proposed checks, and the five final crops were visually inspected. Additional uncropped visuals inspected were Figures 3.1, 8.1 and 8.4. The figure arrows, frame conventions and local-chain symbols were cross-checked with surrounding equations and captions. Figure 3.5's caption/model discrepancy is disclosed in its caution. Numerical reading concerns analytical Example 8.1 and complexity statements; these selected chapters provide no benchmark table or measured ablation. All 21 supplied text chunks were read, but the visual pass and the text input remain selected-chapter coverage. Other chapters, Appendix A and the second edition were not reviewed.

PDF pages inspected for this edition: 1, 3, 4, 53, 54, 57, 58, 59, 62, 65, 66, 67, 68, 74, 83, 88, 93, 96, 98, 99, 103, 104, 313, 314, 315, 316, 317, 318, 319, 321, 322, 326, 327, 335, 337, 338, 340, 342, 343, 344, 345, 346, 347, 348, 349, 350. Appendix coverage: not read.

Original figures and tables remain the work of the source’s authors. Extractions preserve their scientific content; any HTML wrapper layout is disclosed with each figure. The surrounding reading notes are our own.

Text reading scope & known omissions
  • Title/author/version declaration (PDF p. 1) and Revision History (PDF pp. 3–4).
  • Chapter 3, Sections 3.1–3.3: batch estimation, observability, Cholesky/RTS smoothing, Kalman filtering, error dynamics and stability discussion (PDF pp. 53–89).
  • Chapter 3, Sections 3.4–3.6: continuous-time GP estimation, sparse priors, LTI examples, summary and every exercise (PDF pp. 90–106).
  • Chapter 8, Section 8.1: quaternion, rotation-matrix and transformation-matrix alignment, uniqueness, local critical points and Example 8.1 (PDF pp. 313–335).
  • Chapter 8, Section 8.2: tracking models, EKF and batch MAP (PDF pp. 335–345).
  • Chapter 8, Sections 8.3.1–8.3.5: pose-graph relaxation, initialization, sparsity and chain example (PDF pp. 345–352).

Outside the original text pass

  • This is the author-hosted corrected first-edition version compiled December 10, 2022, not the unchanged Cambridge 2017 print edition.
  • The revision history identifies corrections through December 2022 and Appendix A material added after the first edition; do not attribute those additions to the original 2017 print edition.
  • Text extraction may omit rotated annotations and does not replace visual inspection of equations, figures or table layout.
  • Only title/revision pages and Chapters 3 and 8 are supplied as reading text: PDF pages 1, 3-4, 53-106 and 313-352. All other pages (2, 5-52, 107-312 and 353-399) are omitted from this reading input.
  • Chapters 1-2, 4-7 and 9-10, other front matter, references, index and Appendix A are outside this selected reading. The complete raw PDF and full extraction are retained separately, but their availability does not establish that those chapters or appendices were read.
  • Chapter 8 uses three-dimensional and Lie-group machinery developed in omitted Chapters 6-7; describe that dependency rather than claiming those chapters were reviewed.
  • Identity/edition difference: the catalog records a 2024 Cambridge second edition. Title and author agree with the supplied corrected first-edition artifact, but this report does not review or establish equivalence to the second edition.
  • All 21 supplied text chunks were read individually. This completes the selected input, not the full book. Appendix A was not read.
  • Visual inspection covered the pages declared in the illustrated edition. Figures 3.3, 3.4, 3.6 and 8.6 were read through their supplied captions/text but were not visually inspected.
  • No code or external resource was inspected and no estimation experiment was run. Affiliation was not stated on the inspected title page.

The visual audit above records the subsequent illustrated pass.

8.4 Traceable evidence

identityPDF p. 1, title, author and compilation declarationInspect

The title is State Estimation for Robotics, credited to Timothy D. Barfoot. This unofficial version was compiled December 10, 2022; Cambridge University Press is the official publisher. No affiliation is printed on the title page.

Go to primary source ↓
revisionPDF pp. 3–4, Revision HistoryInspect

The history starts with the May 13, 2017 version best matching the published first edition and ends with first-edition corrections on December 10, 2022. It records later appendix additions, Example 8.1's corrected J = 4, and Section 3.2's corrected O(N^3(K+1)) complexity.

Go to primary source ↓
lg-setupChapter 3, PDF pp. 53–54 (printed pp. 37–38), Section 3.1.1, Eq. (3.1) and footnotesInspect

The linear state transition uses deterministic input and Gaussian process noise; measurements are linear with Gaussian noise. Initial state knowledge and noises are mutually uncorrelated, including across times. Estimation returns state and uncertainty. Controller outputs can provide inputs; using interoceptive measurements instead requires accounting for added uncertainty.

Go to primary source ↓
batchPDF pp. 57–59 (printed pp. 41–43), Eqs. (3.9)–(3.16), Figure 3.1Inspect

Initial-state, motion and measurement Mahalanobis penalties form a quadratic objective. Lifted data z, state x, model H and block-diagonal covariance W yield normal equations. The text recommends a sparse solve rather than explicit inversion. Figure 3.1 compares prior-only and measurement-corrected spring systems.

Go to primary source ↓
gaussian-equivalencePDF p. 62 (printed p. 46), Eqs. (3.32)–(3.36); p. 66 (printed p. 50), Eqs. (3.49)–(3.51)Inspect

Bayesian and MAP estimates agree because the Gaussian posterior's mode equals its mean. A unique batch solution requires an invertible normal matrix; posterior covariance is its inverse.

Go to primary source ↓
observabilityPDF pp. 65–66 (printed pp. 49–50), Eqs. (3.46)–(3.48), Figure 3.2 and interpretationInspect

Time-invariant observability uses the rank of a stacked observation/transition matrix. In the one-dimensional spring analogy, an initial anchor or suitable measurements prevents cost-preserving translation. Without either, the connected chain can translate without changing spring energy.

Go to primary source ↓
smoothingPDF pp. 67–68 (printed pp. 51–52), Section 3.2.1, Eqs. (3.55)–(3.59); p. 74 (printed p. 58), Eqs. (3.82)–(3.83) and Section 3.3 introductionInspect

Block-tridiagonal normal equations admit sparse Cholesky and forward/backward solution in O(N^3(K+1)) time. Cholesky and RTS smoothing implement the batch solution without approximation. The RTS forward pass is the Kalman filter. Smoothing uses future data; filtering uses only data available by the current time.

Go to primary source ↓
kalmanPDF p. 83 (printed p. 67), Figure 3.5, caption and Eqs. (3.120a)–(3.120e)Inspect

Prediction propagates mean with A and v and covariance with A and Q. The gain weights the innovation y minus C times predicted state. Figure 3.5 illustrates this, but its caption incorrectly calls the prediction model the measurement model; Eq. (3.120b) uses the motion model.

Go to primary source ↓
ensemblePDF p. 88 (printed p. 72), Eq. (3.141) and final paragraph of Section 3.3.6Inspect

Under the model assumptions, the reported covariance equals error covariance. Unbiasedness concerns an ensemble of trials, not time averages or a promise that a single trajectory has zero or vanishing error.

Go to primary source ↓
sparse-gpPDF p. 93 (printed p. 77), Eqs. (3.158)–(3.159); p. 96 (printed p. 80), Eqs. (3.179)–(3.185); pp. 98–99 (printed pp. 82–83), Eqs. (3.197)–(3.198); pp. 103–104 (printed pp. 87–88), Section 3.4.4 and summaryInspect

A specified linear time-varying stochastic differential-equation class defines GP priors with block-tridiagonal inverse kernels. Interpolation depends on adjacent trajectory states. Appropriate prior terms make discrete-time estimation exactly match the continuous-time construction at measurement times. Solve-plus-query scaling is stated as O(K+J), with state dimension held fixed.

Go to primary source ↓
alignment-setupPDF pp. 313–314 (printed pp. 297–298), Chapter 8 introduction, Section 8.1 and Figure 8.1Inspect

The chapter treats localization with known world geometry. Point alignment assumes correspondences between stationary-frame and vehicle-frame points and equal scale. Non-iterative quaternion and rotation-matrix solutions require scalar weights; an iterative transformation-matrix solution is also developed.

Go to primary source ↓
quaternion-flowPDF pp. 315–317 (printed pp. 299–301), Eqs. (8.1)–(8.16), Figure 8.2Inspect

Homogeneous corresponding points yield weighted centroids, total weight and a quaternion moment matrix. Translation follows aligned centroids; rotation uses a minimizing eigenvector subject to unit norm. Figure 8.2 flows from correspondences through statistics and pose to aligned clouds. Degenerate eigenproblems can have nonunique solutions.

Go to primary source ↓
rotation-svdPDF pp. 318–319 (printed pp. 302–303), Eqs. (8.18)–(8.27); pp. 321–322 (printed pp. 305–306), Eq. (8.37) and singular-value definition; p. 326 (printed p. 310), Eqs. (8.60)–(8.63) and uniqueness summaryInspect

For rotation matrices, W is the normalized centered point cross-moment, different from quaternion W. With W = U D V^T, a unique global solution, when it exists, is C = U diag(1,1,det U det V) V^T. Uniqueness holds for positive determinant, negative determinant with a distinct smallest singular value, or rank two. Translation is p minus C^T y.

Go to primary source ↓
alignment-examplePDF pp. 326–327 (printed pp. 310–311), Example 8.1, Eqs. (8.64)–(8.67)Inspect

Six source points lie at plus/minus three, two and one on the respective coordinate axes; targets are their negatives. W = diag(-18,-8,-2)/6. The simplified solution has determinant -1. The corrected rotation diag(-1,-1,1) rotates by pi about the third axis, makes four point residuals zero, and has reported minimum J = 4.

Go to primary source ↓
pose-inputsPDF p. 335 (printed p. 319), Section 8.2.1; pp. 337–338 (printed pp. 321–322), Eqs. (8.119)–(8.126) and NomenclatureInspect

Tracking combines a pose prior, generalized velocity inputs and point measurements. Piecewise-constant motion propagates nominal pose and local perturbations separately. Pose means are 4 by 4 matrices; state covariances are 6 by 6 and individual measurement-noise covariances 3 by 3. The measurement relation is linearized to first order.

Go to primary source ↓
pose-ekfPDF p. 340 (printed p. 324), Eqs. (8.136)–(8.140) and summaryInspect

The gain maps innovation to a local correction, applied by left multiplication through the exponential map. Means remain in SE(3), while covariance represents local Lie-algebra perturbations. This is an EKF inference construction.

Go to primary source ↓
pose-batchPDF pp. 342–345 (printed pp. 326–329), Eqs. (8.150)–(8.166), including p. 343 footnote 11Inspect

Batch tracking sums covariance-weighted input and measurement residuals, linearizes about a trajectory guess, solves block-tridiagonal normal equations for increments, applies exponential updates and iterates. Input-error linearization uses a crude BCH approximation; retaining the SE(3) Jacobian is suggested as an improvement.

Go to primary source ↓
graphPDF pp. 345–348 (printed pp. 329–332), Sections 8.3.1–8.3.2, Figure 8.3, Eqs. (8.167)–(8.187)Inspect

Pose graphs use uncertain relative measurements with pose 0 fixed. Measurements need not compound to identity around loops. Covariance-weighted logarithmic residuals are minimized with iterative Lie-algebra updates. Residuals generally remain nonzero at convergence; retaining the SE(3) Jacobian can help. No numerical stopping threshold is supplied here.

Go to primary source ↓
graph-initializationPDF p. 349 (printed p. 333), Figure 8.4 and Section 8.3.3Inspect

A spanning tree initializes poses by compounding measurements outward from pose 0. Dotted measurements are discarded only for initialization. A shallow tree is preferred to reduce accumulated uncertainty.

Go to primary source ↓
graph-sparsityPDF pp. 349–350 (printed pp. 333–334), Section 8.3.4, footnote 12 and Figure 8.5Inspect

Constrained chains connect two junctions; cantilevered chains attach at one end. Solve a reduced junction graph, then recover local chains. Sequential constrained-chain ordering gives block-tridiagonal systems and linear cost per iteration in chain length. Footnote 12 labels this nonlinear reduction an approximation; solving the full sparse system is an alternative.

Go to primary source ↓

8.5 Primary sources

Scroll across the image to inspect details. Press Esc to close.