【OpenVINS】（一）ZUPT

参考：Measurement Update Derivations » Zero Velocity Update

在典型的自主汽车场景中，传感器系统将在停止灯处变得静止，其中动态物体（例如交叉路口的其他车辆）可能会快速损坏系统，通过零速检测识别当前车辆处于静止状态，以暂停特征跟踪，来避免定位系统崩溃

1. Inertial-based Detection

1.1 误差构造

当车辆静止（对于车辆匀速直线运动这种case可以通过速度幅值检测剔除），加速度和角速度先验值都是0，先验也是一种观测，所以构造如下观测方程：
z ~ = [ a − ( a m − b a − G I k R G g − n a ) ω − ( ω m − b g − n g ) ] = [ − ( a m − b a − G I k R G g − n a ) − ( ω m − b g − n g ) ] \tilde{\mathbf{z}}=\left[\begin{array}{c} \mathbf{a}-\left(\mathbf{a}_{m}-\mathbf{b}_{a}-{ }_{G}^{I_{k}} \mathbf{R}^{G} \mathbf{g}-\mathbf{n}_{a}\right) \\ \boldsymbol{\omega}-\left(\boldsymbol{\omega}_{m}-\mathbf{b}_{g}-\mathbf{n}_{g}\right) \end{array}\right]=\left[\begin{array}{c} -\left(\mathbf{a}_{m}-\mathbf{b}_{a}-{ }_{G}^{I_{k}} \mathbf{R}^{G} \mathbf{g}-\mathbf{n}_{a}\right) \\ -\left(\boldsymbol{\omega}_{m}-\mathbf{b}_{g}-\mathbf{n}_{g}\right) \end{array}\right] z~=[a−(am​−ba​−GIk​​RGg−na​)ω−(ωm​−bg​−ng​)​]=[−(am​−ba​−GIk​​RGg−na​)−(ωm​−bg​−ng​)​]

openvins代码也利用速度为0的先验，构造误差方程，
0 = v k + 1 = v k − g ∗ d t + R T ∗ ( a m − b a − n a ) ∗ d t 0 = v_{k+1} = v_k - g*dt + R^T*(a_m - b_a - n_a)*dt 0=vk+1​=vk​−g∗dt+RT∗(am​−ba​−na​)∗dt

此误差与上面的 z ~ \tilde{\mathbf{z}} z~ 中以加速度为 0 0 0 的先验构造的误差项可认为是等价的，只需要使用一个即可

1.2 计算雅可比

a = a m − b a − G I k R G g − n a ω = ω m − b g − n g \begin{aligned} \mathbf{a} &=\mathbf{a}_{m}-\mathbf{b}_{a}-{ }_{G}^{I_{k}} \mathbf{R}^{G} \mathbf{g}-\mathbf{n}_{a} \\ \boldsymbol{\omega} &=\boldsymbol{\omega}_{m}-\mathbf{b}_{g}-\mathbf{n}_{g} \end{aligned} aω​=am​−ba​−GIk​​RGg−na​=ωm​−bg​−ng​​

对上式一阶泰勒展开，即可得到雅可比
∂ z ~ ∂ G I k R = − ⌊ G I k R G g × ⌋ ∂ z ~ ∂ b a = ∂ z ~ ∂ b g = − I 3 × 3 \begin{aligned} \frac{\partial \tilde{\mathbf{z}}}{\partial_{G}^{I_{k}} \mathbf{R}} &=-\left\lfloor ^{I_{k}}_{G} \mathbf{R}^{G} \mathbf{g} \times \right\rfloor\\ \frac{\partial \tilde{\mathbf{z}}}{\partial \mathbf{b}_{a}} &=\frac{\partial \tilde{\mathbf{z}}}{\partial \mathbf{b}_{g}}=-\mathbf{I}_{3 \times 3} \end{aligned} ∂GIk​​R∂z~​∂ba​∂z~​​=−⌊GIk​​RGg×⌋=∂bg​∂z~​=−I3×3​​

注意，这里使用的JPL四元数，所以关于 G I k R _{G}^{I_{k}} \mathbf{R} GIk​​R的雅可比前面会多一个负号

1.3 卡方分布来确定当前车辆是否处于静止状态

z ~ ⊤ ( H P H ⊤ + α R ) − 1 z ~ < χ 2 \tilde{\mathbf{z}}^{\top}\left(\mathbf{H P H}^{\top}+\alpha \mathbf{R}\right)^{-1} \tilde{\mathbf{z}}<\chi^{2} z~⊤(HPH⊤+αR)−1z~<χ2

• P \mathbf{P} P表示状态 G I k R G , b g , b a ^{I_{k}}_{G} \mathbf{R}^{G}, \mathbf{b}_g, \mathbf{b}_a GIk​​RG,bg​,ba​的协方差，观测方程会使用两帧图像中间所有的imu测量值构造（每个测量值构造一组观测方程），方程中关于状态的协方差并非都是常量，
  • P \mathbf{P} P中关于 G I k R G ^{I_{k}}_{G} \mathbf{R}^{G} GIk​​RG的协方差是常量，就是 k k k时刻姿态的协方差
  • 而 b g , b a \mathbf{b}_g, \mathbf{b}_a bg​,ba​的协方差则需要从 k k k时刻积分到当前imu测量值时刻 t t t，这段时间记为 Δ t \Delta t Δt，则 Σ t b g , b a = Σ c o n t i n u o u s b g , b a Δ t \Sigma_t^{\mathbf{b}_g, \mathbf{b}_a} = \Sigma_{continuous}^{\mathbf{b}_g, \mathbf{b}_a} \Delta t Σtbg​,ba​​=Σcontinuousbg​,ba​​Δt，也就是说ZUPT中不只是有观测方程，还隐含了一个这样的state propagation的过程
  • openvins为了简便做了近似，使用两帧图像间的时间差 Δ t k , k + 1 \Delta t_{k,k+1} Δtk,k+1​替代 Δ t \Delta t Δt
• 实际使用中，IMU的噪声 R \mathbf{R} R需要放大50-100倍，因为我们有可能低估了我们的IMU的噪声，以及IMU挂在的车体本身存在振动

1.4 状态更新

• 对状态 b g , b a \mathbf{b}_g, \mathbf{b}_a bg​,ba​进行state propagation，即更新状态协方差
• 执行正常的KF更新步骤

2. Disparity-based Detection

1 N ∑ i = 0 N ∥ u v k , i − u v k − 1 , i ∥ < Δ d \frac{1}{N} \sum_{i=0}^{N}\left\|\mathbf{u v}_{k, i}-\mathbf{u v}_{k-1, i}\right\|<\Delta d N1​i=0∑N​∥uvk,i​−uvk−1,i​∥<Δd

注意，这种方式在动态环境中将会失效，所以需要配合第一种方式一起使用