前言

• 相信关注我们之前文章的朋友，对 FAST-LIO2 应该并不陌生。此前我们已经围绕 FAST-LIO2 展开过一系列学习与实践：

  • 【FAST-LIO2 源码解读】从 Mid-360 到 ESEKF — 公式与代码逐行对照
  • 【10天速通ROS2-PX4无人机】(二) FAST-LIO2 仿真部署，200HZ IMU前推的无限魅力

• 不过，最近组内一个部署在四足机器狗上的项目遇到了一个比较棘手的问题：在楼层场景下，机器狗需要通过电梯实现跨楼层移动，而当机器人进入电梯、轿厢开始运行后，FAST-LIO2 输出的里程计却出现了明显漂移，并且随着电梯运行时间的增加，误差不断累积，最终甚至无法恢复到正常状态。

• 为此，本文将介绍一篇针对这一问题提出的工作：Elevator-LIO: Robust LiDAR-Inertial Odometry for Multi-Floor Navigation under Elevator-Induced Non-Inertial Motion。该方法把机器人相对电梯的运动与电梯自身的运输运动拆成两组状态，嵌进一个模式相关的迭代误差状态卡尔曼滤波（IESKF，Iterated Error-State Kalman Filter）框架，从而实现机器人在电梯运行期间的连续定位。

• 本文围绕三个问题展开：电梯里究竟是哪一步让 LIO 失效；Elevator-LIO 的状态方程与观测方程是怎么改的；这些公式在源码里分别落在哪个函数。

---

---

0 回顾

• 在进入 Elevator-LIO 之前，先把两条被反复对比的基线方法回顾一遍。第 1 章的失效分析会反复回到它们的建模假设上。

0-1 FAST-LIO2

• FAST-LIO2 是当前应用最广的紧耦合激光惯性里程计之一，主干可以拆成三步：IMU 前推、整帧点云一次性纠正、IESKF 迭代更新全部点。

0-1-1 IMU 前推

• 两帧激光之间按 IMU 采样时刻逐步积分，得到每个 IMU 时刻的位置、姿态与速度，同时把噪声协方差一路递推：
  $${\hat{x}}_{k+1}=f\left({\hat{x}}_k,u_k\right),P_{k+1}=F_k{P}_k{F}_k^T+B_{k}Q{B}_k^T$$

0-1-2 整帧点云一次性纠正

• 前推留下一条逐 IMU 时刻的位姿序列，每个点按自己的采样时刻取对应位姿变换到帧末，整帧的运动畸变在这一步被一次性纠正掉。

0-1-3 IESKF 迭代更新全部点

• 纠正后的整帧点与局部地图配准，点到面残差为
  $$r_i=u_i^T\left(R{p}_i+t-q_i\right)$$
• 全部点的残差与雅可比堆成一个大矩阵，交给迭代误差状态卡尔曼滤波（IESKF，Iterated Error-State Kalman Filter）求解，状态增量小于阈值即停止。增益形式为
  $$K={\left(H^T{R}^{-1}H+P^{-1}\right)}^{-1}{H}^T{R}^{-1}$$
• 所以 FAST-LIO2 的高频只体现在前推上，量测更新仍是帧级批处理：成百上千个点共享同一个状态估计，在帧边界上被一次性拉回，帧内没有新的量测介入。地图后端使用增量式 k-d 树（ikd-Tree，incremental k-d tree），状态包含位置、姿态、速度、两个 IMU 零偏与重力。

0-1-4 电梯下的 FAST-LIO2

• 这套方法的成立前提有两条：IMU 量测可以被解释为载体的自身运动；LiDAR 观测的是惯性系下的绝对几何。电梯轿厢里，这两条同时不成立。
  • 在电梯场景下，运输加速度被直接积进速度与位置，LiDAR 残差又把这部分位移往回拉，两者形成拉锯。加速度越大拉锯越剧烈，最终表现为Ⅱ型发散，表 II 中 Dormitory 四条序列全部属于这一类。

说人话：FAST-LIO2 依赖 IMU 前推和 LiDAR 配准相互纠正。但在电梯内，机器人虽然相对轿厢静止，IMU 却会将轿厢的加速度积分到机器人位姿中，而 LiDAR 观测到的环境又随轿厢一起运动。两者对运动的解释产生冲突，最终导致状态估计发散。

0-2 Point-LIO

• Point-LIO 走的是另一条路线：取消帧的概念，把量测粒度从"帧"细化到"点"。

0-2-1 逐点更新

• 每个 LiDAR 点携带自己的采样时刻，到达即处理：先从前一点的时刻传播到本点时刻，再用该点的残差做一次更新，然后处理下一个点。
  $${\hat{x}}_{{\tau }_j}^{-}=f\left({\hat{x}}_{{\tau }_{j-1}},u_{{\tau }_{j-1}:{\tau }_j}\right)$$
  $$r_j=z_j-h_j\left({\hat{x}}_{{\tau }_j}^{-}\right),K_j=P_j^{-}{H}_j^T{\left(H_j{P}_j^{-}{H}_j^T+R_j\right)}^{-1},\delta x_j=K_j{r}_j$$
• 更新频率因此由点率而不是帧率决定，论文报告可到千赫兹量级。去畸变与配准被揉进量测更新的节拍里，不再需要单独的帧级去畸变。地图后端使用增量式体素哈希（iVox，incremental voxel），点一到达即可查询邻域。

0-2-2 双 IESKF 与不完全相信 IMU

• 源码里同时存在两个 IESKF，由开关 use_imu_as_input 选择：kf_input 为 24 维，IMU 作控制输入；kf_output 为 30 维，把角速度与加速度也写进状态，IMU 改以观测方程的身份出现。
• 后者的意义是不完全相信 IMU：任一通道读数达到量程的 99% 即判饱和，该通道残差置零、不进增益。叠加逐点更新，IMU 只承担相邻两点之间不足一毫秒的传播。

0-2-3 电梯下的 Point-LIO

• 代价是 LiDAR 几何在估计中的话语权被放大到极致。在轿厢里，四壁、地板与天花板构成一个几乎静止的刚体结构，逐点更新会迅速把位姿收敛到准静态解，IMU 报出的运输加速度反而被当作与几何矛盾的噪声压制掉。
  • 结果是不同楼层的点云被叠到同一高度，而轨迹本身不会立刻崩掉，也就是Ⅰ型漂移。表 II 中 20 条序列全部归入这一类，其中加速度平缓的 Office 序列最难被察觉。
  • 这条失效路径说明：提高更新频率、甚至不再无条件相信 IMU，都不能解决问题，因为矛盾不在时间分辨率与信任程度上，而在参考系。

说人话：由于 Point-LIO 的双 IESKF 机制，IMU 可以作为状态观测而非单纯的控制输入，降低对 IMU 量测的依赖。但在电梯内，逐点 LiDAR 更新会不断强化轿厢内部的几何约束，反而压制 IMU 感知到的运输加速度，最终导致不同楼层的点云被错误叠加，形成Ⅰ型漂移。

---

1 问题背景：电梯场景下的 LIO 失效

1-1 电梯场景下的非惯性运动问题

• 电梯轿厢是一个运动参考系。设世界系为 $F_W$、轿厢系为 $F_E$、IMU 系为 $F_I$，机器人相对轿厢运动、轿厢相对世界运动，两者叠加构成 IMU 看到的绝对运动：$$a_I^W=a_E^W+R_E^W{a}_I^E$$其中：
  • 等式右边第一项是轿厢的运输加速度
  • 第二项是机器人相对轿厢的加速度经旋转到世界系后的结果。
• 加速度计给出的比力量测只包含这两项的叠加，单帧内无法把二者分开；而激光雷达（LiDAR，Light Detection and Ranging）在轿厢里看到的几何，全部是相对于轿厢内壁的几何。
• 于是运动先验（IMU，Inertial Measurement Unit，惯性测量单元）与几何观测（LiDAR）落在了两个不同的参考系上。传统 LIO（LiDAR-Inertial Odometry，激光雷达惯性里程计）的传播方程默认 $a_E^W\equiv 0$，会把整段运输加速度记到机器人自己头上。
• 这带来两个层面的后果：
  • 状态层面：运输状态在 LiDAR 残差里没有直接雅可比，电梯运行期间只能靠 IMU 前推，协方差随时间单调增长，属于典型的弱可观问题。
  • 前端层面：开放楼层与封闭轿厢的空间尺度相差一个量级以上，固定分辨率体素降采样在两侧都无法同时给出合适的点密度。

传统 LIO 的默认假设	电梯场景的实际情况
载体在惯性系中运动	载体位于运动的非惯性系中
IMU 量测即机器人自身运动	IMU 量测为运输加速度与相对加速度的叠加
LiDAR 残差可观测全部状态	运输状态不出现在 LiDAR 残差中
场景尺度在一个量级内平稳变化	开阔楼层与封闭轿厢来回切换

说人话：电梯在动，机器人没动，但 IMU 会如实报出"我在加速"；LiDAR 看到的墙壁又纹丝不动。两路传感器各说各话，滤波器只能在其中挑一个相信，误差就是这么攒出来的。

1-2 两种失效模式

