1. 课程导入
当一台移动机器人驶入走廊、一台无人机悬停在风中、一辆自动驾驶车辆穿过隧道时，它们真正“知道”自己在哪吗？传感器给出的从来不是答案，而是一组带噪声、带延迟、带缺失的观测值。轮式里程计会因打滑而漂移，IMU 会因零偏而积分发散，GPS 在室内失效，激光雷达在长廊中退化。机器人状态估计的任务，就是把这些彼此不完美、彼此互补的信息融合起来，实时推断出位姿、速度、姿态偏差等不可直接测量的状态量。而扩展卡尔曼滤波（EKF），正是工程界使用最广、落地最成熟的状态估计方法之一。
一、为什么需要状态估计：从“能测”到“可信”
很多初学者会问：既然有传感器，为什么不直接读传感器值？原因在于三个工程现实。第一，关键状态不可直接测量：机器人的位置、速度、姿态角、传感器零偏，往往没有单一传感器能直接给出。第二，所有测量都有噪声：编码器有量化误差，IMU 有随机游走，视觉有误匹配。第三，系统存在不可观测或弱可观测量：单目相机无法直接恢复绝对尺度，纯 IMU 无法观测绝对航向。
状态估计的核心思想是：用运动模型预测状态如何演化，用观测模型描述传感器如何反映状态，再按不确定性加权融合两者。EKF 通过一阶泰勒展开把非线性系统局部线性化，从而在每一时刻递推地维护一个高斯分布——均值即估计值，协方差即不确定度。
预测：x̂ₖ⁻ = f(x̂ₖ₋₁, uₖ)，Pₖ⁻ = F Pₖ₋₁ Fᵀ + Q
更新：K = Pₖ⁻ Hᵀ (H Pₖ⁻ Hᵀ + R)⁻¹，x̂ₖ = x̂ₖ⁻ + K(z − h(x̂ₖ⁻))
其中 Q 是过程噪声，R 是观测噪声，K 是卡尔曼增益，决定“更信模型还是更信传感器”。
二、本节核心知识图谱
下图给出本章的知识全景：从传感器输入，到预测—更新递推，再到工程输出与典型应用。它既是本课程的整体框架，也是你理解后续每一章位置的“地图”。
三、学习目标：学完本课程你能做什么
本课程不是纯数学推导课，而是面向工程落地的状态估计实战课。学完后，你应当具备以下能力：
- 建模能力：能把一个机器人系统写成状态方程与观测方程，明确状态量、控制量、观测量。
- 推导能力：能独立推导雅可比矩阵 F 与 H，理解线性化误差从何而来。
- 调参能力：能根据传感器手册与实测数据设定 Q、R，并诊断滤波发散、滞后、过自信等问题。
- 实现能力：能用 Python/C++ 写出可运行的 EKF，并接入 ROS 或仿真环境验证。
- 评估能力：能用 RMSE、NEES、残差一致性等指标判断滤波器是否健康。
四、课程整体框架与工程场景
课程按“基础—建模—实现—调优—应用”五段推进，共 30 章。下表给出模块划分与典型工程场景，帮助你建立学习路径感。
| 模块 | 核心内容 | 典型工程场景 |
|---|---|---|
| 基础篇 | 概率基础、高斯分布、贝叶斯滤波、KF 到 EKF | 理解“为什么加权融合” |
| 建模篇 | 运动模型、观测模型、雅可比、坐标系与状态定义 | 差速底盘、阿克曼车辆、四旋翼 |
| 实现篇 | 递推代码、数值稳定性、矩阵运算、ROS 接入 | 轮式机器人里程计融合 |
| 调优篇 | Q/R 整定、一致性检验、发散诊断、故障复盘 | IMU+轮速+激光融合定位 |
| 应用篇 | 多传感器融合、SLAM 前端、组合导航、工程部署 | 自动驾驶、无人机、仓储 AGV |
五、常见误区与复盘提示
误区二：Q 和 R 随便填。Q 过小会导致滤波器“过度自信”，拒绝观测；R 过小会让噪声被当成真实信号，估计抖动。二者必须与真实噪声统计匹配。
误区三：忽略坐标系与时间同步。大量工程故障并非算法错误，而是外参标定错误、时间戳未对齐、单位不一致。
下面是一段最小可运行的 EKF 预测—更新骨架，后续章节会逐步填充模型细节。你可以先感受它的工程结构：
# EKF 单步骨架（伪代码）
def ekf_step(x, P, u, z, dt):
# 1. 预测
x_pred = motion_model(x, u, dt) # 非线性运动模型
F = jacobian_motion(x, u, dt) # 状态雅可比
P_pred = F @ P @ F.T + Q # 协方差预测
# 2. 更新
z_pred = obs_model(x_pred) # 观测预测
H = jacobian_obs(x_pred) # 观测雅可比
S = H @ P_pred @ H.T + R # 新息协方差
K = P_pred @ H.T @ inv(S) # 卡尔曼增益
x_new = x_pred + K @ (z - z_pred) # 状态更新
P_new = (I - K @ H) @ P_pred # 协方差更新
return x_new, P_new
从这一章开始，你将逐步把上面的骨架变成能跑在真实机器人上的估计器。请记住本课程的主线：用模型预测，用观测修正，用协方差表达不确定度。这条主线会贯穿全部 30 章。