Point-LIO 与 EKF 状态估计基础
Summary
LiDAR—惯性里程计通过 IMU 提供高频、连续但会漂移的运动信息，再利用 LiDAR 点与地图几何结构之间的残差修正状态。EKF 的核心不是直接“计算位置”，而是在维护状态估计及其不确定性的基础上，持续执行预测、比较与校正。
IMU 为什么会漂移
陀螺仪测量角速度，积分后得到姿态变化；加速度计测量的是包含重力、零偏和噪声的比力。系统需要结合姿态完成坐标变换并处理重力，再积分得到速度、二次积分得到位置。
噪声和 bias 会在积分中累积。姿态的小误差还会使重力补偿出现偏差，将一部分重力错误地当作线性加速度，因此位置误差往往增长得尤其快。IMU 的优势是频率高、短时连续性好，而不是能够长期提供无漂移位置。
LiDAR 如何形成几何约束
LiDAR 测到的是点在 LiDAR 坐标系中的位置，不会直接给出传感器在世界坐标系中的绝对位置。系统使用当前估计的位姿将点变换到地图坐标系，再寻找地图邻域并拟合局部平面。
设 LiDAR 点为 ，当前旋转和平移估计为 ，局部平面的法向量为 、参考点为 ，则点到平面的有符号残差可写为：
位姿正确且数据关联可靠时，变换后的点应落在对应表面附近，残差应接近零。这里的 residual 不是“LiDAR 位置减 IMU 位置”，而是由当前状态预测出的点与地图局部几何之间的不一致。
点到平面残差利用了表面的法向约束，不要求新点与地图点之间存在严格的一一对应，通常比简单点到点距离更适合稠密表面配准。不过，它依赖正确的数据关联和可靠的局部平面；在植被、动态物体、重复结构或邻域点不足时，残差可能失真。
残差怎样修正位置和姿态
一个非零残差只说明当前状态与观测不一致，并不会单独给出位置和姿态各应修改多少。系统还需要计算残差对各状态变量的敏感程度，即观测模型的 Jacobian。
直观地说，Jacobian 回答的是：如果位置、姿态或其他状态发生一个很小的变化，残差会怎样变化。平移会直接改变点在地图中的位置；旋转则会改变 ，而且点离旋转中心越远，同样的小角度误差通常会造成越明显的位置偏移。EKF 根据这种敏感关系把标量或低维的几何残差映射成对多维状态的修正。
几何约束与可观测性
地图中的几何结构只能约束会引起残差变化的状态方向。例如面对一面无限大的平面时，沿平面法向移动会明显改变点面距离，而沿平面切向平移可能几乎不改变残差。因此，一面墙无法完整约束六自由度位姿。
地面、不同朝向的墙、柱体和拐角等多方向结构组合起来，才能提供更完整的约束。长走廊或隧道即使点很多，也可能因为大量点落在相互平行的平面上，使某些运动方向对残差不敏感，形成 LIO 退化。退化的本质不只是“特征少”，而是环境提供的独立约束方向不足。
EKF 的状态与不确定性
EKF 同时维护状态估计和协方差：
- 状态估计描述系统当前认为的位置、速度、姿态、bias 等；
- 协方差描述这些估计有多不确定，以及不同状态误差之间如何相关。
预测阶段根据运动模型将状态向前传播，同时传播协方差。持续依赖 IMU 积分时，过程噪声和 bias 不确定性通常会使协方差增大。
观测到来后，系统计算创新或残差，并利用 Jacobian、预测协方差和观测噪声计算 Kalman Gain。它决定观测对各状态分量的修正力度：预测越不确定且观测越可靠，更新越依赖观测；观测噪声越大，修正通常越保守。
因此，EKF 的基本闭环可以概括为：
上一时刻状态与协方差
↓
运动模型预测状态并传播不确定性
↓
根据预测状态生成观测预测
↓
真实观测与观测预测形成残差
↓
Jacobian + 两侧不确定性决定修正量
↓
更新状态与协方差为什么需要 Extended Kalman Filter
标准线性 Kalman Filter 要求状态转移和观测关系是线性的，而惯性导航与点云配准包含三维旋转、坐标变换等非线性关系。EKF 在当前状态估计附近用 Jacobian 对非线性模型做一阶局部线性化，再应用 Kalman Filter 的预测与更新思想。
这种近似依赖当前估计足够接近真实状态。当初值很差、动态过强、观测模型不准确或线性化误差过大时，EKF 可能更新不稳定甚至发散。
Point-LIO 的逐点更新
传统帧级处理常把一帧点云视为同一时刻的观测，但机械式或非重复扫描 LiDAR 的点是在一段时间内依次产生的。平台高速运动时，不同点实际对应不同位姿，若直接使用单一帧位姿，就会产生运动畸变。
Point-LIO 的重要特点是按照测量时间顺序，以单个 LiDAR 点为粒度进行状态传播和观测更新。每个点使用与自身时间戳相对应的状态参与坐标变换和地图匹配，从而显著缓解帧级处理中的运动畸变，并提高高动态场景下的估计带宽。
逐点更新并不意味着运动畸变必然消失。时间戳错误、LiDAR—IMU 不同步、外参不准、状态传播错误或地图关联失败，仍然会造成系统误差。
还需要区分 Point-LIO 前端的定位地图和应用层语义体素地图。原始 Point-LIO 的核心几何观测是点到局部平面的残差；下游项目使用体素哈希保存语义统计，不等于 Point-LIO 本身采用所谓“点到体素残差”。
Point-LIO 中 IMU 建模的特殊性
理解一般 LIO 时，常用“IMU 负责 prediction、LiDAR 负责 measurement update”的框架建立直觉。Point-LIO 的具体建模更特殊：它把真实角速度和加速度纳入状态动态，并将 IMU 读数作为观测参与更新，而不只是把原始 IMU 读数当作传统控制输入。这一设计需要与 Point-LIO 的高频逐点更新和 IMU 饱和处理结合理解。
因此，通用 EKF 直觉适合解释信息融合闭环，但分析 Point-LIO 实现时仍应以其论文中的状态定义、过程模型和观测模型为准，不能直接套用其他 LIO 的状态传播方式。
状态、输入与观测
状态是系统内部需要估计的量，例如位置、速度、姿态、bias，以及 Point-LIO 运动学模型中显式维护的真实角速度和真实加速度。输入是被视为已知、用于驱动状态方程的外部量；观测则是传感器对状态的带噪测量。
在 Point-LIO 中，陀螺仪读数不应被直接等同于真实角速度，加速度计读数也不应被直接等同于真实加速度。IMU 输出作为对相应运动状态的观测参与滤波更新。这个建模与将 IMU 原始读数作为确定性运动输入的常见直觉不同，也不能表述为“先预积分得到角速度和加速度”：预积分通常是把关键帧间大量 IMU 测量压缩成相对运动约束，而 Point-LIO 强调的是高频、异步的逐测量更新。
IMU 饱和时的观测处理
当某个陀螺仪或加速度计通道达到量程上限时，该通道给出的值不再可靠地反映真实运动。Point-LIO 可以识别这种饱和，并在当前滤波更新中不使用该通道的 IMU 观测，而不是把被截断的读数当成真实运动量。
此时系统并不会立即失去状态估计：运动学模型仍可继续传播状态，后续有效的 LiDAR 点仍会通过点到平面残差提供几何约束。只是可用观测信息减少，状态不确定性通常会增大。应准确表述为“跳过饱和通道的观测更新”，而不是“停止该通道的输入处理”，因为在这种建模下 IMU 读数本来就不是确定性输入。
ikd-Tree：定位用几何地图
Point-LIO 的 ikd-Tree 是面向定位前端的增量几何点云地图。其主要职责是维护已经按当前位姿估计配准到地图坐标系的 LiDAR 点，并为新到达的 LiDAR 点提供高效 KNN 查询。滤波器利用查询到的邻域点拟合局部平面，从而构造 point-to-plane residual。
ikd-Tree 本身不负责独立确定 pose；位姿由状态预测、LiDAR 几何残差和 EKF 更新共同得到。IMU 也不会直接向 ikd-Tree 加入点，它通过状态估计间接影响 LiDAR 点被变换到地图中的位置。
局部平面拟合还需要进行可靠性检验。若邻域点数量不足、分布退化、平面拟合误差过大，或存在动态物体与错误数据关联，则该邻域不能提供可信的几何观测。此时应拒绝该点的 LiDAR 更新，而不是强行构造残差；否则错误的法向量或平面位置会将滤波状态推向错误方向。是否把该点注册进地图，还应受地图更新策略、距离阈值和动态点过滤等条件约束。
Key Points
- IMU 提供高频连续运动信息，但积分会累积噪声、bias 和姿态误差。
- point-to-plane residual 是变换后的 LiDAR 点与地图局部平面之间的距离，不是两个传感器位置直接作差。
- Jacobian 描述残差对状态的敏感性，使几何误差能够转化为位置、姿态等状态修正。
- Kalman Gain 综合预测不确定性与观测不确定性，决定修正方向和力度。
- LIO 退化源于环境几何对某些自由度缺少独立约束，而不只是点云数量少。
- Point-LIO 通过按时间戳逐点传播和更新显著缓解高速运动下的帧内畸变。
- 下游语义体素地图与 Point-LIO 前端定位地图应明确区分。
- Point-LIO 中真实角速度和加速度属于状态，IMU 读数是对这些状态的观测；饱和通道应跳过观测更新。
- ikd-Tree 服务于增量几何地图和 KNN 查询，不独立估计位姿，也不替代语义体素地图。