• 论文把主流方法在电梯场景下的失败归结为两类，分别对应漂移与发散。表 II 统计了四种代表性方法在 20 条真实序列上的表现，记法为 成功/Ⅰ型失败/Ⅱ型失败。

场景	FAST-LIO2	LIO-SAM	Point-LIO	VoxelMap	Elevator-LIO
Office办公室	0/9/1	0/0/10	0/10/0	0/5/5	10/0/0
Dormitory宿舍	0/0/4	0/0/4	0/4/0	0/3/1	4/0/0
Campus校园	0/2/2	0/0/4	0/4/0	0/4/0	4/0/0
Mall商场	0/0/2	0/0/2	0/2/0	0/2/0	2/0/0
合计	0/11/9	0/0/20	0/20/0	0/14/6	20/0/0

1-2-1 Ⅰ型失败

• Ⅰ型失败指电梯运行期间估计退化为准静态解。LiDAR 主导的解一旦收敛，竖直方向几乎被轿厢几何"锁死"，IMU 的运输加速度被当作噪声压制，不同楼层的点云被叠到同一个高度上，但轨迹不会立刻崩掉。
• Point-LIO 在这批序列上 20 条全部属于Ⅰ型，与它逐点高频更新、强依赖 LiDAR 先验的设计直接相关。在加速度平缓的 Office 序列中，这种"叠层"现象最难被察觉。

1-2-2 Ⅱ型失败

• Ⅱ型失败指 IMU 与 LiDAR 的观测出现严重冲突，状态被错误地推着走，位姿突变后发散。
• Dormitory 序列的加速度更大，FAST-LIO2 更容易进入这种拉锯，最终轨迹飞出建筑。
• 根本原因是同一套滤波框架下，惯性约束与几何约束在非惯性系中无法同时被满足。

• 图 7 给出了同一场景下的定性对比：上方 Elevator-LIO 完成了一趟 1F 到 2F、再经楼梯回到 3F 的闭环；中间的 Point-LIO 直接丢掉了两段乘梯过程，只剩楼梯段；下方的 FAST-LIO2 在电梯内发散，轨迹脱离建筑。

---

2 Elevator-LIO

2-1 论文信息与算法简介

• Elevator-LIO 是上海交通大学 Yifan Zhang 等人提出的面向电梯非惯性运动场景的激光惯性里程计，论文发表于 2026 年 5 月的 arXiv（arXiv:2605.24495v1），基于 Livox MID-360 采集数据，并提供包含 20 条序列、79 次乘梯的数据集及 ROS 1 / ROS 2 Humble 开源实现。
• 论文要解决的问题是：
  • 机器人被电梯运输时如何保持连续定位。轿厢本身是一个非惯性参考系，IMU 感受到的是运输运动与相对运动的叠加，LiDAR 只能观测轿厢内壁的相对几何，两路传感器的参考系并不一致。
• 算法由三层构成，后文各小节的展开顺序与这三层一一对应：
  • 状态层：把机器人相对轿厢的 15 维状态与电梯的 3 维竖直运输状态拼接成解耦状态向量。
  • 滤波层：一个模式相关的迭代误差状态卡尔曼滤波，普通环境退化为标准 LIO，进入轿厢后按非惯性模型传播，并在退出时刻施加零状态约束。
  • 前端层：事件触发的电梯模式管理器与反馈驱动的自适应体素降采样。
• 实验规模为 20 条真实序列、共 79 次电梯乘坐，覆盖办公楼层、宿舍、校园与商场四类场景，其中 17 条序列的终端高度误差低于 1 cm。在 Hilti 2022 与 Hilti 2023 标准惯性数据集上，精度与主流方法处于同一量级，说明这套非惯性改造没有牺牲常规场景的表现。
• 从工程借鉴角度看，这套方法没有替换 LIO 主干，而是在原有 IESKF 上增加了三维运输状态与一段条件填表的传播逻辑。因此"关闭电梯模式即普通 LIO"这一退化性质可以被严格保证，改造代价停留在状态维度与若干条件分支上。

说人话：Elevator-LIO 并没有推翻 FAST-LIO2 的原有框架，而是专门增加了一套电梯运动建模机制，将机器人自身运动与电梯竖直运输运动分开估计，再通过模式切换处理进出电梯的状态变化，从而实现跨楼层连续定位，同时尽可能保持普通环境下的 LIO 性能。

2-2 方法概览

• 三个层次之间的关系可以放到一张图里看：模式管理器给出当前所处的参考系，传播方程据此决定是否打开电梯子状态，LiDAR 更新与退出约束在两个模式间共享同一套求解器。
  

• 图 1 的三个分图把问题与方案连在一起看：a 是跨楼层导航需要机器人在乘梯全程保持定位；b 是轿厢内的传感不一致，IMU 测到的是叠加运动，LiDAR 测到的是相对几何；c 是真实结果，跨楼层轨迹连续、地图全局一致。

• 手持平台采集的一条序列在图 2 中给出。a 到 f 六处局部放大覆盖了加速、匀速、减速与进出轿厢的全过程，其中 a 与 d 是轿厢内的剧烈晃动，估计结果依然把两种运动分得开。
  

• 论文列出的四条贡献可以概括为：

  • 一套面向电梯场景的解耦状态估计框架，并给出非惯性系下的运动方程。
  • 面向电梯"走走停停"特性的漂移抑制机制，把零状态更新与轻量级电梯状态检测组合起来。
  • 反馈驱动的自适应降采样前端，在开阔楼层与封闭轿厢之间维持稳定的有效点数。
  • 真实序列、公开数据集与自研仿真器上的完整验证，并公开数据与工具。

2-3 系统结构

• 整个系统分为预处理、状态估计与建图三段。预处理负责缓冲排序、静态初始化与自适应降采样；状态估计里嵌入电梯模式管理器，决定当前走惯性传播还是非惯性传播；建图部分在 IESKF 收敛后把点云插入增量式 k-d 树（ikd-Tree，incremental k-d tree）。

• 对照源码，论文里的每个模块都能找到对应的实现文件，这也是本文后续逐节展开的依据。

论文模块	源码位置	职责
预处理与调度	src/node/LidarPipeline.cpp、src/main.cpp	缓冲出队、状态查询、降采样、调用链编排
IMU 传播	src/estimator/IMUProcess.cpp	状态预测、协方差传播、电梯子状态积分
IESKF 更新	src/estimator/ESEKF.cpp	残差构建、雅可比、迭代求解
模式管理器	src/elevator/ElevatorProcess.cpp、src/elevator/ElevatorSelfExit.cpp	进门检测、停稳检测、模式切换
自适应降采样	include/AdaptiveFilter/	幂律反馈控制器与分区域体素滤波
建图与发布	src/node/map.cpp、src/node/publish.cpp	ikd-Tree 增量插入、里程计与话题发布

2-4 电梯非惯性坐标系与状态解耦

2-4-1 动参考系中的运动学

• 传统的动参考系中的运动学（Kinematics in a Moving Reference Frame）在讲一个非常经典的力学问题：
  • 当参考坐标系本身也在运动时，如何描述物体的真实运动？
• 记号沿用论文表 I：$F_W$、$F_E$、$F_I$ 分别为世界系、轿厢系与 IMU 系；$p_I^E$ 是 IMU 在轿厢系中的位置；$p_{Ez}^W$、$v_{Ez}^W$、$a_{Ez}^W$ 是轿厢沿世界竖直轴的位移、速度与加速度。
• 相对运动定理给出的一般形式为
  $$p_I^W=p_N^W+R_N^W{p}_I^N,v_I^W=v_N^W+R_N^W{v}_I^N+{\omega }_N^W\times \left(R_N^W{p}_I^N\right)$$
  $$a_I^W=a_N^W+R_N^W{a}_I^N+2{\omega }_N^W\times \left(R_N^W{v}_I^N\right)+{\dot{\omega }}_N^W\times \left(R_N^W{p}_I^N\right)+{\omega }_N^W\times \left({\omega }_N^W\times \left(R_N^W{p}_I^N\right)\right)$$
• 后两项分别是欧拉加速度与向心加速度。完整形式里，科氏项与欧拉项只有在参考系存在转动时才出现。

说人话：相对运动定理告诉我们，机器人在世界中的运动不等于机器人自身的相对运动。必须把参考系自身的运动也考虑进去，这就是 Elevator-LIO 状态解耦的物理基础。

2-4-2 电梯场景

• 电梯场景可以大幅简化：轿厢只做竖直平移，转动可忽略，即 ${\omega }_{WE}\approx 0$；同时轿厢系初始化为与世界系平行，$R_E^W=I$。于是加速度合成退化为
  $$a_I^W=a_E^W+R_E^W{a}_I^E=\left[\begin{array}{c}0\\ 0\\ a_{Ez}^W\end{array}\right]+a_I^E$$
• 最终采用的状态为机器人相对部分与电梯运输部分的拼接：
  $$x={\left[x_{rel}^T,x_{elev}^T\right]}^T$$
  $$x_{rel}={\left[(p_I^E{)}^T,(q_I^E{)}^T,(v_I^E{)}^T,b_a^T,b_{\omega }^T\right]}^T,x_{elev}={\left[p_{Ez}^W,v_{Ez}^W,a_{Ez}^W\right]}^T$$
