引言
单个传感器永远不够。IMU 高频但有累积漂移，轮式里程计在打滑时完全失效，激光雷达精度高但更新慢且易受遮挡，视觉提供丰富信息但受光照与纹理影响。状态估计的任务就是把这些各有缺陷的信号融成一个连续、平滑、有置信度的位姿估计，并把不确定性显式地表达为协方差。
工程上的难点首先是姿态表示的流形性。旋转属于 SO(3)，不是向量空间，不能直接做线性加减。把旋转矩阵或四元数直接放进卡尔曼滤波的状态向量会导致协方差失去物理意义、四元数失去单位约束。误差状态卡尔曼滤波（ESKF）通过在切空间维护误差状态解决了这个问题，是现代机器人状态估计的标准做法。
第二个难点是IMU 的处理方式。IMU 有 1 kHz 的高频，但直接把它作为观测量塞进滤波器会因采样率差异导致计算量爆炸；作为控制输入则要处理积分漂移与重力耦合。IMU 预积分（Forster 等人，2015）是优雅的解法：在两次关键帧之间预先积分出相对运动与协方差，让优化器只处理低频的关键帧。
第三个难点是可观性与一致性。某些状态下观测无法确定（比如单目视觉的绝对尺度、无加速度激励时的 IMU 零偏），滤波器却可能因为线性化误差给出「过度自信」的协方差，导致后续融合时权重失衡。这需要用可观性约束的 EKF（OC-EKF）或不变 EKF（Invariant EKF）来修正。
本文按「贝叶斯框架 → EKF → ESKF → UKF → IMU 预积分 → 融合架构 → 时间同步 → 可观性 → 工程实践」的顺序展开，给出完整公式与代码。数学细节尽量给到可以直接实现的粒度。
目录
- 状态估计的贝叶斯表述
- 卡尔曼滤波与扩展卡尔曼滤波
- 误差状态卡尔曼滤波（ESKF）
- 无迹卡尔曼滤波与粒子滤波
- IMU 预积分
- 多传感器融合架构
- 时间同步与延迟补偿
- 可观性与一致性
- 工程实践：robot_localization 与 GTSAM
1. 状态估计的贝叶斯表述
状态估计的本质是求后验概率 p(x_t | z_{1:t}, u_{1:t})，即给定全部观测与控制，当前状态的概率分布。
两步递归（贝叶斯滤波）：
预测：p(x_t | z_{1:t-1}) = ∫ p(x_t | x_{t-1}, u_t) · p(x_{t-1} | z_{1:t-1}) dx_{t-1}
更新：p(x_t | z_{1:t}) ∝ p(z_t | x_t) · p(x_t | z_{1:t-1})
预测用运动模型把不确定性「摊开」（协方差变大）
更新用观测把不确定性「收紧」（协方差变小）
不同实现对应不同假设：
卡尔曼滤波：假设线性高斯，闭式解
扩展卡尔曼：非线性但用一阶线性化
无迹卡尔曼：用 sigma 点近似非线性传播
粒子滤波：用样本近似任意分布
因子图优化：批处理，用全部历史联合优化
滤波器与优化器的取舍：滤波器只维护当前状态，内存固定、延迟低；优化器维护历史窗口，精度高但延迟随窗口增长。控制回路用滤波器（动力学与控制基础 会用到它的输出），建图与定位的全局一致用优化器，两者常同时存在。
2. 卡尔曼滤波与扩展卡尔曼滤波
线性高斯情形下，卡尔曼滤波是贝叶斯滤波的精确解。
线性系统：
x_t = F·x_{t-1} + B·u_t + w, w ~ N(0, Q)
z_t = H·x_t + v, v ~ N(0, R)
预测：
x̂⁻ = F·x̂ + B·u
P⁻ = F·P·F^T + Q
更新：
K = P⁻·H^T·(H·P⁻·H^T + R)^-1
x̂ = x̂⁻ + K·(z - H·x̂⁻)
P = (I - K·H)·P⁻
// 一个完整的 1D 卡尔曼滤波，理解各量的物理意义
struct KF1D {
double x = 0.0; // 状态估计
double P = 1.0; // 状态方差
double Q = 0.01; // 过程噪声（模型不确定性）
double R = 0.1; // 观测噪声（传感器不确定性）
void predict(double u, double dt) {
x = x + u * dt; // 匀速模型
P = P + Q * dt; // 方差随时间增长
}
void update(double z) {
const double K = P / (P + R); // 卡尔曼增益 = 信任度之比
x = x + K * (z - x); // 向观测修正
P = (1 - K) * P; // 方差减小
}
};
卡尔曼增益的直觉：K 是「模型方差 / (模型方差 + 观测方差)」。模型越不确定（P 大），越信任观测；观测越不准（R 大），越信任模型。理解这一点就能理解所有滤波器的调参本质：Q 与 R 的比值决定信任分配。
扩展卡尔曼滤波（EKF）把非线性函数在均值处一阶泰勒展开，用雅可比代替 F 与 H。代价是线性化误差，且误差随非线性强度增长。
3. 误差状态卡尔曼滤波（ESKF）
ESKF 把状态分成「名义状态」与「误差状态」，只对误差做线性滤波，从根本上解决了旋转的流形问题。
状态分解：
名义状态 x：在流形上传播（用完整非线性运动学）
误差状态 δx：在切空间（局部向量空间），维度固定
真实状态 = 名义状态 ⊞ 误差状态
IMU 场景的 15 维误差状态：
δx = [δp, δv, δθ, δb_a, δb_g]^T
δp ：位置误差（3）
δv ：速度误差（3）
δθ ：姿态误差，用旋转向量表示（3）
δb_a ：加速度计零偏误差（3）
δb_g ：陀螺仪零偏误差（3）
优势：
1. 误差状态始终接近零，线性化精度极高（这是 ESKF 精度优于 EKF 的根本原因）
2. 姿态误差是 3 维向量，协方差矩阵不奇异
3. 四元数的单位约束在名义状态中由归一化自动维持
// ESKF 的预测步：用 IMU 传播名义状态与误差协方差
void Eskf::predict(const ImuSample & imu, double dt) {
// 1. 去偏置
const Eigen::Vector3d a = imu.accel - b_a_;
const Eigen::Vector3d w = imu.gyro - b_g_;
// 2. 名义状态传播（用完整非线性运动学）
const Eigen::Vector3d a_world = q_ * a + g_; // 转到世界系并加重力
p_ += v_ * dt + 0.5 * a_world * dt * dt;
v_ += a_world * dt;
q_ = q_ * Eigen::Quaterniond(1, 0.5*w.x()*dt, 0.5*w.y()*dt, 0.5*w.z()*dt);
q_.normalize();
// 3. 误差协方差传播：P = F·P·F^T + G·Q·G^T
Eigen::Matrix<double, 15, 15> F = Eigen::Matrix<double,15,15>::Identity();
F.block<3,3>(0, 3) = Eigen::Matrix3d::Identity() * dt; // δp ← δv
F.block<3,3>(3, 6) = -skew(q_ * a) * dt; // δv ← δθ
F.block<3,3>(3, 9) = -q_.toRotationMatrix() * dt; // δv ← δb_a
F.block<3,3>(6, 6) = Eigen::Matrix3d::Identity()
- skew(w) * dt; // δθ ← δθ
F.block<3,3>(6, 12) = -Eigen::Matrix3d::Identity() * dt; // δθ ← δb_g
P_ = F * P_ * F.transpose() + G_ * Q_ * G_.transpose();
}
关键细节：姿态误差的传播用了「局部扰动」的约定（q = q̂ ⊗ δq），这样 δθ 的动力学是 δθ̇ = -[ω]× δθ - δb_g，而不是全局扰动下的另一种形式。两种约定（局部/全局）推导出的雅可比不同，混用会导致滤波器发散。选一种并在注释中写清。
4. 无迹卡尔曼滤波与粒子滤波
当非线性很强（比如观测模型是距离与方位角）时，EKF 的一阶线性化误差不可忽略，UKF 用确定性采样点（sigma 点）传播分布。
UKF 的核心步骤：
1. 从均值与协方差生成 2n+1 个 sigma 点
χ_0 = x̂
χ_i = x̂ + (sqrt((n+λ)P))_i, i = 1..n
χ_{i+n} = x̂ - (sqrt((n+λ)P))_i
2. 把每个 sigma 点通过非线性函数传播
3. 用加权平均与加权协方差重建分布
λ = α²(n+κ) - n，典型 α = 1e-3, κ = 0, β = 2
UKF 的优势是精度达到二阶（泰勒展开），代价是计算量约为 EKF 的 2n+1 倍。对 15 维状态就是 31 倍的函数求值，因此高维时 EKF/ESKF 仍更常用。
粒子滤波（PF）用样本近似任意分布，能处理多峰后验（如全局定位的多个候选位置）。代价是「维数灾难」：状态维度超过 6~10 维时，需要指数级增长的粒子数。因此 PF 主要用于 2D/3D 的位姿估计（AMCL），不用于高维状态。
5. IMU 预积分
IMU 以 200~1000 Hz 输出，若把每个采样都作为因子图的节点，规模会爆炸。预积分把两个关键帧之间的 IMU 采样压缩成一个相对运动约束。
两个关键帧 i 与 j 之间的预积分量（在 i 的体坐标系下）：
Δp_ij = Σ [ v_k·Δt + 0.5·(R_k·(a_k - b_a) + g_world_in_body?)·Δt² ]
Δv_ij = Σ R_k·(a_k - b_a)·Δt
ΔR_ij = Π Exp( (ω_k - b_g)·Δt )
关键性质：预积分量只依赖于 i 与 j 之间的 IMU 读数与零偏，
与 i、j 的绝对位姿无关
因此优化过程中位姿变化不需要重新积分（零偏变化需要，用一阶近似修正）
预积分的用途：
1. 作为因子图中的一个约束（IMU 因子）
2. 提供高频的运动先验，给激光/视觉匹配一个好初值
3. 去运动畸变（点云与图像的卷帘畸变）
// 预积分的增量更新（每来一个 IMU 采样调用一次）
struct Preintegration {
Eigen::Vector3d dp = Eigen::Vector3d::Zero();
Eigen::Vector3d dv = Eigen::Vector3d::Zero();
Eigen::Quaterniond dq = Eigen::Quaterniond::Identity();
Eigen::Matrix<double, 15, 15> cov = Eigen::Matrix<double,15,15>::Zero();
double dt_sum = 0.0;
void integrate(const Eigen::Vector3d & a,
const Eigen::Vector3d & w, double dt) {
// 中值积分（比欧拉精度高，比 RK4 快）
const Eigen::Quaterniond dq_half =
Eigen::Quaterniond(1, 0.5*w.x()*dt, 0.5*w.y()*dt, 0.5*w.z()*dt);
const Eigen::Vector3d a_mid = dq * a; // 转到起始体坐标系
dp += dv * dt + 0.5 * a_mid * dt * dt;
dv += a_mid * dt;
dq = (dq * dq_half).normalized();
dt_sum += dt;
// 协方差传播（雅可比 F、噪声 G 与 ESKF 类似）
propagateCovariance(a, w, dt);
}
};
零偏变化时的重积分问题：优化器调整零偏后，理论上需要重新积分。工程做法是用一阶雅可比修正：Δp(b + δb) ≈ Δp(b) + J_p^b · δb，避免重积分，这是 VINS-Mono 与 LIO-SAM 的做法。
6. 多传感器融合架构
融合架构分松耦合、紧耦合两大类，选择取决于精度要求与算力。
松耦合（Loosely Coupled）：
各传感器先独立解算位姿，再把位姿作为观测融合
例：轮式里程计 + IMU + GPS 各自给出位姿，EKF 融合
优点：模块化、易实现、可增量加入传感器
缺点：丢失原始观测信息、无法处理某传感器单独失效
紧耦合（Tightly Coupled）：
原始观测（特征点、点云、IMU 读数）一起进入一个估计器
例：VINS-Mono（视觉 + IMU）、LIO-SAM（激光 + IMU）
优点：精度高、单传感器退化时仍可用（如视觉弱纹理时靠 IMU）
缺点：实现复杂、计算量大、调试难
半紧耦合：
用 IMU 预积分给激光匹配提供初值，但观测仍分开处理
例：FAST-LIO2 用 IEKF 融合，介于两者之间
典型架构的分层：
第 1 层（1 kHz）：ESKF 融合 IMU + 轮速，输出高频里程计与 TF
第 2 层（10~30 Hz）：激光/视觉里程计作为观测更新第 1 层
第 3 层（1~10 Hz）：GPS、回环、地图匹配做全局修正
第 4 层：把修正后的全局位姿写回，形成闭环
关键：每层输出都带协方差，上层据协方差决定信任度
7. 时间同步与延迟补偿
融合系统里最隐蔽的 bug 来源是时间不同步。10 ms 的偏差在 1 m/s 的运动下就是 1 cm 的位置误差，在 1 rad/s 的旋转下是 0.6° 的姿态误差。
三个层次的时间问题：
1. 采样时刻 vs 接收时刻
消息的 header.stamp 必须是采样时刻，不是发布时刻
2. 传感器之间的时钟偏差
硬件同步（PPS/PTP）> 软件时间戳对齐 > 无处理
3. 处理延迟
观测对应的其实是「过去的某个时刻」，必须按延迟回退
延迟补偿的做法：
维护一个状态历史缓冲区（保留最近 0.5~1 秒的状态与协方差）
收到观测时，按观测的时间戳在缓冲区中定位，
用该时刻的状态做更新，然后把更新量传播回当前时刻
// 带延迟补偿的观测更新
void Fusion::updateWithDelay(const Obs & obs) {
// 1. 在历史缓冲中找到观测时刻的状态
const StateAtTime s = history_.interpolate(obs.stamp);
// 2. 用历史状态计算卡尔曼增益与修正量
const Eigen::VectorXd dx = computeCorrection(s, obs);
// 3. 把修正量通过状态转移矩阵传播到当前时刻
const Eigen::MatrixXd Phi = stateTransition(s.stamp, now());
const Eigen::VectorXd dx_now = Phi * dx;
// 4. 应用到当前状态
applyCorrection(dx_now);
}
8. 可观性与一致性
可观性决定「哪些状态能被观测确定」，是滤波器设计中容易被忽略但极其重要的一环。
常见不可观/弱可观状态：
1. 单目视觉的绝对尺度（除非有 IMU 提供加速度激励）
2. 静止时的 IMU 零偏（无激励则零偏与重力不可分）
3. 匀速运动时的 IMU 零偏与速度（不可分）
4. 平面运动时的滚转/俯仰零偏（无旋转激励）
一致性问题的表现：
EKF 的线性化误差让协方差被低估（过度自信）
→ 滤波器逐渐忽略新观测
→ 估计值缓慢偏离真值而不自知
→ 表现为「滤波器看起来平滑但误差在增长」
对策：
1. 可观性约束 EKF（OC-EKF）：修正雅可比，让不可观方向不产生虚假信息
2. 不变 EKF（Invariant EKF / IEKF）：用李群上的误差定义，天然保持可观性
3. 人为加过程噪声：给不可观方向加 Q，避免协方差塌缩（简单有效）
4. 一致性检验：用 NEES（归一化估计误差平方）监控是否超出卡方分布界
NEES 是检验一致性的标准工具：
NEES = (x_true - x̂)^T · P^-1 · (x_true - x̂)
若滤波器一致，NEES 应服从自由度为 n 的卡方分布
对 15 维状态，95% 置信区间约为 [6.3, 25.0]
长期超出上界 → 协方差被低估（过度自信）
长期低于下界 → 协方差被高估（过于保守，融合效率低）
9. 工程实践：robot_localization 与 GTSAM
两个层次的工具：robot_localization 用于实时的松耦合 EKF/UKF，GTSAM 用于批处理优化。
# robot_localization 的 EKF 配置：融合轮速与 IMU
ekf_filter_node:
ros__parameters:
frequency: 50.0 # 输出频率
sensor_timeout: 0.1
two_d_mode: true # 平面运动，忽略 z/roll/pitch
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom # 局部 EKF 用 odom，全局用 map
odom0: /wheel/odom
odom0_config: [false, false, false,
false, false, false,
true, true, false, # vx, vy 由轮速提供
false, false, true, # vyaw
false, false, false]
odom0_differential: false
odom0_relative: false
imu0: /imu/data
imu0_config: [false, false, false,
false, false, true, # roll, pitch（IMU 提供）
false, false, false,
false, false, true, # vyaw
true, false, false] # ax 前向加速度
imu0_differential: false
imu0_relative: false
imu0_remove_gravitational_acceleration: true # 关键：去掉重力
odom0_config 的 15 个布尔值对应 [x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az]。最容易错的是把同一个量配置到两个传感器上（比如 IMU 的 yaw 与轮速的 yaw），这会让滤波器把同一信息用了两次，协方差塌缩、过度自信。正确做法是每个量只由一个传感器提供，其他传感器用 _differential 或不同分量补充。
# 验证融合输出：对比 EKF 输出与原始里程计，看平滑度与延迟
ros2 run plotjuggler plotjuggler -d /odom /wheel/odom /imu/data
# 检查 TF 树是否完整无环
ros2 run tf2_tools view_frames
权衡取舍
| 决策 | 选 A | 选 B |
|---|---|---|
| 滤波器 | EKF/ESKF：高维、实时、工程成熟 | UKF：强非线性、低维 |
| 姿态表示 | ESKF：误差在切空间、精度高 | 四元数 EKF：简单但协方差奇异 |
| 融合方式 | 松耦合：模块化、易调试 | 紧耦合：精度高、抗退化 |
| IMU 处理 | 预积分：优化器、低频关键帧 | 直接传播：滤波器、高频 |
| 延迟处理 | 状态历史缓冲：精度高 | 忽略延迟：简单但误差大 |
| 全局修正 | 因子图优化：一致性最好 | 卡尔曼更新：实时但局部 |
核心原则：协方差要诚实。宁可给稍大的 Q 让滤波器保持对新观测的敏感，也不要让协方差塌缩导致滤波器「自我封闭」。这是工程上比精度更重要的属性。
常见坑清单
- 四元数直接进 EKF 状态向量，协方差失去意义——用 ESKF 在切空间维护 3 维误差。
- 局部扰动与全局扰动的雅可比混用，滤波器缓慢发散——选定一种约定并写进注释。
- IMU 未去重力就作为加速度观测，滤波器把重力当成运动——启用
remove_gravitational_acceleration。 - 同一物理量配置到两个传感器，协方差塌缩、过度自信——每个量只由一个源提供。
- 忽略观测延迟，高速运动时估计滞后——用状态历史缓冲做延迟补偿。
- 消息时间戳用发布时刻，时间轴被拉伸——驱动层必须写采样时刻。
- 静止时试图估计 IMU 零偏，零偏与重力不可分导致漂移——只在有激励时估计零偏，或先做静态标定。
- 线性化误差导致协方差被低估，滤波器逐渐忽略观测——用 OC-EKF/IEKF，或给不可观方向加 Q。
- 零偏变化后不重积分也不做一阶修正，预积分量失配——用雅可比做一阶修正。
- 不做一致性检验，滤波器「看起来平滑」但误差在增长——用 NEES 监控并对照卡方界。
小结
状态估计的核心是把「模型的不确定」与「观测的不确定」按协方差正确加权。ESKF 解决了姿态的流形问题，IMU 预积分解决了高频采样与低频关键帧的尺度矛盾，因子图与滤波器的组合解决了实时性与全局一致性的矛盾。这三件事理解了，就能读懂绝大多数现代机器人状态估计系统。
下一步建议阅读 SLAM 与定位建图 ，看状态估计如何作为 SLAM 的前端；运动规划与轨迹优化 会用到状态估计输出的位姿与协方差（协方差大的区域要规划得更保守）；机器人部署、实时性与安全 那一篇会讲状态估计失效时如何安全降级。
最后一句：先做静态标定，再做在线估计。IMU 零偏、相机-IMU 外参、轮径与轮距这些量，离线标定的精度远高于在线估计，能让滤波器少背很多负担。
继续阅读
探索更多技术文章
浏览归档，发现更多关于系统设计、工具链和工程实践的内容。