第一章 状态估计导论
Chapter 1 Introduction to State Estimation
1.1 什么是状态估计？ / What Is State Estimation?
中文
机器人总是在运动的世界里工作。无论是火星探测车、低空无人机，还是即将普及的自动驾驶汽车，它们都面临一个共同的核心问题：我在哪里？我的速度是多少？我的姿态是什么？ 这一系列描述机器人运动状态的量，统称为状态（state）。
如果状态已知，控制问题就变得相对简单——告诉机器人”向前走 1 米”之前，你必须先知道它现在在哪里。然而在真实世界里，传感器永远带有噪声，没有任何测量是精确的。状态估计（state estimation） 就是在传感器噪声下，利用一系列测量和关于系统动力学的先验知识，尽可能准确地重建系统状态的学问。
核心定义
状态估计是在给定一组测量序列以及系统的先验模型的条件下，重建系统底层状态的问题。
Estimation is the problem of reconstructing the underlying state of a system given a sequence of measurements as well as a prior model of the system.
English
Robots inherently deal with things that move in the world. We live in an era of rovers on Mars, drones surveying the Earth, and self-driving cars. Although specific robots have their subtleties, all applications share common challenges, particularly state estimation.
The state of a robot is the minimal set of quantities—position, orientation, velocity—that fully describes its motion at any instant. Knowing the state is a prerequisite for control: you cannot command a robot to go somewhere without first knowing where it is. Yet in the real world every sensor is imperfect, so states can never be measured directly and exactly. State estimation is the discipline of doing the best we can with the noisy sensors we have.
1.2 简史：从航海到卡尔曼滤波 / A Brief History: From Navigation to the Kalman Filter
中文
状态估计的需求远早于机器人时代。
4000 年前，早期航海者就面临着一个经典的状态估计问题：如何在茫茫大海上确定船的位置？他们依靠天体观测（恒星仰角→纬度）和简陋的罗盘来进行推算。纬度的测量相对直接，但经度问题困扰了人类数个世纪——直到 1764 年，英国钟表匠约翰·哈里森造出了第一块能在海上保持精确走时的便携时钟，经度才得以精确测量（误差约 10 海里）。
直觉：已知你出发的时间（格林威治时间）和当地正午时间，两者之差乘以地球自转速度，就得到经度。时钟精度→经度精度，这其实是一个状态估计问题：用时间测量来估计空间位置。
估计理论的数学根源来自天文学。1801 年，23 岁的高斯（Carl Friedrich Gauss）用最小二乘法，根据谷神星消失前的少量观测，准确预测了它在太阳背面消失 9 个月后重新出现的位置（误差不到半度）。这是人类历史上第一次用数学方法”从噪声数据中估计轨道”。1809 年，高斯证明：当测量误差服从正态分布时，最小二乘是最优估计器。
二十世纪的飞跃：计算机的出现把估计理论带进了现代。1960 年，卡尔曼（Rudolf Kálmán）发表了两篇奠基性论文：
- 引入**可观测性（observability）**概念——什么时候状态可以被测量序列唯一确定？
- 提出卡尔曼滤波器（Kalman filter）——线性高斯系统的最优递归估计算法。
卡尔曼滤波很快被 NASA 采用于 Apollo 登月任务：阿波罗 11 号登月舱上的计算机，就是用卡尔曼滤波融合惯性传感器和雷达信号，估计飞船在月面上方的精确位置。
English
State estimation has roots far older than robotics.
Early navigation. Four thousand years ago seafarers faced a classic estimation problem: determining a ship’s position at sea. Latitude was obtainable from celestial observations (angle of Polaris above horizon), but longitude remained elusive for centuries. The missing ingredient was an accurate portable timepiece. In 1764, John Harrison’s chronometer solved the problem: compare local noon time to the known Greenwich time, multiply the difference by Earth’s rotation rate, and you have longitude. Precision of the clock → precision of the longitude estimate.
Least squares and Gauss. Estimation theory’s mathematical roots lie in astronomy. In 1801, the 23-year-old Carl Friedrich Gauss applied his method of least squares to predict the re-emergence of the dwarf planet Ceres after it passed behind the Sun—accurate to within half a degree, nine months after last observation. In 1809 he proved least squares is optimal when measurement errors are Gaussian.
The Kalman revolution. The advent of digital computers catalysed a leap forward. In 1960 Rudolf Kálmán published two landmark papers: one introducing the concept of observability (when can a state be uniquely inferred from measurements?), and one introducing the Kalman filter—the optimal recursive estimator for linear systems with Gaussian noise. NASA adopted it almost immediately for the Apollo program; the onboard computer of Apollo 11’s Lunar Module used a Kalman filter to fuse noisy inertial and radar data into a reliable position estimate.
Historical milestones at a glance:
| 年份 / Year | 事件 / Event |
|---|---|
| 1654 | Pascal 与 Fermat 奠定概率论基础 / Pascal & Fermat lay foundations of probability |
| 1764 | Bayes 定理 / Bayes’ rule |
| 1801 | 高斯用最小二乘估计谷神星轨道 / Gauss estimates Ceres orbit with least squares |
| 1960 | 卡尔曼滤波器 / Kalman (Bucy) filter |
| 1965 | Rauch–Tung–Striebel 平滑器 / RTS smoother |
| 1970 | Jazwinski 提出”贝叶斯滤波器”术语 / Jazwinski coins “Bayes filter” |
1.3 传感器与测量 / Sensors and Measurements
中文
理解状态估计，首先要理解传感器的本质：所有传感器都有有限精度，所有测量都含有不确定性。 状态估计就是把这些不确定的测量尽可能聪明地组合起来。
传感器通常分为两类：
| 类型 | 定义 | 典型例子 |
|---|---|---|
| 本体感知型（interoceptive） | 感受来自机器人内部的信号 | 加速度计、陀螺仪、轮速计 |
| 外感知型（exteroceptive） | 感受来自外部环境的信号 | 摄像头、激光雷达、GPS |
粗略地说：
- 外感知传感器 → 测量位置/姿态（告诉你”在哪里”）
- 本体感知传感器 → 测量速度/加速度（告诉你”移动有多快”）
直觉：只有外感知传感器会累积误差吗？不对！本体感知传感器（如 IMU）通过积分速度得到位置，误差会随时间累积（积分漂移）。外感知传感器（如 GPS）则能给出绝对位置，但受环境遮挡影响。二者融合才是鲁棒估计的关键。
最优状态估计的思路就是：在考虑每个传感器的不确定性前提下，融合所有可用测量，得到对状态的最佳猜测，并量化该猜测的不确定性（协方差）。
English
All sensors have limited precision; all measurements carry uncertainty. State estimation is the art of combining uncertain measurements intelligently.
Sensors are conveniently classified into two categories:
- Interoceptive (proprioceptive): measure quantities internal to the robot — accelerometers (translational acceleration), gyroscopes (angular rate), wheel odometers.
- Exteroceptive: measure quantities in the environment — cameras (bearing/range to landmarks), lidar, GPS.
Roughly: exteroceptive sensors yield position/orientation information; interoceptive sensors yield velocity/acceleration. Neither alone is sufficient for robust estimation:
- Interoceptive sensors alone suffer from integration drift — small velocity errors accumulate over time into large position errors.
- Exteroceptive sensors alone are susceptible to occlusion, outliers, and data-association failures.
Fusing both types—carefully accounting for each sensor’s uncertainty—is the cornerstone of practical state estimation.
1.4 本书结构 / How This Book Is Organized
中文
本书分为三个主要部分和附录，按逻辑递进顺序组织：
第一部分：估计机器（Estimation Machinery，第 2–6 章）
在不涉及三维旋转的情况下，系统地介绍估计理论。状态被抽象为普通向量，这使我们可以专注于估计方法本身。内容涵盖：
- 第 2 章：概率论基础（高斯分布、贝叶斯推断）
- 第 3 章：线性高斯估计（批量 MAP、卡尔曼滤波器、平滑器、连续时间估计）
- 第 4 章：非线性非高斯估计（EKF、粒子滤波器、批量非线性 MAP）
- 第 5 章：实际问题处理（估计器性能评估、偏差、离群值、协方差估计）
- 第 6 章：变分推断（Variational Inference）
第二部分：三维机器（Three-Dimensional Machinery，第 7–8 章）
三维空间中的旋转不是普通向量——将机器人朝某方向旋转 90°再往另一方向旋转 90°，结果取决于顺序（旋转不可交换）。因此第一部分的方法不能直接应用于旋转状态。本部分介绍：
- 第 7 章：三维几何基础（旋转矩阵、四元数、位姿变换、传感器模型）
- 第 8 章：矩阵李群（SO(3)、SE(3)、李代数、扰动微积分）
第三部分：应用（Applications，第 9–11 章）
将前两部分融合，求解经典三维估计问题：
- 第 9 章：位姿估计问题（点云对齐、位姿图优化、惯性导航）
- 第 10 章：位姿-点联合估计（束调整、SLAM）
- 第 11 章：连续时间估计
附录：矩阵代数与微积分速查。
English
The book is organized into three main parts.
Part I — Estimation Machinery (Chapters 2–6): Estimation theory without the complication of three-dimensional rotations. The state is treated as a generic vector. Covers both recursive (filter/smoother) and batch methods, including Gaussian process trajectory estimation and variational inference.
Part II — Three-Dimensional Machinery (Chapters 7–8): Three-dimensional rotations are not vectors in the ordinary sense (rotation is non-commutative: the order matters). Part II develops the geometry, kinematics, and probability/statistics of rotations and poses using matrix Lie groups SO(3) and SE(3).
Part III — Applications (Chapters 9–11): Combines Parts I and II to solve classic robotics problems: point-cloud alignment, pose-graph relaxation, bundle adjustment, SLAM, inertial navigation, and continuous-time trajectory estimation.
1.5 阅读建议 / Reading Notes
中文
- 如果你只关心估计算法本身，可以只读第一部分（第 2–6 章），它是自成一体的。
- 如果你对三维机器人应用感兴趣，第二部分（第 7–8 章）是前提，需要先读第一部分。
- 第三部分（第 9–11 章）将前两部分融合为具体工程方法，是本书最有实践价值的部分。
- 附录 A（矩阵基础）可作为参考随时查阅。
全书采用贝叶斯观点：状态不是固定的未知量，而是随机变量，我们用概率分布来描述对它的知识。随着测量的加入，分布不断更新。这一视角将贯穿始终。
English
- Part I is self-contained and can be read independently for estimation methods without 3D geometry.
- Parts II and III require Part I as a prerequisite.
- Appendix A (matrix primer) can be consulted as a reference at any point.
Throughout the book we adopt a Bayesian viewpoint: the state is treated as a random variable and our knowledge about it is represented as a probability distribution. As measurements arrive, the distribution is updated. This perspective unifies all the methods presented.
下一章将建立本书所需的全部概率论工具。/ The next chapter builds all the probability-theoretic tools required for the rest of the book.