• 论文特意把运输加速度 $a_{Ez}^W$ 而不是机器人相对加速度 $a_{Iz}^E$ 放进状态，理由有两点：
  • 电梯的加速度曲线在运行过程中更平滑，以随机游走建模成立性更好。
  • 未被建模的分量会被 IMU 前推立刻吸收，被建模的分量则以潜变量的身份接受 LiDAR 更新与退出阶段约束，把平滑的那部分交给滤波器是更合理的信息分配。
• 再往上加急动度（jerk）会扩大状态维度并放大噪声敏感度，作者在表达力与数值稳定性之间取了折中。重力在静态初始化后固定，不参与在线估计：在轿厢系里重力与 IMU 偏置、运输加速度、相对加速度强耦合，在线估计只会让参数纠缠得更厉害。

说人话：为了描述电梯运输带来的额外运动，Elevator-LIO 在原有 15 维机器人相对运动状态的基础上，增加了 3 维电梯运输状态，分别表示轿厢的竖直位移、速度与加速度，最终形成 18 维误差状态。这样就能将机器人自身运动与电梯整体运输运动分开估计，避免两种运动相互干扰。

// include/support/type.h
// 状态索引：R/P/V 为世界系下的姿态、位置、速度，BW/BA 为陀螺与加计零偏，G 为重力
enum StateIndex : int {
	R = 0,                   // (3 dimention) rotation in world frame
	P = 3,                   // (3 dimention) position in world frame
	V = 6,                   // (3 dimention) velocity in world frame
	BW = 9,                  // (3 dimention) IMU gyroscope bias
	BA = 12,                 // (3 dimention) IMU acceleration bias
	G = 15,                  // (3 dimention) Gravity in world frame
	// Elevator 1D substate (world gravity axis)  (scalars)
	Z  = 18,                 // z position (1)
	VZ = 19,                 // z velocity (1)
	AZ = 20,                 // z acceleration (1)
	STATE_TOTAL = 21
};

struct State {
	// ------------ Nominal (world imu) ------------
	Eigen::Quaterniond q;   // w_q_imu
	Eigen::Vector3d p;      // w_p_imu
	Eigen::Vector3d v;      // w_v_imu
	Eigen::Vector3d bw;
	Eigen::Vector3d ba;
	Eigen::Vector3d g;      // world
	// ------------ Elevator 1D substate (world z-axis) -----------
	// z-axis is defined by n_z = normalize(g)
	double z  = 0.0;        // 1D position along n_z
	double vz = 0.0;        // 1D velocity along n_z
	double az = 0.0;        // 1D acceleration along n_z
	// ------------ Book-keeping / flags ------------
	bool in_elevator = false;
	Eigen::MatrixXd P;
	double time;
};

• 代码里的 State 与论文的状态向量结构对应：前五项构成 $x_{rel}$，最后三个标量构成 $x_{elev}$。
  • 有一处需要特别说明：源码中电梯子状态保存的是本段相对位移，而不是世界系下的绝对高度。进入模式时三个量被清零（见 2-8 节的 ElevatorModeEnter），退出时再整体并入 p(2)，因此全局高度恒等于 p(2) + z。
  • 坐标系口径也要先对齐：源码里 q、p、v 的注释写作 world，但在电梯运行期间它们表达的是轿厢系下的量，对应论文的 $R_I^E$、$p_I^E$、$v_I^E$。代码里的 world 指"初始化时刻与世界系重合、之后随轿厢竖直平移"的参考系，地图也建在这个系里；世界系下的高度要在发布时叠加 $z$ 才成立。
• 源码把状态维度统一为 21 维，比论文正文写的 18 维多出重力三项。后文凡是两套维度同时出现的地方，论文一律记作 18 维误差状态，源码一律记作 21 维。online_gravity_estimation_enable 默认关闭时，雅可比与噪声通道上的重力块都不会被填充，重力块在传播中保持常数，等价于论文"重力固定"的处理；打开该开关才会真正在线估计重力。

2-5 非惯性环境下的 IMU 状态传播

• 以防你忘记，在 ESKF（误差状态卡尔曼滤波）中，状态通常分为两部分：
  • 名义状态（Nominal State）：系统当前对真实状态的最佳估计，例如位置、姿态、速度和 IMU 零偏。
  • 误差状态（Error State）：描述名义状态与真实状态之间偏差的小量，用于滤波器修正。

2-5-1 名义状态更新

• 忽略轿厢转动后，角速度合成只剩一项：
  $${}^I{\omega }_W^I={}^I{\omega }_E^I+{}^I{\omega }_W^E\approx {}^I{\omega }_I^E$$
• 非惯性系下的 IMU 量测模型为
  $${\omega }_m={}^I{\omega }_E^I+b_{\omega }+n_{\omega },a_m={\left(R_I^E\right)}^T\left(a_I^E+a_{Ez}^W{\mathbf{e}}_3-g\right)+b_a+n_a$$
• 由量测模型反解出轿厢系下的有效相对加速度，它同时出现在名义状态传播与雅可比中：
  $$a_{I,k}^E=R_{I,k}^E\left(a_{m,k}-b_{a,k}\right)+g-a_{Ez,k}^W{\mathbf{e}}_3$$
• 离散名义传播为
  $$\begin{array}{rl}{p}_{I,k+1}^E& =p_{I,k}^E+v_{I,k}^E\Delta t+\frac{1}{2}{a}_{I,k}^E\Delta t^2\\ q_{I,k+1}^E& =q_{I,k}^E\otimes \mathrm{Exp}\left(\left({\omega }_{m,k}-b_{\omega ,k}\right)\Delta t\right)\\ v_{I,k+1}^E& =v_{I,k}^E+a_{I,k}^E\Delta t\\ p_{Ez,k+1}^W& =p_{Ez,k}^W+v_{Ez,k}^W\Delta t+\frac{1}{2}{a}_{Ez,k}^W\Delta t^2\\ v_{Ez,k+1}^W& =v_{Ez,k}^W+a_{Ez,k}^W\Delta t\\ a_{Ez,k+1}^W& =a_{Ez,k}^W\end{array}$$

说人话：通过上式，我们可以将 IMU 测量到的加速度分解为机器人相对于轿厢的运动加速度和电梯自身的运输加速度，并分别传播两部分状态。这样就能避免将电梯的竖直运输运动错误地积分为机器人自身的运动，实现非惯性环境下的状态解耦。

2-5-2 误差状态更新

• 误差状态取 18 维，机器人部分 15 维、电梯部分 3 维：
  $$\delta x={\left[\delta x_r^T,\delta x_e^T\right]}^T\in {\mathbb{R}}^{18},\delta x_r={\left[(\delta p_I^E{)}^T,(\delta {\theta }_I^E{)}^T,(\delta v_I^E{)}^T,\delta b_a^T,\delta b_{\omega }^T\right]}^T$$
• 过程噪声按五个通道建模，其中电梯加速度按随机游走处理：
  $$Q_{w,k}=\mathrm{diag}\left({\sigma }_a^2{I}_3,{\sigma }_{\omega }^2{I}_3,{\sigma }_{b_a}^2{I}_3,{\sigma }_{b_{\omega }}^2{I}_3,{\sigma }_{a_E}^2\right)\Delta t$$
• 误差状态转移写成 $\delta x_{k+1}=F_{x,k}\delta x_k+F_{w,k}{w}_k$，协方差随之传播：
  $$P_{k+1}=F_{x,k}{P}_k{F}_{x,k}^T+F_{w,k}{Q}_{w,k}{F}_{w,k}^T$$
• $F_{x,k}$ （误差状态转移矩阵）按 $F_{rr}$、$F_{re}$、$F_{ee}$ 三块组织。$F_{rr}$ 是标准惯性导航的 15 维转移矩阵，$F_{re}$ 描述运输加速度对机器人位置与速度的反向耦合，$F_{ee}$ 是常加速度模型：
  $$F_{re}=\left[\begin{array}{cc}{0}_{3\times 2}& -\frac{1}{2}{\mathbf{e}}_3\Delta t^2\\ 0_{3\times 2}& -{\mathbf{e}}_3\Delta t\\ 0_{9\times 2}& 0_{9\times 1}\end{array}\right],F_{ee}=\left[\begin{array}{ccc}1& \Delta t& \frac{1}{2}\Delta t^2\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right]$$
• 那两个负号就是有效相对加速度里 $-a_{Ez}^W{\mathbf{e}}_3$ 的直接体现：运输加速度每增加一点，机器人相对轿厢的位置与速度就少一点。

说人话：通过误差状态转移矩阵，滤波器就能将这种影响传递到下一时刻，并同步更新状态估计的不确定性。

• 把上述公式落到 IMUProcess::predictOnce，可以逐行对照。

