ZUPT 开发日志 - HIT-K1928-LAB/HIT-K1928-wiki GitHub Wiki
2025-11-18
为 vins-fusion 增加 zupt 功能
来一帧 imu 数据，进行实时一阶滤波，存到队列，包括原始数据和滤波数据，用于相减后求高频变化
-
分析 imu 数据结构，设计队列
- 修复 imu 队列长度问题：弹出 应该在 使用队列数据之前，否则 imu 队列长度在 275 附近波动，符合两帧图像之间的 imu 数据数目 25 左右
-
写低通与幅值判断
-
是否增加速度模长判断
- 不增加，功能上可选：增加了会受到零偏不准而导致的速度估计不准的影响；不增加则因为低通滤波，能够消除零偏对静止检测的影响，但会引入匀速运动判定为静止的风险（不过实际中可能几乎不会发生，因为无人机匀速运动时，难免有振动，而振动幅度大于给定阈值就不会判定为静止了）
-
测试与调参
- 看曲线变化
- 看脉冲输出是否正确
-
分析与另一种方法的等价性：P = 0 时等价
-
补充均值判断
-
加 imu 数据去重力操作
-
打印数据，plotjuggler 可视化，确定合适的阈值
-
增加视差判断逻辑
-
视差判断与 imu 判断函数可放在 processMeasurement() 函数内，会比放在 回调函数 inputFeature 里更合适
- 首先是实验证明 improcessMeasurement 正常情况下的执行时长会比两帧图像帧间隔长，因为把 zupt 检测部分放在 inputFeature 还是 processMeasurement 里，新增的 imu 都是 25 左右。也就是 inputFeature 里放进去的数据很快就被 processMeasurement 里消耗了
- 其次从函数角色角度来看，inputFeature 是生产者，而 processMeasurement 是消费者，处理的逻辑应当放在后者，以保持自明性
-
zupt 检测结果有两个去路，一个是话题发布，另一个是类内状态量，用于通知后端构造残差
- 类内结果队列以适应多线程场景，存储 (时间、结果) 对；加 mutex；更新代码后因为 检测 和 结果获取 都在 processMeasurement 里，不是多线程，因此队列和 mutex 不是必须的
- 结果需额外补充 am RGI wm，用以残差构建
- 发布话题
-
新增用于存储 zupt 残差所需的一些信息的滑窗数组
- 参照 imu 因子的构建思路，高频数据存入队列，弹出图像（processMeasurement）时往 滑窗数组 中最新的那一个位置存入新数据；因为该位置在上一轮的 SlideWindows 时就已经空出来了
- 滑窗操作部分补充 zupt 部分
-
加残差
- 验证 q 的 PlusJac = [I 0]^T 的正确性
- 确定残差项及其雅可比
- 重力忘记传参导致 Bug；
- 测试结果
- 如果考虑视差的结果，漂移 2 m；只考虑 IMU 的检测结果，0.62 m
- 如果零偏约束的权重较小（1e1量级），则会发生较大漂移，但静止时不会有抖动
- 如果权重太大，会出现速度压到0附近抖动，而位置则会明显抖动
-
边缘化
- 是否需要边缘化：需要
x | o---x---o [ H1 0 ] + [ H2 H3 ] ---> H4 - H3 * (H2+H1)^-1 H3 [ 0 0 ] [ H3 H4 ] - 确认 marginalization 的逻辑和接口
- 会先 构造一系列和要边缘掉的状态相连的 ResisualBlockInfo，记录的 costfunction parameter 的信息，然后 add 到 MarginalizeionInfo 内，在这步会先将待边缘化的 parameter_blocks 先编号（parameter_block_idx）
- preMarginalize 内对以上的 resisual 进行 evaluate，会获取刚优化过后的值作为线性化点的残差值和雅可比，并存储雅可比。
- 测试效果：无明显影响
- 是否需要边缘化：需要
-
增加更多参数配置
-
排查：USE_ZUPT = 1，即使注释掉 add_resisual 和 add_margin，仍有较大漂移（1m）
-
USE_ZUPT = 0: 9/20 的概率漂移 1m 以上，其余为 0.54m 左右漂移
-
发现只约束 bg 和 vel，不约束 ba 和 R: 1/4 飘 1m 以上，其余为 0.5m 左右
- 测了两次效果都还可以（0.033303 -0.366908 0.379974）
- 第三次漂移1.5m，第四次漂移 0.5m
-
如果在此基础上打开 zupt log，大概率漂移 1.5m 左右，不打开则 0.5m 左右
-
将外参更换成离线标定的结果，效果并没有更好：
- 如果不在线估计，则轨迹更不平滑，漂移 0.8m 左右，t: 0.754935 -0.193036 -0.187951
- 如果在线估计，则轨迹更不平滑，漂移 0.63m 左右，t: 0.018170 -0.520624 0.374678
- 第一次静到动，估计结果比图像先动，待排查原因
-
录制含动捕真值的数据集 [2025-12-29]
-
使用旧外参
- 开外参在线估计
- 不开 zupt: 0.08m; 0.3m; 0.2m 漂移
- 开 zupt: 0.2m 漂移
- 不开外参在线估计
- 不开 zupt: 前期折线较多，不久后发散崩溃
- 开 zupt: 同不开，原因在于还没有静止就崩了，没有太多 zupt 的发挥空间
- 开外参在线估计
-
使用新外参
- 开外参在线估计
- 不开 zupt: 0.135m t: -0.090853 -0.030245 0.094739 q: 0.998322 -0.022108 -0.027384 0.045976
- 开 zupt:
- 0.117m t: -0.112668 0.018123 0.025415 q: 0.998461 -0.021246 -0.025417 0.044478
- t: 0.046103 -0.274492 0.015186 q: 0.994259 -0.020037 -0.027410 0.101470
- t: 0.069893 0.015145 -0.036661 q: 0.997685 -0.019859 -0.025356 0.059896
- 不开外参在线估计
- 不开 zupt: 0.649m t: 0.465684 -0.424460 0.154550 q: 0.980075 -0.017463 -0.033086 0.195074
- 开 zupt: 0.704m t: 0.561373 -0.425389 -0.010809 q: 0.984859 -0.016621 -0.031798 0.169604
- 开外参在线估计
-
使用 bkr 给的收敛外参
- 开外参在线估计
- 不开 zupt: 0.276m t: 0.041937 -0.273132 0.006299 q: 0.994406 -0.020075 -0.027366 0.100024
- 开 zupt: 0.283m t: 0.069802 -0.274117 -0.003857 q: 0.994426 -0.021424 -0.028401 0.099253
- 不开外参在线估计
- 不开 zupt: 0.289m t: 0.031330 -0.284367 0.041966 q: 0.995025 -0.020308 -0.026023 0.093999
- 开 zupt（no log）:
- 0.167m t: -0.148868 -0.015099 0.075016 q: 0.998379 -0.021948 -0.026605 0.045269
- 0.197m t: -0.013130 -0.160896 -0.112877 q: 0.996978 -0.017866 -0.025579 0.071146
- 0.289m t: 0.017742 -0.284895 0.040357 q: 0.995100 -0.020402 -0.026293 0.093104
- 开 zupt （log）: 0.201m t: -0.008134 -0.164618 -0.114565 q: 0.996869 -0.017892 -0.025678 0.072615
- 开外参在线估计
-
使用本地最佳收敛外参
- 开外参在线估计
- 不开 zupt: 0.279m t: 0.015910 -0.278310 0.016140 q: 0.994572 -0.020014 -0.026572 0.098591
- 开 zupt: 0.137m t: 0.091468 -0.072452 -0.070939 q: 0.997754 -0.019869 -0.025398 0.058706
- 不开外参在线估计
- 不开 zupt: 0.133m t: 0.126532 -0.038519 -0.014140 q: 0.998086 -0.020112 -0.025260 0.052743
- 开 zupt: 0.186m t: 0.063969 -0.241476 -0.008334 q: 0.995425 -0.020929 -0.026175 0.089474
- 开外参在线估计
-
| 外参新旧（收敛2，新1，旧0） | 外参在线估计（开1，不开0） | ZUPT（开1， 不开0） | 漂移误差（m） |
|---|---|---|---|
| 0 | 1 | 0 | 0.19 |
| 0 | 1 | 1 | 0.20 |
| 0 | 0 | 0 | x |
| 0 | 0 | 1 | x |
| 1 | 1 | 0 | 0.135 |
| 1 | 1 | 1 | 0.117 |
| 1 | 0 | 0 | 0.649 |
| 1 | 0 | 1 | 0.704 |
| 2 | 1 | 0 | 0.279 |
| 2 | 1 | 1 | 0.137 |
| 2 | 0 | 0 | 0.133 |
| 2 | 0 | 1 | 0.186 |
- 用这个数据集测的结果，用新外参效果都更好，所以上次测的外参还算准。旧的外参适用于kf数据集，新外参适用于这个数据集，具体原因不知道，可能是结构有小变化导致外参变了的。
- 开在线估计，新旧外参都基本一开始就收敛了，之后不怎么剧烈变化，且收敛到基本相同的值。但是从一开始的变化幅度来看，新外参确实更准
- 不管新旧外参，不开外参在线估计要么崩，要么变差。可能说明测的新外参比旧的准，但还不够准，但再标定精度大概率不会有质的提升。
- 填入较好结果的外参收敛值，之后进行固定外参和在线估计的测试，均表明没有变更好
- zupt 效果变好，只发生在 新外参+在线估计
RIC -> RIG g -> ba
2026-01-07
- 测试强先验的外参
- 0.96m t: 0.915851 -0.279134 0.037149
- 加回 zupt 对 RIG 和 ba 的约束
- 0.1 权重：0.3 m 漂移
- 观察一下收敛外参的在线估计结果是否会再次收敛到其他值
- Tx_init 是 bkr 板子上的外参收敛值，Tx_new 是我本地使用新外参运行后的收敛值，x = {0, 1}
In [15]: T1_init - T0_init Out[15]: array([-4.86419032e-04, -6.02870070e-05, -1.18035602e-05, 5.67593182e-05, -9.14516388e-06, -2.00061250e-04, -4.94080872e-04, -4.95882512e-02, 2.31643774e-04, -1.13714419e-05, -9.38853270e-05, -4.08338182e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]) In [16]: T1_new -T0_new Out[16]: array([-4.70094821e-04, -9.33991003e-05, -1.36469640e-05, 5.07733500e-05, -3.82828184e-06, -1.23840959e-04, -4.70588827e-04, -4.95919106e-02, 1.55114222e-04, -3.55579572e-06, -1.26892875e-04, -4.05702765e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]) In [17]: T0_new - T0_init Out[17]: array([ 2.91559805e-05, -9.59861632e-04, -5.90997670e-05, -1.80768491e-04, 3.98013889e-05, 5.46721662e-04, 1.35621549e-04, 6.97523905e-05, -5.38366810e-04, 1.01201940e-04, -9.61325023e-04, -1.45118223e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]) In [18]: T1_new - T1_init Out[18]: array([ 4.54801912e-05, -9.92973725e-04, -6.09431708e-05, -1.86754459e-04, 4.51182709e-05, 6.22941953e-04, 1.59113594e-04, 6.60929793e-05, -6.14896362e-04, 1.09017587e-04, -9.94332572e-04, -1.42482805e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00])- Tx_init 是我本地使用新外参，Tx_new 是第 1 次收敛值，x = {0, 1}
In [40]: T0_new - T0_init Out[40]: array([-6.55640124e-04, 1.57210905e-02, 1.07746960e-03, -2.92924569e-02, 3.20385936e-05, 1.20957748e-04, -1.84144236e-03, -2.53960712e-02, -2.69557003e-04, -1.08279415e-03, 1.56234449e-02, 1.36879985e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]) In [41]: T1_new -T1_init Out[41]: array([ 1.71932975e-03, 2.01294941e-02, 1.47473498e-03, -2.75389834e-02, 2.38837757e-05, 3.52258826e-04, 2.23388772e-04, -2.54623593e-02, -7.58090437e-04, -1.41838746e-03, 2.01949960e-02, 1.33501728e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]) In [42]: T0_init - T1_init Out[42]: array([ 2.851320e-03, 4.518950e-03, 4.127400e-04, 1.810080e-03, -1.380000e-06, 4.035100e-04, 2.543130e-03, 4.930514e-02, -6.923800e-04, -3.291400e-04, 4.715550e-03, 3.673190e-03, 0.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00]) In [43]: T0_new - T1_new Out[43]: array([ 4.76350127e-04, 1.10546416e-04, 1.54746217e-05, 5.66065263e-05, 6.77481789e-06, 1.72208922e-04, 4.78298867e-04, 4.93714281e-02, -2.03846567e-04, 6.45331022e-06, 1.43998912e-04, 4.01101564e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00])- Tx_init 是我本地使用新外参第 1 次收敛值，Tx_new 是第 2 次收敛值，x = {0, 1}
In [24]: T0_new - T0_init Out[24]: array([ 2.75978072e-03, -1.55981461e-04, 3.62606662e-05, -1.69477074e-03, -1.11782232e-04, -1.04103337e-03, 2.70365496e-03, -5.34358055e-03, 8.72252478e-04, -7.01016743e-05, 6.51894012e-05, -1.09285662e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]) In [25]: T1_new - T1_init Out[25]: array([ 2.77500182e-03, -1.35424865e-04, 3.90667677e-05, -1.55634279e-03, -1.08350427e-04, -9.78456774e-04, 2.72112920e-03, -5.29310619e-03, 8.08055338e-04, -6.64241660e-05, 8.60737027e-05, -1.07068962e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]) In [26]: T0_init - T1_init Out[26]: array([ 4.76350127e-04, 1.10546416e-04, 1.54746217e-05, 5.66065263e-05, 6.77481789e-06, 1.72208922e-04, 4.78298867e-04, 4.93714281e-02, -2.03846567e-04, 6.45331022e-06, 1.43998912e-04, 4.01101564e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]) In [27]: T0_new - T1_new Out[27]: array([ 4.61129024e-04, 8.99898200e-05, 1.26685201e-05, -8.18214230e-05, 3.34301314e-06, 1.09632329e-04, 4.60824626e-04, 4.93209538e-02, -1.39649426e-04, 2.77580189e-06, 1.23114610e-04, 3.98884864e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]) - 双目外参减少自由度
- zupt 阶段增加对外参的强约束
- 新 IMU 跑 vins，查看零偏估计情况