// src/estimator/IMUProcess.cpp
State IMUProcess::predictOnce(const State &prev, const Eigen::Vector3d &acc_b,
                              const Eigen::Vector3d &gyr_b, double dt, double stamp) const
{
	State nxt = prev;
	nxt.time = stamp;

	/* === 1. 姿态更新（四元数左乘） === */
	Eigen::Vector3d dtheta = (gyr_b - prev.bw) * dt;     // 去 gyro-bias
	double th = dtheta.norm();
	Eigen::Quaterniond dq(1.0, 0.0, 0.0, 0.0);
	if (th > 1e-12) {
		Eigen::Vector3d axis = dtheta / th;
		double half = 0.5 * th;
		dq.w()   = std::cos(half);
		dq.vec() = axis * std::sin(half);
	}
	nxt.q = (prev.q * dq).normalized();

	/* === 2. 世界系线加速度 === */
	Eigen::Vector3d acc_w = nxt.q * (acc_b - prev.ba) + prev.g;
	double elevator_acc = prev.az;

	if (prev.in_elevator) {
		const Eigen::Vector3d ezu = ElevatorProcess::ezu_from_g(prev.g);
		if (elevator_strong_prior_enable) {
			Eigen::Matrix<double, 1, StateIndex::STATE_TOTAL> H;
			H.setZero();
			H(0, StateIndex::AZ) = 1.0;

			Eigen::Matrix<double, 1, 1> r;
			r(0) = ezu.dot(acc_w) - nxt.az;

			Eigen::Matrix<double, 1, 1> R;
			R(0, 0) = elevator_strong_prior_acc_var;
			ekfUpdate<1>(nxt, H, r, R);

			acc_w = nxt.q * (acc_b - nxt.ba) + nxt.g;
			elevator_acc = nxt.az;
		}
		acc_w -= elevator_acc * ezu;
	}

	/* === 3. 位置 & 速度 === */
	nxt.p = prev.p + prev.v * dt + 0.5 * acc_w * dt * dt;
	nxt.v = prev.v + acc_w * dt;
	if (prev.in_elevator) {
		nxt.z  = prev.z + prev.vz * dt + 0.5 * elevator_acc * dt * dt;
		nxt.vz = prev.vz + elevator_acc * dt;
	}

	/* === 4. 构建连续时间雅可比 A 和 噪声耦合 U === */
	Eigen::Matrix<double, STATE_TOTAL, STATE_TOTAL> A;
	A.setZero();

	// A(δθ,δθ)
	A.block<3,3>(R, R) = -skew3d(gyr_b - prev.bw);
	// A(δθ,δbw)
	A.block<3,3>(R, BW) = -Eigen::Matrix3d::Identity();
	// A(δp,δv)
	A.block<3,3>(P, V) = Eigen::Matrix3d::Identity();
	// A(δv,δθ)
	A.block<3,3>(V, R) = nxt.q.toRotationMatrix() * -skew3d(acc_b - prev.ba);
	// A(δv,δba)
	A.block<3,3>(V, BA) = -nxt.q.toRotationMatrix();
	// A(δv,δg)
	if (online_gravity_estimation_enable) {
		A.block<3,3>(V, G) = Eigen::Matrix3d::Identity();
	}

	/* --- U (源码注释仍写作 18×15) --- */
	Eigen::Matrix<double, STATE_TOTAL, NOISE_TOTAL> U;
	U.setZero();
	U.block<3,3>(R, GYRO_NOISE)   = -Eigen::Matrix3d::Identity();           // gyro-noise
	U.block<3,3>(V, ACC_NOISE)   = -nxt.q.toRotationMatrix();              // acc-noise
	U.block<3,3>(BW, GYRO_RANDOM_WALK) =  Eigen::Matrix3d::Identity();     // gyro-bias random walk
	U.block<3,3>(BA, ACC_RANDOM_WALK) =  Eigen::Matrix3d::Identity();      // acc-bias random walk
	if (online_gravity_estimation_enable) {
		U.block<3,3>(G, GRAVITY_NOISE) = Eigen::Matrix3d::Identity();
	}

	/* === 5. 离散化：F ≈ I + A·dt === */
	Eigen::Matrix<double, STATE_TOTAL, STATE_TOTAL> F =
		Eigen::Matrix<double, STATE_TOTAL, STATE_TOTAL>::Identity() + dt * A;

	/* === 6. 根据是否在电梯中填充 F 和 U 矩阵 === */
	if (prev.in_elevator) {
		const double dt2 = dt * dt;
		F.block<3,1>(P, AZ) = -Eigen::Vector3d(0, 0, 0.5 * dt2);
		F.block<3,1>(V, AZ) = -Eigen::Vector3d(0, 0, dt);
		F(Z, VZ) = dt;
		F(Z, AZ) = 0.5 * dt2;
		F(VZ, AZ) = dt;
		U(AZ, ELEVATOR_ACC_NOISE) = 1.0;
	}

	/* === 7. 协方差传播 === */
	nxt.P = F * prev.P * F.transpose() + U * Q_ * U.transpose() * dt;
	return nxt;
}

• 第 6 步的六行赋值正是 $F_{re}$ 与 $F_{ee}$ 的代码形态，与论文中该矩阵右下角的两块完全对应。
• 第 2 步里还有一条可选支路，对应配置项 strong_prior_enable：当雷达水平安装、轿厢内俯仰方向几乎没有几何约束时，把 IMU 竖直比力直接当作运输加速度的观测，以 1 维 EKF 的形式写进 az。它换来的是竖直方向的约束，代价是默认假设设备相对轿厢没有明显的上下起伏。
• 模式切换时 $F$ 的填充是条件性的：不在电梯里，F.re、F.ee 与电梯噪声通道都不被赋值，矩阵退化为标准 LIO 的 21 维形式。图 4 用分块颜色把这个差异画了出来。
• 源码采用一阶离散化 $F\approx I+A\Delta t$，因此位置行没有论文雅可比中的二阶项 $-\frac{1}{2}R\lfloor {\hat{a}}_k{\rfloor }_{\wedge }\Delta t^2$。两者的差别在 200 Hz 量级的 IMU 频率下可以忽略，实现上省掉了两次矩阵乘法。

2-6 电梯模式管理与事件检测

• 在引入电梯运输状态后，Elevator-LIO 还需要判断机器人当前是否处于电梯环境中，并根据不同场景切换状态估计模式。
  • 当机器人进入电梯时，启用非惯性运动模型，对机器人相对运动与电梯运输状态进行联合估计；
  • 当机器人离开电梯后，则退出电梯模式，恢复标准 LIO 的状态估计流程。
• 接下来，我们来看看 Elevator-LIO 是如何检测电梯环境并触发模式切换的。

2-6-1 模式管理器

• 模式管理器的整体设计： 模式管理器对外只暴露两个信号：Flag_Entry 与 Flag_Exit，分别表示进入和退出电梯。内部检测逻辑与状态估计模块相互解耦，因此未来也可以替换为视觉识别、语义检测或楼宇物联网信号等其他检测方式。

2-6-2 进入电梯检测

• 进入检测主要利用轿厢内部空间相对封闭的几何特征。当机器人驶入轿厢时，随着电梯门逐渐关闭，激光雷达能够观测到的空间范围会逐渐收缩，最终形成四周相对封闭的点云结构。因此，可以通过点云距离的稳健最大值判断机器人是否已经进入电梯：
  $$F_{\mathrm{entry}}=\{\begin{array}{ll}1,& d_{\max }\left(t\right)<d_{th},\forall t\in \left[t_0,t_0+\Delta t_{\mathrm{door}}\right]\\ 0,& \text{otherwise}\end{array}$$
• 为避免少量穿门缝的远点、玻璃反射等因素造成误判，$d_{\max }$ 并不直接取所有点的最大距离，而是取所有有效点三维欧氏距离的 94 分位数。论文设置 $d_{th}=3.0\mathrm{m}$、$\Delta t_{\mathrm{door}}=2.0\mathrm{s}$，只有在整个时间窗口内持续满足距离条件，才会触发 Flag_Entry。

说人话：当机器人进入电梯并且电梯门关闭后，激光雷达观测到的周围环境会变得相对封闭，点云距离也会被压小。Elevator-LIO 利用这一几何特征，统计点云三维距离的 94 分位数，并要求它连续 2 秒小于 3 米，才判定机器人已经进入电梯，触发 Flag_Entry。

2-6-3 退出电梯检测

• 与进入检测不同，退出检测不再依赖点云几何，而是利用电梯运输过程中的运动特征。一次典型的电梯行程通常包含加速、匀速和减速三个阶段，因此可以通过监测电梯竖直速度的变化情况，判断电梯是否完成运输。
  $${\sigma }_{v_E}^2\left(t\right)=\frac{1}{K}\sum _{i=0}^{K-1}{\left(v_{Ez}^W\left(t-i\right)-{\bar{v}}_{Ez}^W\left(t\right)\right)}^2,{\bar{v}}_{Ez}^W\left(t\right)=\frac{1}{K}\sum _{i=0}^{K-1}{v}_{Ez}^W\left(t-i\right)$$
• 方差在两个加减速阶段各出现一次峰值，中间匀速段回落。有限状态机据此跟踪整趟行程，当两个峰值之后方差与竖直速度同时低于阈值并保持足够长时间，判定停稳并发出 Flag_Exit。

说人话：电梯运行时通常会经历启动加速、匀速运行和减速停止三个阶段。Elevator-LIO 通过计算竖直速度在一段时间内的方差，识别启动和停止时的两个变化峰值。当两个峰值都出现，并且电梯竖直速度与速度方差连续一段时间保持较低水平时，就认为电梯已经停稳，触发 Flag_Exit，退出电梯模式，恢复标准 LIO。

2-6-4 代码实现

// src/elevator/ElevatorProcess.cpp
void ElevatorProcess::processDoorDetector(const PointCloudXYZI& cloud,
                                          double timestamp,
                                          const State& state,
                                          bool lidar_process_active)
{
	if (!elevator_enable || !elevator_door_detector_enable) return;
	if (!lidar_process_active) return;

	std::vector<double> distances;
	distances.reserve(cloud.points.size());
	for (const auto& p : cloud.points) {
		if (p.x == 0.0f && p.y == 0.0f && p.z == 0.0f) continue;
		distances.push_back(std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z));
	}
	if (distances.empty()) return;

	std::sort(distances.begin(), distances.end());
	int index = static_cast<int>(distances.size() * (1.0 - elevator_door_filter_percent));
	if (index < 0) index = 0;
	if (index >= static_cast<int>(distances.size())) index = static_cast<int>(distances.size()) - 1;
	const double filtered_max_d = distances[static_cast<size_t>(index)];

	const bool door_closed_like = filtered_max_d > 0.1 && filtered_max_d < elevator_door_dist_threshold;
	door_low_distance_active_ = door_closed_like;

	if (!state.in_elevator && !ELEVATOR_TRIGGER) {
		if ((timestamp - door_last_enter_trigger_time_) < elevator_door_cooldown_time) return;

		if (door_closed_like) {
			if (door_low_dist_start_time_ < 0.0) door_low_dist_start_time_ = timestamp;
			if (!door_closed_triggered_ &&
			    (timestamp - door_low_dist_start_time_) >= elevator_door_time_threshold) {
				ELEVATOR_TRIGGER = true;
				door_closed_triggered_ = true;
				door_last_enter_trigger_time_ = timestamp;
			}
		} else {
			door_low_dist_start_time_ = -1.0;
			door_closed_triggered_ = false;
		}
	}
}

• filter_percent 取 0.06 时，排序后取第 94 百分位，正好忽略最远端的 6% 点。door_last_enter_trigger_time_ 与 cooldown_time 一起防止同一次乘梯被重复触发，!state.in_elevator && !ELEVATOR_TRIGGER 则保证模式已经生效时不再受理新的进门事件。
• 停稳检测的状态机单独放在 ElevatorSelfExit 中，五个状态的迁移条件由速度与滑窗方差共同决定。

// src/elevator/ElevatorSelfExit.cpp
	const double kVelThresh      = 0.2;
	const double kStopVelProtect = 0.5;
	const double kStdHigh        = 0.01;
	const double kStdLow         = 0.01;
	const double kConfirmHoldS   = 1.5;

	switch (ele_state_) {
	case IDLE:
		if (abs_vel > kVelThresh && current_stdDev > kStdHigh) {
			ele_state_ = ACCELERATING;
		}
		break;

	case ACCELERATING:
		if (abs_vel > kVelThresh && current_stdDev < kStdLow) {
			ele_state_ = CONSTANT_SPEED;
		}
		break;

	case CONSTANT_SPEED:
		if (current_stdDev > kStdHigh) {
			ele_state_ = DECELERATING;
		}
		break;

	case DECELERATING:
		if (current_stdDev < kStdLow && abs_vel < kStopVelProtect) {
			ele_state_ = STOP_CHECK;
			stable_tick_ = 0;
			low_var_hold_s_ = 0.0;
			stop_check_lidar_zupt_count_ = 0;
			stop_check_confirmed_ = false;
		}
		break;

	case STOP_CHECK:
		if (current_stdDev < kStdLow && abs_vel < kStopVelProtect) {
			stable_tick_++;
			low_var_hold_s_ += eval_dt;
			if (low_var_hold_s_ >= kConfirmHoldS && !stop_check_confirmed_) {
				stop_check_confirmed_ = true;
				stop_check_lidar_zupt_count_ = 0;
				result.stop_confirmed = true;
			}
		} else {
			low_var_hold_s_ = 0.0;
			stop_check_confirmed_ = false;
			if (current_stdDev > kStdHigh) {
				ele_state_ = DECELERATING;
			}
		}
		break;
	}

• 状态机在第一阶段要求方差同时高于阈值、速度同时超过 0.2 m/s，才认为电梯真的开始加速，避免把机器人自身的轻微起伏当成电梯启动。
• 当前源码里 kStdHigh 与 kStdLow 取同一数值 0.01，等效于单阈值判据：严格大于进入加减速阶段，严格小于才进入匀速与停稳判定，等于时两个分支都不触发，状态保持不变。这两个常量在代码里是分开定义的，需要加重迟滞时直接拉开数值即可。
• 停稳确认后还要等若干帧 LiDAR 的零速更新做完，才真正清掉触发标志，这一段在 2-8 节展开。

2-6-5 特殊情况说明

• 检测器整体是启发式的，极端环境下会失效。图 10 给出了镜像轿厢的例子：反射与多径在点云里造出多个相邻轿厢的伪结构，距离阈值判据会把门当成没关，从而漏掉进入事件。
  

说人话：进入检测依赖点云距离判断轿厢是否封闭，但镜面电梯可能产生激光反射和多径效应，在点云中形成虚假的远距离结构。即使电梯门已经关闭，算法仍可能认为周围存在较大的开放空间，导致进入事件漏检。

• 论文在 79 段标注电梯中自动识别出 76 段，漏检集中在 Campus4 与 Mall1，且漏检序列在评测时改为手动发布触发话题，以便把检测器误差与估计器误差分开。

2-7 模式依赖的 IESKF 与 LiDAR 更新

• 有了电梯进入和离开的判据，接下来就需要根据模式切换结果，调整滤波器的状态传播与 LiDAR 更新方式。Elevator-LIO 保留了原有 IESKF 框架，但针对普通环境和电梯环境采用不同的状态估计模型：
  • 普通模式： 使用标准 LIO 的状态传播与 LiDAR 配准流程；
  • 电梯模式： 使用前面介绍的非惯性运动模型进行状态传播，并在轿厢局部坐标系下进行 LiDAR 配准。

• 电梯模式下的配准发生在轿厢局部系。LiDAR 点先经外参转到 IMU 系，再转到轿厢系：
  $$p_j^I=R_L^I{p}_j^L+p_L^I,p_j^E=R_I^E{p}_j^I+p_I^E$$
• 点到面残差为
  $$r_j\left(x\right)=u_j^T\left(R_I^E\left(R_L^I{p}_j^L+p_L^I\right)+p_I^E-q_j\right)$$
• 残差只含机器人相对状态，运输状态不出现在其中。全局位置由局部位置与运输位移组合得到：
  $$p_I^W=p_I^E+p_{Ez}^W{\mathbf{e}}_3$$
• 对 18 维误差状态求导，雅可比只有位置与姿态两块非零：
  $$H_j=\left[u_j^T,-u_j^T{R}_I^E{\lfloor p_j^I\rfloor }_{\wedge },0_{1\times 12}\right]$$
• 后面 12 维全为零，说明在完全封闭的轿厢里，LiDAR 无法直接约束运输状态。注意这不等于运输状态在数值上被冻结：它们仍在 IMU 前推中演化，只是不确定性持续增长，要等退出阶段的物理约束来收口。这也正是必须设置退出修正的原因。

// src/estimator/ESEKF.cpp
H = Eigen::MatrixXd::Zero(valid_points_num, StateIndex::STATE_TOTAL);
Z.resize(valid_points_num, 1);

for (int effect_i = 0; effect_i < valid_points_num; ++effect_i) {
	const size_t vi = static_cast<size_t>(effective_indices_[static_cast<size_t>(effect_i)]);
	down_effect_cloud_lidar->points.push_back(clouds_lidar.points[vi]);

	const Eigen::Vector3d dr = -norm_vector_[vi].norm_vec.transpose() * R_wb * skew3d(norm_vector_[vi].point_imu_xyz);
	H.block<1,3>(effect_i, 0) = dr.transpose();
	H.block<1,3>(effect_i, 3) = norm_vector_[vi].norm_vec.transpose();
	H.block<1,6>(effect_i, 6).setZero();
	Z(effect_i, 0) = norm_vector_[vi].d;
	effect_meas_inv_var_.push_back(1.0);
}

• 矩阵在循环前已整体置零，因此除了显式填充的姿态块与位置块，速度、两个零偏、重力以及电梯三块都保持为零。论文的 18 维误差状态里这段是 12 维零块，源码因多出重力三项而为 15 维零块，两者内容上一一对应。代码在这里额外显式清零了第 6 到第 11 列，是为了让阅读者一眼看出量测与速度、零偏无关。

• 代码的状态排布是姿态在前、位置在后，与论文量测矩阵的行内顺序互为转置，内容一致。

• 迭代求解采用 IESKF 的最大后验形式。第 $\kappa$ 次迭代的增益、增量与流形修正为
  $$K={\left(H^T{R}^{-1}H+P^{-1}\right)}^{-1}{H}^T{R}^{-1}$$
  $${\hat{x}}^{\kappa +1}={\hat{x}}^{\kappa }⊞\left(-Kr\left({\hat{x}}^{\kappa }\right)-\left(I-KH\right){\left(J^{\kappa }\right)}^{-1}\left({\hat{x}}^{\kappa }\boxminus \bar{x}\right)\right)$$

• 由于状态里只有 $q_I^E$ 一个 SO(3) 分量，流形修正矩阵退化为分块对角阵，其中只有姿态块取 $\mathrm{SO}(3)$ 右雅可比的逆，其余 12 维是单位阵。协方差用 Joseph 形式更新以保证数值稳定：
  $${\hat{P}}_k=\left(I-KH\right)P{\left(I-KH\right)}^T+KR{K}^T$$

// src/estimator/ESEKF.cpp
// 由误差状态构造流形修正矩阵 J_k^{-1}
J_k_inv.setIdentity();
J_k_inv.block<3,3>(StateIndex::R, StateIndex::R) = A_T(error_s.block<3,1>(StateIndex::R, 0));
const Eigen::MatrixXd P_k = J_k_inv * s_next.P * J_k_inv.transpose();

// 迭代增益与增量：K = (HᵀH + P⁻¹·prior_scale)⁻¹ Hᵀ
K = (H_kt * H_k + P_k_inv * kLidarPriorScale).inverse() * H_kt;
Eigen::MatrixXd left  = -1 * K * Z_k;
Eigen::MatrixXd right = -1 * (identity - K * H_k) * J_k_inv * error_s;
Eigen::MatrixXd update_x = left + right;

// 流形注入：姿态右乘，其余加法
s_next.q  = s_next.q.toRotationMatrix() * so3Exp(update_x.block<3,1>(StateIndex::R, 0));
s_next.p  = s_next.p  + update_x.block<3,1>(StateIndex::P, 0);
s_next.v  = s_next.v  + update_x.block<3,1>(StateIndex::V, 0);
s_next.z  = s_next.z  + update_x(StateIndex::Z, 0);
s_next.vz = s_next.vz + update_x(StateIndex::VZ, 0);
s_next.az = s_next.az + update_x(StateIndex::AZ, 0);

• 代码用 kLidarPriorScale 对先验信息矩阵做了缩放，等价于在增益公式的先验项 $P^{-1}$ 上乘一个常数，用来调节单帧 LiDAR 与 IMU 先验的相对权重。
• 由于 $H$ 只有前 6 列非零，仓库另外提供了一条 6 维量测的快速路径：配置项 estimator.lidar_update_mode 取 pose6 时，只构建 6 维的 $H_{\mathrm{pose}}$ 与残差，再通过协方差交叉项把修正分配到完整 21 维状态上；取 full21 时按上面展示的完整形式求解。两种写法代数上等价，pose6 省掉的是大量零元的乘加。

2-8 ZUPT 与电梯退出状态重置

• 当机器人完成乘梯、电梯到达目标楼层并停稳后，Elevator-LIO 就需要退出非惯性运动估计模式，恢复普通 LIO 的状态传播与定位流程。
• 但在电梯模式下，滤波器额外维护了电梯的竖直位移、速度和加速度三个辅助状态。此时不能简单地关闭电梯模式，而是需要先利用电梯停稳这一物理约束修正状态估计，再将累计的运输位移合并到机器人位置中，最后清除电梯辅助状态及其相关协方差。
• 整个过程主要分为三个步骤：零速与零加速度联合更新、运输位移提交、辅助状态与协方差重置。

说人话：电梯模式下额外维护了三个运输状态，因此退出时不能直接清除。需要先利用电梯停稳时速度和加速度为零的约束修正状态，再将累计运输位移合并到机器人位置，最后清除辅助状态及其协方差，恢复普通 LIO。

2-8-1 零速与零加速度联合更新

• ZUPT（Zero-Velocity Update，零速度更新） 是惯性导航中常见的状态修正方法，其核心是利用载体静止时速度为零这一先验约束，修正 IMU 积分产生的速度误差。
• 退出阶段轿厢物理静止，运输速度与加速度都应趋近于零。把这作为高置信度先验，做一次标准的 EKF 更新：
  $$r=\left[\begin{array}{c}0-v_{Ez}^W\\ 0-a_{Ez}^W\end{array}\right],H=\left[\begin{array}{cccc}{0}_{1\times 15}& 0& 1& 0\\ 0_{1\times 15}& 0& 0& 1\end{array}\right]$$
• 观测噪声取 $R=\mathrm{diag}\left(10^{-5},10^{-4}\right)$，用一个极小的方差把零状态"钉"住，同时依靠协方差交叉项把修正传导到机器人位置与偏置上。

说人话：电梯停稳后，算法认为其竖直速度和加速度都应该为零，因此利用这两个约束修正状态估计。同时，滤波器还会通过状态之间的协方差关系，将修正传递给机器人位置和 IMU 偏置，减少乘梯过程中的累计误差。

2-8-2 运输位移提交

• 更新完成后立刻做重锚定：把本段运输位移并入机器人位置状态，再把辅助子状态清零：
  $$p_I^E\leftarrow p_I^E+p_{Ez}^W{\mathbf{e}}_3,p_{Ez}^W\leftarrow 0,v_{Ez}^W\leftarrow 0,a_{Ez}^W\leftarrow 0$$

2-8-3 辅助状态与协方差重置

• 最后把与电梯子状态相关的交叉协方差块置零、对角线重置为小先验值，非惯性阶段累积的不确定性不会污染后续常规导航。整个重置过程可以理解为状态重参数化的最后一步：位移已经被机器人位置吸收，辅助变量在回到惯性模式前退场。

说人话：电梯到站那一刻，"我在这栋楼的第几层"其实已经由这一段的高度积分给出。把这笔账一次性记到机器人位置上，再把电梯那三个临时变量清空，滤波器就干净地回到普通 LIO 的模式。

2-8-4 代码实现

// src/elevator/ElevatorProcess.cpp
// 零速 + 零加速度联合更新，两行量测
Eigen::Matrix<double, 2, StateIndex::STATE_TOTAL> H;
H.setZero();
H(0, StateIndex::VZ) = 1;
H(1, StateIndex::AZ) = 1;
Eigen::Matrix<double, 2, 1> r;
r(0) = -S.vz;
r(1) = -S.az;
Eigen::Matrix<double, 2, 2> R;
R.setZero();
R(0, 0) = 1E-5;  // 速度观测噪声
R(1, 1) = 1E-4;  // 加速度观测噪声
ekfUpdate<2>(S, H, r, R);

// src/node/LidarPipeline.cpp
void LidarPipeline::handleElevatorExit(State& post_state) {
	if (!elevator_enable) return;
	if (!exit_from_elevator_) return;
	exit_from_elevator_ = false;
	EXIT_FROM_ELEVATOR = false;
	elevator_self_exit_.reset();

	const double world_z_before_commit = post_state.p(2);
	const double segment_z_before_zupt = post_state.z;

	if (elevator_zupt_enable) {
		imu_process_.elev_process.applyZeroVelocityUpdateZ(post_state);
	}

	// 位移并入世界位置
	const double delta_z_commit = post_state.z;
	post_state.p(2) += delta_z_commit;

	if (elevator_exit_icp_z_enable) {
		// 可选的仅 z 方向 ICP 精配准，通过后累加到 p(2)
	}

	// 辅助子状态清零，协方差交叉块解耦
	post_state.z = 0;
	post_state.vz = 0;
	post_state.az = 0;
	imu_process_.elev_process.ElevatorModeExit(post_state);
	imu_process_.elev_process.finalizeExitCovariance(post_state);
	estimator_->set_cur_state(post_state);
}

• 执行顺序是零速更新、位移提交、可选 ICP、子状态清零，四步不能交换：先更新是为了让本段位移本身被修正，先提交是为了让 p(2) 拿到最终高度，最后清空与解耦是为了让后续传播不再看到这些变量。
• 两处协方差重置的数值并不相同。ElevatorModeEnter 把三个对角元设为 $10^{-4}$、$10^{-4}$、$10^{-3}$，ElevatorModeExit 设为 $10^{-4}$、$10^{-3}$、$10^{-2}$。退出时给加速度留了更大的先验方差，下一段行程重新开始时由 IMU 数据把它收缩回来。

2-8-5 加强项

• 仓库还提供两个可选的加强项，默认关闭：
  • elevator.exit_icp_z：退出后用当前帧与已有地图做仅竖直方向的 ICP（Iterative Closest Point，迭代最近点）精配准，修正量必须小于阈值且 z 方向信息足够才被接受。
  • elevator.rebuild_map_on_exit：退出后累计若干帧重建 ikd-Tree，把轿厢内插入的几何清掉。
• 论文对毫米级终端误差的解读值得注意：它来自"局部非惯性估计"与"历史地图对齐"的共同作用，而不是说整段乘梯过程都保持着亚厘米的绝对高度精度。

2-9 自适应体素降采样

• 固定分辨率体素在两种场景下都不合适：狭窄轿厢里体素太大，几何细节被抹掉；开阔大厅里体素太小，冗余点把实时性拖垮。Elevator-LIO 用反馈控制在线调整体素尺寸，让本帧有效点数逼近目标值：
  $$v_{t+1}=v_t{\left(\frac{N_t}{N_{\mathrm{target}}}\right)}^{1/\alpha }$$
• 幂律的来由是点数与分辨率之间的非线性关系：均匀分布在体积中时 $N\propto v^{-3}$，分布在平面上时 $N\propto v^{-2}$，稀疏的激光束采样还会再把有效指数压得更低。真实场景是混合结构，于是用经验指数 $\alpha$ 统一刻画，实现中取 $\alpha =1.2$。
• 为应对进出轿厢时的突变，体素尺寸被夹在上下界之间：
  $$v_{t+1}\in \left[v_{\min },v_{\max }\right],v_{\min }=0.05\mathrm{m},v_{\max }=0.8\mathrm{m}$$

说人话：Elevator-LIO 不再使用固定大小的体素，而是根据当前帧降采样后的点数，动态调整体素尺寸：点数太多就增大体素，减少点云数量；点数太少就缩小体素，保留更多几何细节。同时，通过设置体素尺寸的上下限，避免进出电梯时场景突变导致降采样尺度剧烈变化，从而兼顾点云质量与计算效率。

// include/AdaptiveFilter/AdaptiveVoxelPController.hpp
double update(std::size_t n_points) {
	if (cfg_.alpha <= 0.0 || cfg_.target_points <= 0.0) return voxel_;
	const double ratio = static_cast<double>(n_points) / cfg_.target_points;
	const double scale = std::pow(ratio, 1.0 / cfg_.alpha);  // (N_t / N_target)^{1/alpha}
	voxel_ = voxel_ * scale;
	clamp_state_();                                          // 夹到 [min_voxel, max_voxel]
	return voxel_;
}

• 目标点数在配置里写的是每秒点数，运行时会按输入时间戳折算到每帧，这样更换 LiDAR 型号或帧率时不必重调参数。LidarPipeline::updateAdaptiveConfig 负责估计平滑后的帧间隔，并对突变做钳制，避免偶发的掉帧把体素一把推到底。
• 反馈项的实际效果可以这样理解：输入点数偏多时比值大于一，体素被放大；点数偏少时比值小于一，体素被收缩。由于每次只按幂律走一步，控制器对小波动不敏感，对进出轿厢这种量级跳变又能在几帧内跟上。

• 另有一个 AdaptiveVoxelFilter，按距离分区域配置不同的叶子尺寸（近处密、远处疏），与上面的幂律控制器是两条独立的实现路径，可按场景二选一。

2-10 地图构建与 ROS 工程实现

• 建图是状态估计的下游。IESKF 收敛后，当前帧的点先转到轿厢局部系做配准，再投影到世界系插入地图：
  $$p_j^E=R_I^E\left(R_L^I{p}_j^L+p_L^I\right)+p_I^E,p_j^W=R_I^E\left(R_L^I{p}_j^L+p_L^I\right)+p_I^E+p_{Ez}^W{\mathbf{e}}_3$$
• 地图后端使用 ikd-Tree，它在增量插入、删除与最近邻查询上都有专门的实现，适合多楼层大范围建图。

// src/node/map.cpp
void LIONode::map_incremental(PointCloudXYZI & lidar_clouds) {
	if (lidar_clouds.empty()) return;
	if (relocation_enable) return;
	if (!ikd_map) return;
	PointCloudXYZI::Ptr world_cloud = p_eskf_estimator->transLidar2World(lidar_clouds);
	ikd_map->add(world_cloud);
}

• 里程计输出需要在发布时补上运输位移，世界高度由局部位置与当前段位移相加得到。

// src/node/publish.cpp
odom_imu.pose.pose.position.x = state.p(0);
odom_imu.pose.pose.position.y = state.p(1);
odom_imu.pose.pose.position.z = state.p(2);
if (state.in_elevator) {
	odom_imu.pose.pose.position.z += state.z;   // 全局高度 = 局部位置 + 本段运输位移
}

• 这里有一处值得注意的实现差异。论文给出的是完整的全局投影；代码中 transLidar2World 只使用 state.p，全局高度是在发布（publish.cpp）与退出提交（LidarPipeline.cpp）两处叠加的。也就是说，电梯运行期间插入地图的点落在进入楼层的高度上，退出后才按新高度继续。论文为此设置了 Map Reset 消融（每到一个新楼层清空并重建 ikd-Tree），用来剥离历史地图重访带来的修正，单独评估非惯性前推与退出修正的作用；elevator.rebuild_map_on_exit 开关的作用与它一致。
• 整条调用链可以概括为：

main.cpp 定时器轮询缓冲
  └─ LidarPipeline::process
       ├─ [1] dequeueFrame                  缓冲出队
       ├─ [2] updateAdaptiveConfig          更新体素目标
       ├─ [3] handleFirstLidarInit          前若干帧只建初始地图
       ├─ [4] handlePostElevatorRebuild     退出后的地图重建（可选）
       ├─ [5] resolveFrameState             查询状态 + processDoorDetector
       ├─ [6] undistortFrame                电梯模式下跳过去畸变
       ├─ [7] downsampleFrame               自适应体素降采样
       ├─ [8] runIeskfUpdate                IESKF 迭代更新
       ├─ [9] handleElevatorStop            停稳确认后的 LiDAR 侧 ZUPT
       ├─ [10] handleElevatorExit           零速更新 + 重锚定 + 协方差重置
       ├─ [11] commitFrame                  状态回写、地图插入、缓冲出队
       └─ [12] 结果返回                     交由调用方发布

• 第 6 步的判据是 in_elevator && !exit_from_elevator_，也就是电梯模式内部直接跳过运动去畸变。这一点在实际使用中要留意：若机器人在轿厢内快速移动，单帧内的点云形变不会被补偿，此时应降低移动速度或依赖迭代更新的重新匹配。

• 电梯状态的对外接口除了里程计，还有专用话题 /LIO/elevator_state，发布模式标志、本段位移、速度与加速度，RViz 面板直接消费这些字段做可视化。

2-11 实验结果与性能分析

• 真实序列没有完整的轨迹真值，评测以竖直一致性为主，终端高度误差定义为$$e_z=z_{end}-z_{ref}$$其中：
  • $z_{end}$ 是末时刻的估计高度
  • $z_{ref}$ 是对应物理楼层的参考高度。对于回到原点或同层重访的序列取 $z_{ref}=0$，指标退化为 $e_z=z_{end}$；另有一部分序列通过电梯与楼梯两条路线到达同一楼层，构成交叉验证。
• 全部实验使用单台 Livox MID-360，自适应降采样目标 20000 点每秒、$\alpha =1.2$、体素范围 0.05 m 至 0.8 m，ikd-Tree 分辨率 0.1 m，进门检测距离阈值 3 m、分位数 94。

2-11-1 终端高度误差与消融

• 消融含四个变体：完整系统、去掉退出零状态更新（No ZUPT）、到达新楼层后清空重建地图（Map Reset）、关闭自适应前端改用固定 0.2 m 体素（w/o Adapt.）。
• 三组消融给出三条清晰的结论。
  • 去掉退出零状态更新后，20 条序列中多数误差升到 0.3 m 以上，其中 Office4 达到 3.074 m。这一项是抑制竖直漂移的主项，没有它，运输状态的不确定性只能靠 IMU 前推一路累积。
  • 清空并重建地图后误差整体仍保持在较小范围内，多数落在 0.1 m 至 0.4 m，最大绝对误差 0.662 m。这说明非惯性前推与退出修正在没有历史地图辅助的条件下也成立；该变体剥离了重访旧楼层时地图带来的修正，是更接近"纯估计器"的评价。
  • 关闭自适应前端后，Office6、Dormitory4 与 campus2 三条直接发散，误差越过 10 m。固定 0.2 m 体素在开阔大厅丢细节、在狭窄轿厢里也未必合身，进出轿厢时的几何约束质量因此明显下降。
• 完整系统在 20 条序列中有 17 条终端误差低于 1 cm，其余三条分别是 Office5（0.117 m）、Office7（-0.136 m）与 campus3（-0.499 m）。campus3 只有一次乘梯且电梯占比仅 8.5%，属于电梯贡献很小的序列。
• 检测器在 79 段标注电梯中识别出 76 段，漏检集中在 campus4（1/3）与 Mall1（6/7），共 3 段，全部来自镜像反射与多径生成的伪结构。

说人话：消融实验表明，退出时的零状态更新是抑制竖直漂移的关键，自适应降采样则保证了不同场景下的点云约束质量。同时，即使清空历史地图，算法仍能保持较好的高度估计精度，说明其核心能力来自非惯性状态建模与退出修正，而不完全依赖地图匹配。检测器则能识别 79 次乘梯中的 76 次，主要漏检原因是镜面反射与多径效应。

2-11-2 长距离垂直运输与大范围建图

• 图 8 展示了大范围多楼层序列，右下角数字是各条轨迹的终点世界坐标。这一类序列考验的是长轨迹下跨楼层地图的一致性，电梯只占其中一部分，但对全局高度误差的贡献最大。
  

• 图 9 是宿舍楼的长距离竖直往返。Dormitory4 连续乘梯 13 次，电梯时间占比 88.7%，累计竖直行程 138.9 m，最后仍回到接近初始高度，终端误差 -0.007 m。这一条是对"反复上下楼后高度是否还能收敛"最直接的检验。

• Office 与 Campus 的序列以水平长轨迹与跨楼层地图一致性为主，其中 campus2 轨迹长度达 893.9 m；Mall 的序列强调开放公共空间中行人干扰下的稳定性。两类场景下系统都保持稳定，包括轿厢内有剧烈晃动的手持序列。

2-11-3 标准惯性环境与扰动鲁棒性

• 在 Hilti 2022 与 Hilti 2023 上，平台不在电梯里时电梯相关状态块与噪声通道全部关闭，估计器走标准惯性传播与 LiDAR 更新。这一组实验检验的是：改造后的 LIO 子系统是否仍具备通用性。
  • 17 条序列上，Elevator-LIO 的误差中位数为 0.063 m，与 Faster-LIO、Point-LIO、FAST-LIO2 处于同一量级；在 Underground 1、Underground 4、11 Lower Gallery、21 Outside Building 与 14 Basement 2 五条上取得最好或次好成绩。这说明模式相关的设计并没有破坏常规惯性环境下的表现。
  • 表现不如其他方法的两条集中在窄楼梯与跨楼层过渡区域：Hilti2023 的 Stair 为 1.262 m，Hilti2022 的 07 Long Corridor 为 1.019 m。论文给出的解释是这类区域点云分布突然收缩、台阶与扶手产生重复结构，配准本身存在多解；这两处的失败模式与电梯场景无关，属于通用 LIO 的固有难点。
• 真实电梯的运动状态难以受控，模型失配的鲁棒性通过定制仿真器评估。三组扰动分别对应重力漂移步长 0.10 度、0.30 度、0.80 度，轿厢倾角 0.30 度、1.00 度、3.00 度，以及初始重力失配 0.50 度、1.50 度、4.00 度，另有一组三者同时施加的组合扰动。
• 四类扰动的终端误差都随幅度单调增大而有界：时变重力漂移 0.012 m 至 0.118 m，轿厢倾角误差 0.009 m 至 0.094 m，初始重力失配 0.015 m 至 0.167 m，组合扰动 0.024 m 至 0.312 m，最大档也没有发散。这说明把电梯建模为一维平移、并把重力固定下来这两个近似，在实际偏差范围内是可用的。

说人话：实验表明，关闭电梯专用状态后，Elevator-LIO 在普通环境下仍能保持与主流 LIO 相当的性能，没有因为电梯模型的引入而明显影响常规定位。同时，即使存在重力估计偏差、轿厢倾斜等模型失配，终端误差也只是随扰动增大而逐渐上升，并未发生发散，说明算法具有一定的扰动鲁棒性。

---

3 部署指南

3-1 环境与依赖

• 同一套源码通过 include/support/ros_compat.h 兼容两套中间件，支持以下组合：

系统	ROS 版本	说明
Ubuntu 20.04	ROS 1 Noetic	包内自带 Livox 消息定义
Ubuntu 22.04	ROS 2 Humble	依赖外部 livox_ros_driver2 的 CustomMsg

• 主要依赖为 PCL、Eigen 与 OpenCV。OpenCV 只服务于协方差矩阵与电梯状态曲线的调试窗口，默认配置下不弹出；仿真节点额外依赖 nlohmann-json3-dev。
• ARM64 平台需要在目标机上原生编译 Livox-SDK2、livox_ros_driver2 与本工程，不能复用 x86_64 的预编译库。

3-2 编译

• ROS 1 按标准 catkin 流程编译：

ROS1_WS=~/catkin_ws
cd "$ROS1_WS"
source /opt/ros/noetic/setup.bash
rosdep install --from-paths src --ignore-src -r -y
catkin_make -DCMAKE_BUILD_TYPE=Release
source devel/setup.bash

• ROS 2 需要先构建驱动，再构建本工程：

LIVOX_WS=~/ws_livox_ros_driver2
cd "$LIVOX_WS/src/livox_ros_driver2"
source /opt/ros/humble/setup.bash
./build.sh humble

LIO_ROS2_WS=~/ws_elevator_lio_ros2
cd "$LIO_ROS2_WS"
source /opt/ros/humble/setup.bash
source "$LIVOX_WS/install/setup.bash"
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

• 注意：驱动必须先从自身目录执行 ./build.sh humble 切到 ROS 2 的 package.xml，否则裸 colcon build 会因为清单仍是 ROS 1 状态而失败。

3-3 关键参数

• 电梯相关参数集中在 yaml/runtime/mapping.yaml 的 elevator 段，降采样参数在 downsample 段。

参数	默认值	含义
elevator.enable	true	电梯处理总开关，关闭后退化为普通 LIO
elevator.self_exit_detector	true	依据 IMU 运动阶段自动退出电梯模式
elevator.door_detector.dist_threshold	3.0	封闭判据的距离阈值，单位 m
elevator.door_detector.time_threshold	2.0	封闭状态需持续的时间，单位 s
elevator.door_detector.filter_percent	0.06	计算最远距离时忽略的远端比例
elevator.zupt.enable	true	电梯相关零速更新总开关
elevator.zupt.waiting.period_s	3.0	等待阶段周期 ZUPT 的间隔
elevator.exit_icp_z.enable	false	退出后是否做仅 z 方向 ICP
elevator.rebuild_map_on_exit.enable	false	退出后是否重建 ikd-Tree
downsample.adaptive.target_points	20000	目标每秒点数
downsample.adaptive.alpha	1.2	幂律指数
downsample.adaptive.min_voxel	0.05	体素下界，单位 m
downsample.adaptive.max_voxel	0.8	体素上界，单位 m
estimator.lidar_update_mode	pose6	LiDAR 更新走 6 维快速路径或 21 维完整路径
estimator.online_gravity_estimation_enable	false	是否在线估计重力

• 调参的基本规律是：dist_threshold 过小，大轿厢可能触发不了；过大，窄走廊与靠墙场景会误触发。time_threshold 过短容易被瞬时遮挡骗到。filter_percent 过大则可能把真实的开门方向也忽略掉。

3-4 运行与话题

• ROS 1 与 ROS 2 的启动方式分别为：

roslaunch lio start.launch
roslaunch lio start.launch config_path:=path_to_yaml

ros2 launch lio start_ros2.launch.py
ros2 launch lio start_ros2.launch.py config_path:=path_to_yaml use_rviz:=false

• 主要话题如下：

话题	类型	说明
/LIO/odom_imu	nav_msgs/Odometry	IMU 系里程计，全局高度已在其中
/LIO/odom_vehicle	nav_msgs/Odometry	车体系里程计
/LIO/clouds_lidar	sensor_msgs/PointCloud2	当前帧点云
/LIO/global_map	sensor_msgs/PointCloud2	全局地图
/LIO/elevator_state	自定义消息	电梯模式、位移、速度、加速度
/LIO/set_elevator_flag	std_msgs/Bool	手动触发或取消电梯模式

3-5 常见问题

• 镜像轿厢、透明玻璃或超大货梯导致进门漏检时，直接向 /LIO/set_elevator_flag 发布 true，把检测器与估计器解耦后再看轨迹是否正常。
• 机器人进入轿厢后长时间等待、电梯并未启动，会持续累积漂移。等待阶段的周期 ZUPT 就是为这种情况准备的，period_s 控制检查频率，速度与加速度阈值控制触发条件。
• 乘梯后地图出现叠层残影，可以打开 elevator.rebuild_map_on_exit，用退出后的若干帧重建局部地图；代价是重建窗口内暂时不写入历史地图。
• 若设备在轿厢内被拿起晃动，不要打开 strong_prior_enable。该先验假设设备相对轿厢没有竖直运动，晃动场景下会引入错误高度。

---

总结

• 本文从电梯场景下 LIO 失效的物理原因出发，完整拆解了 Elevator-LIO 的解耦状态模型、非惯性传播、模式相关 IESKF、模式管理、退出修正与自适应前端，并把每条公式落到了对应的源码函数上。
• 核心要点回顾：
  • 问题的本质是参考系错配：IMU 量的叠加运动与 LiDAR 量的相对几何不在同一系，运输状态在 LiDAR 残差中雅可比为零，属于弱可观。
  • 解耦的关键是多三个标量：把运输加速度按随机游走建模进状态，比事后用启发式去扣漂移更自洽，且不影响普通 LIO 的行为。
  • 模式切换只是条件填表：$F_x$ 与 $F_w$ 的电梯块填或不填，决定了当前走非惯性传播还是标准传播。
  • 退出修正是闭环的最后一步：零速零加速度更新压低漂移，重锚定把位移并入机器人位置，协方差重置切断不确定性传播。
  • 前端调节同样影响可观性：自适应体素让有效点数在任何尺度下都落在合理区间，紧耦合才有稳定的几何约束可用。
• 如有错误，欢迎指出！
• 感谢观看！