290 |
291 | ### 3.基于四元数的 error-state Kalman Filter
292 |
293 | 其实原理部分相当重要,包括你对error-state Kalman Filter的理解,但是如果要从头讲起的话可以说篇幅太长,考虑到做SLAM的同学们基本上都应该知道这本书 [Quaternion kinematics for the error-state Kalman filter](https://arxiv.org/abs/1711.02508), 这本书会让你在四元数,SO3,IMU的模型以及基于IMU的ESKF(error-state Kalman Filter)都会有比较深刻的理解,对应这本书我也做了很多注释,关于基于四元数原理部分由于篇幅限制,我这里不想做太多的说明,但是接下来在讲解S-MSCKF原理部分我们会将参考公式附上.下面是一些我在这本参考资料中的注释图.
294 |
295 | | 
| 
|
296 | | :----------------------------------------------------------: | :----------------------------------------------------------: |
297 | | 
| 
|
298 |
299 | 当然重要的事情需要说三遍,那就是[Quaternion kinematics for the error-state Kalman filter](https://arxiv.org/abs/1711.02508)四元数是Hamilton表示方法,而MSCKF中采用的是JPL表示方法,关于这两者的不同,可以参考书中34页,Table2.
300 |
301 | 看过论文代码的同学可能会说,MSCKF这一部分代码参考的是MARS实验室[Indirect Kalman Filter for 3D Attitude Estimation-A Tutorial for Quaternion Algebra](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.304.6207&rep=rep1&type=pdf),答案是肯定的.但是我的建议是以Hamilton那本书为基础,然后自行再去推导MARS出品那一本,那样你的体验会更加深刻.
302 |
303 | ### 4.S-MSCKF
304 |
305 | #### 4.1 基本原理
306 |
307 | 在讲解之前,我们先来定义一下坐标系,如下图所示:
308 | 
309 |
310 | 其中$I$表示IMU机体坐标系(Body Frame),$G$表示的是惯性系(Inertial Frame),$C$表示的是相机坐标系(Camera Frame).
311 |
312 | 作为一个滤波器,我们首先来看滤波器的状态向量,它包含两个部分,IMU状态和Camera状态:
313 | $$
314 | \textbf{x}_{I}=\begin{bmatrix}
315 | _{G}^{I}\textbf{q}^{T} & \textbf{b}_{g}^{T} & ^{G}\textbf{v}_{I}^{T} & \textbf{b}_{a}^{T} &
316 | ^{G}\textbf{p}_{I}^{T} & _{C}^{I}\textbf{q}^{T} & ^{I}\textbf{p}_{C}^{T}
317 | \end{bmatrix}^{T}
318 | \tag{4.1}
319 | $$
320 | 其中$_{G}^{I}\textbf{q}$表示的是从惯性系到机体IMU系的旋转变换, $^{G}\textbf{v}_{I}$和$^{G}\textbf{p}_{I}$分别表示机体在惯性系下的速度和位置,$\textbf{b}_{g}$和$\textbf{b}_{a}$分别代表IMU的bias,而$_{C}^{I}\textbf{q}$ 和$ ^{I}\textbf{p}_{C}$分别代表从相机坐标系到IMU坐标系的旋转与平移,其中以左相机为准. 但是我们注意到旋转实际上只有三个自由度,而且四元数必须是单位单位四元数,那这样额外的约束会使得协方差矩阵奇异,所以我们定义error IMU状态如下:
321 | $$
322 | \tilde{\textbf{x}}_{I}=\begin{bmatrix}
323 | _{G}^{I}\tilde{\theta }^{T} & \tilde{\textbf{b}}_{g}^{T} & ^{G}\tilde{\textbf{v}}_{I}^{T} & \tilde{\textbf{b}}_{a}^{T} &
324 | ^{G}\tilde{\textbf{p}}_{I}^{T} & _{C}^{I}\tilde{\textbf{q}}^{T} & ^{I}\tilde{\textbf{p}}_{C}^{T}
325 | \end{bmatrix}^{T}
326 | \tag{4.2}
327 | $$
328 |
329 | 所以IMU的状态一共是$3\cdot7=21$维向量,后6维度其实是相机与IMU的外参,这6个维度在MSCKF1.0版本是不在状态向量里边的. MSCKF的状态向量还包含另外一个组成部分,那就是N个相机的姿态,那么每个相机的姿态误差向量定义为 $\tilde{\textbf{x}}_{C_{i}}=\left ( \begin{matrix}_{G}^{C_{i}}\tilde{\theta }^{T} & ^{G}\tilde{\textbf{p}}_{C_{i}}^{T}\end{matrix} \right )^T$,所以当滑窗里边有N个相机姿态的时候,整个误差状态向量为:
330 | $$
331 | \tilde{\textbf{x}} = \begin{pmatrix}
332 | \tilde{\textbf{x}}_{I}^{T} & \tilde{\textbf{x}}_{C_{1}}^{T} & ... & \tilde{\textbf{x}}_{C_{N}}^{T}
333 | \end{pmatrix}
334 | \tag{4.3}
335 | $$
336 | 我们以标准error-state KF为准,其过程包含运动模型和观测模型.
337 |
338 | ##### A.运动模型
339 |
340 | 我们知道对于IMU的状态连续时间的运动学有:
341 | $$
342 | \begin{split}
343 | _{G}^{I}\dot{\hat{\textbf{q}}}=\frac{1}{2}\cdot_{G}^{I}\hat{\textbf{q}}\otimes\hat{w} =\frac{1}{2}\Omega (\hat{w})_{G}^{I}\hat{\textbf{q}},\\
344 | \dot{\hat{\textbf{b}}}_{g}=\textbf{0}_{3\times 1},\\
345 | ^{G}\dot{\hat{\textbf{v}}}=C(_{G}^{I}\hat{\textbf{q}})^{T}\hat{\textbf{a}}+^{G}\textbf{g},\\
346 | \dot{\hat{\textbf{b}}}_{a}=\textbf{0}_{3\times 1},\\
347 | ^{G}\dot{\hat{\textbf{p}}_{I}}=^{G}{\hat{\textbf{v}}},\\
348 | _{C}^{I}\dot{\hat{\textbf{q}}}=\textbf{0}_{3\times 1},\\
349 | ^{I}\dot{\hat{\textbf{p}}_{C}}=\textbf{0}_{3\times 1}
350 | \end{split}\tag{4.4}
351 | $$
352 | 其中 $\hat{w}$ 和 $\hat{a}$ 分别为角速度和加速度的估计值(测量值减去bias),即有:
353 | $$
354 | \hat{w}=w_{m}-\hat{b}_{g},\hat{a}=a_{m}-\hat{b}_{a}\tag{4.5}
355 | $$
356 | 其中有几点要说明,其中,
357 | $$
358 | \Omega (\hat{w})=\Omega (\hat{w})=\begin{pmatrix}
359 | -[\hat{w}_{\times }] & w\\
360 | -w^{T} & 0
361 | \end{pmatrix}
362 | \tag{4.6}
363 | $$
364 | 这个直接参考四元数乘法就可以了,然后 $[\hat{w}_{\times }]$ 是 $\hat{w}$的反对称矩阵; $C(\cdot)$表示四元数到旋转矩阵的转换,这个可以参照[1]和[2]. 那按照S-MSCKF的论文所述,我们可以得到以下式子,
365 | $$
366 | \dot{\tilde{\textbf{x}}}_{I}=\textbf{F}\tilde{\textbf{x}}_{I}+
367 | \textbf{G} \textbf{n}_{I}
368 | \tag{4.7}
369 | $$
370 | 其中
371 | $$
372 | \textbf{n}_{I}^{T}=\begin{pmatrix}
373 | \textbf{n}_{g}^{T}& \textbf{n}_{wg}^{T} & \textbf{n}_{a}^{T} & \textbf{n}_{wa}^{T}
374 | \end{pmatrix}^{T}
375 | \tag{4.8}
376 | $$
377 | $\textbf{n}_{g}$ 和 $\textbf{n}_{a}$ 分别代表角速度和加速度的测量噪声,服从高斯分布; $\textbf{n}_{wg}$ 和 $\textbf{n}_{wa}$ 分别代表角速度和加速度的bias的随机游走噪声. $\textbf{F} $ 是 $21\times21$ 大小矩阵, $\textbf{G}$ 是 $21\times12$ 大小的矩阵,其详细推到见附录A.
378 |
379 | 对于IMU的状态来说,我们可以采用RK4的积分方法根据(4.4)求得IMU的状态. 那么对于IMU的协方差矩阵呢,我们需要事先求取状态转移矩阵和离散的运动噪声协方差矩阵,如下:
380 | $$
381 | \Phi_{k}=\Phi(t_{k+1},t_{k})=\textbf{exp}(\int_{t_{k}}^{t_{k+1}} \textbf{F}(\tau)d\tau) \\
382 | \textbf{Q}_{k}=\int_{t_{k}}^{t_{k+1}}\Phi({t_{k+1},\tau})\textbf{GQG}\Phi({t_{k+1},\tau})^{T}d\tau
383 | \tag{4.9}
384 | $$
385 |
386 | 关于这个状态转移矩阵 $\Phi_{k}$ 的求法, 其实式子4.9是一个指数,指数项是一个积分项,当 $t_{k+1}$ 与 $t_{k}$ 间 $\Delta t$ 较小时候,可以得到这样的式子:
387 | $$
388 | \Phi_{k}=\Phi(t_{k+1},t_{k})=\textbf{exp}(\int_{t_{k}}^{t_{k+1}} \textbf{F}(\tau)d\tau) = \textbf{exp}(\textbf{F}\Delta t)= I+\textbf{F}\Delta t + \frac{1}{2!}(\textbf{F}\Delta t)^{2}+\frac{1}{3!}(\textbf{F}\Delta t)^{3}+...
389 | \tag{4.10}
390 | $$
391 | 整个状态(IMU+Camera)的covariance传播过程如图所示:
392 |
393 | 
394 |
395 | 那么对于左上角IMU的corvariance的传播有:
396 | $$
397 | \textbf{P}_{II_{k+1|k}}=\Phi_{k}\textbf{P}_{II_{k|k}}\Phi_{k}^{T}+\textbf{Q}_{k}
398 | \tag{4.11}
399 | $$
400 | 其中Camera的covariance暂时还没有变化是因为这个时间段图像还没有到来,只有IMU的影响,但是会影响到IMU与Camera协方差,即上图灰色矩形块.
401 |
402 | ##### B. 增广
403 |
404 | 当图像到来时,需要对当前相机姿态做增广,这个时刻的相机姿态是由上一时刻的IMU propagate的结果结合外参得到的:
405 |
406 | $$
407 | _{G}^{C}\hat{\textbf{q}}=_{I}^{C}\hat{\textbf{q}}\otimes _{G}^{I}\hat{\textbf{q}} \\
408 | ^{G}\hat{\textbf{p}}_{C} = ^{G}\hat{\textbf{p}}_{I} + C(_{G}^{I}\hat{\textbf{q}})^{T} \cdot{^{I}\hat{\textbf{p}}_{C}}
409 | \tag{4.12}
410 | $$
411 | 假设上一时刻共有N个相机姿态在状态向量中,那么当新一帧图像到来时,这个时候整个滤波器的状态变成了$21+6(N+1)$的向量, 那么它对应的covariance维度变为 $(21+6(N+1))\times(21+6(N+1))$ .其数学表达式为:
412 | $$
413 | \textbf{P}_{k|k}=\begin{pmatrix}
414 | \textbf{I}_{21+6N}\\
415 | \textbf{J}
416 | \end{pmatrix}\textbf{P}_{k|k}\begin{pmatrix}
417 | \textbf{I}_{21+6N}\\
418 | \textbf{J}
419 | \end{pmatrix}^{T}
420 | \tag{4.13}
421 | $$
422 | 这个过程对应如下图过程:
423 |
424 | 
425 |
426 | 其中$J$ 的详细推导过程见附录B.
427 |
428 | ##### C.观测模型
429 |
430 | MSCKF的观测模型是以特征点为分组的,我们可以知道一个特征(之前一直处于跟踪成功状态)会拥有多个Camera State.所有这些对于同一个3D点的Camera State都会去约束观测模型. 那这样其实隐式的将特征点位置从状态向量中移除,取而代之的便是Camera State. 我们考虑单个feture $f_{j}$, 假设它所对应到 $M_{j}$ 个相机姿态 $(_{G}^{C_{i}}\textbf{q}, ^{G}\textbf{p}_{C_{i}}), i \in j$. 当然双目版本的包含左目和右目两个相机姿态, $(_{G}^{C_{i,1}}\textbf{q}, ^{G}\textbf{p}_{C_{i,1}})$ 和 $(_{G}^{C_{i,2}}\textbf{q}, ^{G}\textbf{p}_{C_{i,2}})$ 右相机很容易能通过外参得到. 其中双目的观测值可以表示如下:
431 | $$
432 | \textbf{z}^{j}_{i}=\begin{pmatrix}
433 | u^{j}_{i,1}\\
434 | v^{j}_{i,1}\\
435 | u^{j}_{i,2}\\
436 | v^{j}_{i,2}
437 | \end{pmatrix}
438 | = \begin{pmatrix}
439 | \frac{1}{^{C_{i,1}}Z_{j}} & \textbf{0}_{2\times 2} \\
440 | \textbf{0}_{2\times 2} & \frac{1}{^{C_{i,2}}Z_{j}}
441 | \end{pmatrix}
442 | \begin{pmatrix}
443 | \frac{1}{^{C_{i,1}}X_{j}}\\
444 | \frac{1}{^{C_{i,1}}Y_{j}}\\
445 | \frac{1}{^{C_{i,2}}X_{j}}\\
446 | \frac{1}{^{C_{i,2}}Y_{j}}
447 | \end{pmatrix}
448 | \tag{4.14}
449 | $$
450 | 而特征点在两个相机坐标系下可以分别表示为:
451 | $$
452 | \begin{split}
453 | ^{C_{i,1}}\textbf{p}_{j}=\begin{pmatrix}
454 | ^{C_{i,1}}X_{j}\\
455 | ^{C_{i,1}}Y_{j}\\
456 | ^{C_{i,1}}Z_{j}
457 | \end{pmatrix}
458 | = C(^{C_{i,1}}_{G} \textbf{q})(^{G}\textbf{p}_{j}-^{G}\textbf{p}_{C_{i,1}})\\
459 | ^{C_{i,2}}\textbf{p}_{j}=\begin{pmatrix}
460 | ^{C_{i,2}}X_{j}\\
461 | ^{C_{i,2}}Y_{j}\\
462 | ^{C_{i,2}}Z_{j}
463 | \end{pmatrix}
464 | = C(^{C_{i,2}}_{G} \textbf{q})(^{G}\textbf{p}_{j}-^{G}\textbf{p}_{C_{i,2}})\\
465 | =C(^{C_{i,2}}_{C_{i,1}} \textbf{q})(^{C_{i,1}}\textbf{p}_{j}-^{C_{i,1}}\textbf{p}_{C_{i,2}})
466 | \end{split}
467 | \tag{4.15}
468 | $$
469 | 其中 $^{G}\textbf{p}_{j}$ 是特征点在惯性系下的坐标,这个是通过这个特征点的对应的所有camera state三角化得到的结果. 将观测模型在当前状态线性化可以得到如下式子:
470 | $$
471 | \textbf{r}^{j}_{i}=\textbf{z}^{j}_{i}-\hat{\textbf{z}}^{j}_{i}=
472 | \textbf{H}^{j}_{C_{i}}\tilde{\textbf{x}}_{C_{i}}+ \textbf{H}^{j}_{f_{i}}\tilde{\textbf{p}}_{j}+\textbf{n}^{j}_{i}
473 | \tag{4.16}
474 | $$
475 | 其中 $\textbf{n}^{j}_{i}$是观测噪声, $\textbf{H}^{j}_{C_{i}}$和 $\textbf{H}^{j}_{f_{i}}$ 是对应的雅克比矩阵.其详细推导和解析见附录C. 式子(4.16)对应到的是单个特征点对应的其中某一个相机姿态, 但是这个特征点会对应到很多相机姿态,我们直接将它贴在后边可以得到一个特征点的残差模型为:
476 | $$
477 | \textbf{r}^{j}=
478 | \textbf{H}^{j}_{\textbf{x}}\tilde{\textbf{x}}+ {\color{Red} \textbf{H}^{j}_{f}\tilde{\textbf{p}}_{j}}+\textbf{n}^{j}
479 | \tag{4.17}
480 | $$
481 | 但是这个其实并不是一个标准的EKF观测模型,因为我们知道 $\tilde{\textbf{p}}_{j}$ 并不在我们的状态向量里边,所以做法是将式子(4.17)中红色部分投影到零空间, 假设 $\textbf{H}^{j}_{f}$ 的[left null space](https://blog.csdn.net/liuheng0111/article/details/52522845) 为$\textbf{V}^{T}$, 即有$\textbf{V}^{T}\textbf{H}^{j}_{f}=\textbf{0}$, 所以式(4.17)可有写成:
482 | $$
483 | \textbf{r}_{o}^{j}=
484 | \textbf{V}^{T}\textbf{H}^{j}_{\textbf{x}}\tilde{\textbf{x}}+\textbf{V}^{T}\textbf{n}^{j}=\\
485 | \textbf{H}^{j}_{\textbf{x,o}}\tilde{\textbf{x}}+\textbf{n}^{j}_{o}
486 | \tag{4.18}
487 | $$
488 | 式(4.18)则是一个标准的EKF观测模型了,下面简单分析一下维度.分析时针对单个特征点, 我们知道 $\textbf{H}^{j}_{f}$ 的维度是 $4M_{j}\times 3 $, 那么它的left null space的维度即$\textbf{V}^{T}$ 的维度为 $(4\textbf{M}_{j} - 3)\times 4\textbf{M}_{j}$, 则最终 $\textbf{H}^{j}_{\textbf{x,o}}\tilde{\textbf{x}}$的维度变为 $(4\textbf{M}_{j} - 3)\times 6$, 残差的维度变为 $(4\textbf{M}_{j} - 3)\times 1$, 假设一共有L个特征的话,那最终残差的维度会是 $L(4\textbf{M}_{j} - 3)\times 1$. 更多详细的代码细节见给到的注释版代码,然后H矩阵的详细推导见附录C.
489 |
490 | #### 4.2 三角化
491 |
492 | 三角化是通过多帧相机对同一个点的观测计算出特征点在世界坐标系下的绝对3D坐标,或者你可以认为是恢复出一个比较可靠的3D点. 在讲这个之前,我们先来简单过一下相机的投影模型, 假设相机图像已经去畸变了,那么我们很容易得到这样一个模型:
493 | $$
494 | \begin{bmatrix}
495 | u\\
496 | v
497 | \end{bmatrix}
498 | =h\begin{pmatrix}
499 | X/Z\\
500 | Y/Z
501 | \end{pmatrix}=
502 | \begin{bmatrix}
503 | f_{x} & 0\\
504 | 0 & f_{y}
505 | \end{bmatrix}
506 | \begin{bmatrix}
507 | X/Z\\
508 | Y/Z
509 | \end{bmatrix}
510 | +\begin{bmatrix}
511 | c_{x}\\
512 | c_{y}
513 | \end{bmatrix}
514 | \tag{4.19}
515 | $$
516 | 其中(X,Y,Z)为相机坐标系下的一个点. 我们再来看下图:
517 |
518 | 
519 |
520 | 其中在惯性系下 $^{G}\hat{\textbf{p}}_{f}$ 被多帧相机观测到,其中在每个相机下的坐标表示为 $^{C_{i}}\textbf{p}_{f} = (^{C_{i}}X, ^{C_{i}}Y, ^{C_{i}}Z)^{T}$, 假设该特征第一个观测为 $^{C_{0}}\textbf{p}$, 余数在第 $i$个相机帧中可以表示为:
521 | $$
522 | ^{C_{i}}\textbf{p}_{f} = ^{C_{i}}_{C_{0}}\textbf{R}^{C_{0}}\textbf{p}_{f} + ^{C_{i}}\textbf{p}_{C_{0}}
523 | \tag{4.20}
524 | $$
525 | 我们将这个转换为逆深度的表达形式,可以得到下面一系列式子:
526 | $$
527 | ^{C_{i}}\textbf{p}_{f} = ^{C_{i}}_{C_{0}}\textbf{R}
528 | \begin{bmatrix}
529 | ^{C_{0}}X\\
530 | ^{C_{0}}Y\\
531 | ^{C_{0}}Z
532 | \end{bmatrix}
533 | + ^{C_{i}}\textbf{p}_{C_{0}}\\
534 | =^{C_{0}}Z(^{C_{i}}_{C_{0}}\textbf{R}
535 | \begin{bmatrix}
536 | ^{C_{0}}X / ^{C_{0}}Z\\
537 | ^{C_{0}}Y/ ^{C_{0}}Z \\
538 | 1
539 | \end{bmatrix}+
540 | \frac{1}{^{C_{0}}Z} \cdot ^{C_{i}}\textbf{p}_{C_{0}}
541 | )\\
542 | =\frac{1}{\rho } \textbf{g}_{i}(^{C_{i}}_{C_{0}}\textbf{R}
543 | \begin{bmatrix}
544 | \alpha \\
545 | \beta \\
546 | 1
547 | \end{bmatrix}+\rho ^{C_{i}}\textbf{p}_{C_{0}}
548 | )\\
549 | =\frac{1}{\rho }\textbf{g}_{i}(\begin{bmatrix}
550 | \alpha \\
551 | \beta \\
552 | \rho
553 | \end{bmatrix})
554 | =\frac{1}{\rho }\textbf{g}_{i}(\theta)
555 | \tag{4.21}
556 | $$
557 | 其中$\theta$ 为参数, $\alpha=^{C_{0}}X/^{C_{0}}Z$, $\beta=^{C_{0}}Y/^{C_{0}}Z$, $\rho=1/^{C_{0}}Z$.
558 |
559 | 并且这个假设我们的$^{C_{i}}\textbf{p}_{f}$ 为$(^{C_{i}}X / ^{C_{i}}Z, ^{C_{i}}Y/^{C_{i}}Z)^{T})$, 那么就是说$g_{i}(\theta)$是一个3维输入,二维输出的函数. 所以误差函数可以写成:
560 | $$
561 | f_{i}(\theta)=\textbf{z}_{i}-h(g_{i}(\theta))
562 | \tag{4.22}
563 | $$
564 | 假设一共共有N个相机观测,那么我们可以构建一个最小二乘问题,形如下式:
565 | $$
566 | arg\: min\sum_{i=1}^{n}\left \| f_{i}(\theta) \right \|_{2}
567 | \tag{4.23}
568 | $$
569 | 其中对应于单个特征点的$Jocabian$形式如下:
570 | $$
571 | J_{f}=\frac{\partial f }{\partial \theta}=\frac{\partial h }{\partial g}\frac{\partial g }{\partial \theta}
572 | \tag{4.24}
573 | $$
574 | 其中第一项 $\frac{\partial f }{\partial g}$非常简单,就是参考式(4,19), 得到的结果第比较简单,第二部分$\frac{\partial g}{\partial \theta}$ 根据式子(4.21)可以很容易得到
575 | $$
576 | \frac{\partial g}{\partial \theta}=\begin{bmatrix}
577 | \frac{\partial g_{i}}{\partial \alpha} & \frac{\partial g_{i}}{\partial \beta} & \frac{\partial g_{i}}{\partial \rho}
578 | \end{bmatrix}\\
579 | =
580 | [^{C_{i}}_{C_{0}}R\begin{bmatrix}
581 | 1\\
582 | 0\\
583 | 0
584 | \end{bmatrix},\: ^{C_{i}}_{C_{0}}R\begin{bmatrix}
585 | 0\\
586 | 1\\
587 | 0
588 | \end{bmatrix},\; ^{C_{i}}\textbf{p}_{C_{0}}]
589 | \tag{4.25}
590 | $$
591 | 然后用高斯-牛顿法就可以红容易解决这个最小二乘问题. 最后能得到$\hat{\theta}=[\hat{\alpha}, \hat{\beta}, \hat\rho]$, 那么其实也就是特征点在首个观测到它的相机帧下的坐标,根据下面的式子则很容易恢复出惯性系下的特征点的位置:
592 | $$
593 | ^{G}\hat{\textbf{p}}_{f}=\frac{1}{\hat{\rho}} \: ^{G}_{C_{0}}\textbf{R}\begin{bmatrix}
594 | \hat{\alpha}\\
595 | \hat{\beta}\\
596 | 1
597 | \end{bmatrix}
598 | + ^{G}\textbf{p}_{C_{0}}
599 | \tag{4.26}
600 | $$
601 | 注意,代码中的实现和现在这个推到稍微有点出入,它的实现主要参考的是文献7,不过基本大同小异,大家阅读起来应该也不会有太大的阻碍.
602 |
603 |
604 |
605 | #### 4.3 能观测性分析
606 |
607 | 关于能观性分析,我个人感觉公式太多了,并且没有想到一个很好的描述方式,理解的也不算太透彻,所以这里还希望有大佬能把这部分写一下,或者单独写一个专题,我觉得那是极好的.
608 |
609 | 另外开源版本的S-MSCKF用的是OC-EKF, 这个主要参考了这两篇论文 [Consistency Analysis and IMprovement of Vision-aided Inertial Navigation](http://xueshu.baidu.com/s?wd=paperuri:(f308abb33c5a80cd81f674dff09513a2)&filter=sc_long_sign&sc_ks_para=q%3DConsistency+Analysis+and+Improvement+of+Vision-aided+Inertial+Navigation&tn=SE_baiduxueshu_c1gjeupa&ie=utf-8&sc_us=13835246309820327221) 和 [On the consistency of Vision-aided Inertial Navigation](http://xueshu.baidu.com/s?wd=paperuri:(c1627c8837b2165d335c77f39de88f7d)&filter=sc_long_sign&sc_ks_para=q%3DOn+the+Consistency+of+Vision-Aided+Inertial+Navigation&tn=SE_baiduxueshu_c1gjeupa&ie=utf-8&sc_us=7890470351829265780). 代码中的实现主要参考了第二篇的给出的公式,我在注释里应该都有注明.
610 |
611 | #### 4.4 滤波器更新机制
612 |
613 | 大致是有两种更新策略,假设新进来一帧图像,这个时候会丢失一些特征点,这个时候丢失的特征点(且三角化成功)用于滤波器更新,如下图所示:
614 |
615 | 
616 |
617 | 那当然随着时间的推移,相机状态会越来越多,这个时候呢, 相机状态会有一个阈值,也就是滑窗的上限, S-MSCKF与msckf1.0有稍微不同,它是当满了之后每次迭代的清除两个,最新的这个状态肯定保持, 清除依据就是帧间的旋转跟位移大小,如下图所示,假设Slide Window的大小正好为7,且已经经过了上面的update过程,那么这个时候还会再update一次,这个时候它的所有特征都会用于更新.因为要移除两个camera state,所以对应的状态和covariance也都需要剔除掉.所以删掉的两个状态其实肯定处于紫色框其中的两个.
618 |
619 | 
620 |
621 | ### 5.S-MSCKF代码流程
622 |
623 | 这里放一张之前做的图,清晰图片从这里[下载](https://github.com/TurtleZhong/msckf_mono/blob/master/msckf_vio.jpg).另外中文注释版本的代码在[这里](https://github.com/TurtleZhong/msckf_mono)
624 |
625 | 
626 |
627 | ### 6.总结
628 |
629 | 全文以S-MSCKF为依托, 主要对MSCKF的理论基础和实现原理及细节进行了讲述, 并且对论文公式进行了详细的推导(很多手写的地方实在是不想敲了,太费时间了), 然后还对[Quaternion kinematics for the error-state Kalman filter](https://arxiv.org/abs/1711.02508) 这本书进行了详细的注释, 同时对开源版本的S-MSCKF的代码进行了注释. 由于笔者水平有限,有些地方理解难免不到位,其中就包括能观性分析这部分还没有做比较到位的解释,最后希望望读者批评指正文中不足的地方.
630 |
631 | ### 附录
632 |
633 | #### A. F矩阵和G矩阵的推导
634 |
635 | 
636 |
637 | 
638 |
639 | 
640 |
641 | #### B. $J_{I}$的计算
642 |
643 | $J_{I}$ 的计算与正文有点出入,但还是先贴上来了,希望哪位大佬能推导得到论文的结果并告知我一下.
644 |
645 | 
646 |
647 | #### C. H矩阵
648 |
649 | 
650 |
651 | 
652 |
653 | 
654 |
655 | #### D. 三种常用数值积分方式:欧拉,中值,RK4积分
656 |
657 | 这部分其实比较简单, 大家也可以参考 参考文献1中附录A部分,也讲的很详细.这里简单附上三张图,分别对应三种积分方式.
658 |
659 | 
660 |
661 | 
662 |
663 | 
664 |
665 | ### 7.参考文献
666 |
667 | (1) [Quaternion kinematics for the error-state Kalman filter](https://arxiv.org/abs/1711.02508)
668 |
669 | (2) [Indirect Kalman Filter for 3D Attitude Estimation-A Tutorial for Quaternion Algebra](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.304.6207&rep=rep1&type=pdf)
670 |
671 | (3) [Consistency Analysis and IMprovement of Vision-aided Inertial Navigation](http://xueshu.baidu.com/s?wd=paperuri:(f308abb33c5a80cd81f674dff09513a2)&filter=sc_long_sign&sc_ks_para=q%3DConsistency+Analysis+and+Improvement+of+Vision-aided+Inertial+Navigation&tn=SE_baiduxueshu_c1gjeupa&ie=utf-8&sc_us=13835246309820327221)
672 |
673 | (4) [On the consistency of Vision-aided Inertial Navigation](http://xueshu.baidu.com/s?wd=paperuri:(c1627c8837b2165d335c77f39de88f7d)&filter=sc_long_sign&sc_ks_para=q%3DOn+the+Consistency+of+Vision-Aided+Inertial+Navigation&tn=SE_baiduxueshu_c1gjeupa&ie=utf-8&sc_us=7890470351829265780).
674 |
675 | (5) [Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight](https://arxiv.org/abs/1712.00036)
676 |
677 | (6) [Monocular Visual Inertial Odometry on a Mobile Device](http://xueshu.baidu.com/s?wd=paperuri:(68dda4a4688ea361c151eab22dedbe17)&filter=sc_long_sign&sc_ks_para=q%3DMonocular+Visual+Inertial+Odometry+on+a+Mobile+Device&tn=SE_baiduxueshu_c1gjeupa&ie=utf-8&sc_us=7517713721102109000)
678 |
679 | (7) [A multi-state constraint Kalman filter for vision-aided inertial navigation](http://xueshu.baidu.com/s?wd=paperuri:(fc81955af6e11c8b7d738d8ef94b512d)&filter=sc_long_sign&sc_ks_para=q%3DA+Multi-State+Constraint+Kalman+Filter+for+Vision-aided+Inertial+Navigation&tn=SE_baiduxueshu_c1gjeupa&ie=utf-8&sc_us=10867325448677835818)
680 |
681 | (8) [视觉SLAM十四讲](https://baike.baidu.com/item/%E8%A7%86%E8%A7%89SLAM%E5%8D%81%E5%9B%9B%E8%AE%B2/20786239?fr=aladdin)
682 |
683 | (9) [卡尔曼滤波与组合导航原理](https://baike.baidu.com/item/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2%E4%B8%8E%E7%BB%84%E5%90%88%E5%AF%BC%E8%88%AA%E5%8E%9F%E7%90%86/12130448?fr=aladdin)
684 |
685 |
686 |
687 |
688 |
689 |
--------------------------------------------------------------------------------
/notes/msckf_notes.emmx:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/TurtleZhong/msckf_notes/e70b4df5ebfb6b47b7769f010516f3ba309b0022/notes/msckf_notes.emmx
--------------------------------------------------------------------------------
/notes/notes.dia:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/TurtleZhong/msckf_notes/e70b4df5ebfb6b47b7769f010516f3ba309b0022/notes/notes.dia
--------------------------------------------------------------------------------
/notes/notes.md:
--------------------------------------------------------------------------------
1 | ## 作者:钟心亮
3 |联系方式:xinliangzhong@foxmail.com
4 | 5 | [TOC] 6 | 7 | ### 符号说明 8 | 9 | ### 0.前言 10 | 11 | ### 1.简介 12 | 13 | MSCKF (Multi-State Constraint Kalman Filter),从2007年提出至今,一直是filter-based SLAM比较经典的实现.据说这也是谷歌tango里面的算法，这要感觉Mingyang Li博士在MSCKF的不懈工作。在传统的EKF-SLAM框架中，特征点的信息会加入到特征向量和协方差矩阵里,这种方法的缺点是特征点的信息会给一个初始深度和初始协方差，如果不正确的话，极容易导致后面不收敛，出现inconsistent的情况。MSCKF维护一个pose的FIFO，按照时间顺序排列，可以称为滑动窗口，一个特征点在滑动窗口的几个位姿都被观察到的话，就会在这几个位姿间建立约束，从而进行KF的更新。 14 | 15 | ### 2.前端 16 | 本文主要针对2017年Kumar实验室开源的S-MSCKF进行详细分析,其实这篇文章整体思路与07年提出的基本上是一脉相承的.作为一个VIO的前端,MSCKF采用的是光流跟踪特征点的方法,特征点使用的是FAST特征,另外这是MSCKF双目的一个实现,双目之间的特征点匹配采用的也是光流,这与传统的基于descriptor匹配的方法不同.前端部分其实相对简单,整个前端部分基本在 **image_processor.cpp**中实现. 17 | 18 | #### 2.1 基本数据结构说明 19 | 20 | ##### A.特征点检测和跟踪的参数 21 | 22 | ```c++ 23 | struct ProcessorConfig { 24 | int grid_row; //划分图像网格的行数 25 | int grid_col; //划分图像网格的列数 26 | int grid_min_feature_num; //每个网格特征点的最小个数 27 | int grid_max_feature_num; //每个网格特征点的最大个数 28 | 29 | int pyramid_levels; //金字塔层数 30 | int patch_size; 31 | int fast_threshold; 32 | int max_iteration; 33 | double track_precision; 34 | double ransac_threshold; 35 | double stereo_threshold; 36 | }; 37 | ``` 38 | 39 | ##### B. 特征点数据 40 | 41 | ```c++ 42 | struct FeatureMetaData { 43 | FeatureIDType id; //unsigned long long int 每个特征都有单独的ID 44 | float response; //Feature的响应值 45 | int lifetime; //Feature的生存周期 46 | cv::Point2f cam0_point; //Feature在左相机的图像坐标 47 | cv::Point2f cam1_point; //Feature在右相机的图像坐标 48 | }; 49 | ``` 50 | 51 | #### 2.2 跟踪流程 52 | 53 | 整体框架如下面的流程图所示: 54 | 55 |  56 | 57 | ##### 2.2.1 Initialization 58 |  59 | ##### 2.2.2 trackFeatures 60 | 61 | 当第一帧初始化完成之后,后面帧则只需要进行跟踪第一帧的特征点,并且提取新的特征点,整个流程如下: 62 | 63 |  64 | 65 | 整个流程还算比较清晰,但是有一个步骤需要单独说明一下,也就是作者在论文中提到的twoPointRansac的方法.我们先来看一下函数原型: 66 | 67 | ```C++ 68 | /** 69 | * @brief 计算原图像帧关键点对应的矫正位置 70 | * @param pts1：上一时刻的关键点位置 71 | * @param pts2:当前时刻跟踪匹配到的关键点位置 72 | * @param R_p_c:根据imu信息计算得到的两个时刻相机的相对旋转信息 73 | * @param distortion_model,intrinsics：相机内参和畸变模型 74 | * @param inlier_error：内点可接受的阈值（关键点距离差） 75 | * @param success_probability：成功的概率 76 | * @return inlier_markers：内点标志位 77 | */ 78 | void ImageProcessor::twoPointRansac( 79 | const vector
244 |
245 | ### 3.基于四元数的 error-state Kalman Filter
246 |
247 | 其实原理部分相当重要,包括你对error-state Kalman Filter的理解,但是如果要从头讲起的话可以说篇幅太长,考虑到做SLAM的同学们基本上都应该知道这本书 [Quaternion kinematics for the error-state Kalman filter](https://arxiv.org/abs/1711.02508), 这本书会让你在四元数,SO3,IMU的模型以及基于IMU的ESKF(error-state Kalman Filter)都会有比较深刻的理解,对应这本书我也做了很多注释,关于基于四元数原理部分由于篇幅限制,我这里不想做太多的说明,但是接下来在讲解S-MSCKF原理部分我们会将参考公式附上.下面是一些我在这本参考资料中的注释图.
248 |
249 | |
|
|
250 | |:----------------------------------------------------------:|:----------------------------------------------------------:|
251 | |
|
|
252 |
253 | 当然重要的事情需要说三遍,那就是[Quaternion kinematics for the error-state Kalman filter](https://arxiv.org/abs/1711.02508)四元数是Hamilton表示方法,而MSCKF中采用的是JPL表示方法,关于这两者的不同,可以参考书中34页,Table2.
254 |
255 | 看过论文代码的同学可能会说,MSCKF这一部分代码参考的是MARS实验室[Indirect Kalman Filter for 3D Attitude Estimation-A Tutorial for Quaternion Algebra](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.304.6207&rep=rep1&type=pdf),答案是肯定的.但是我的建议是以Hamilton那本书为基础,然后自行再去推导MARS出品那一本,那样你的体验会更加深刻.
256 |
257 | ### 4.S-MSCKF
258 |
259 | 巴拉巴拉这里放一点大致性的简介
260 |
261 | #### 4.1 基本原理
262 |
263 | 在讲解之前,我们先来定义一下坐标系,如下图所示:
264 | 
265 | 其中$I$表示IMU机体坐标系(Body Frame),$G$表示的是惯性系(Inertial Frame),$C$表示的是相机坐标系(Camera Frame).
266 |
267 | 作为一个滤波器,我们首先来看滤波器的状态向量,它包含两个部分,IMU状态和Camera状态:
268 | $$
269 | \textbf{x}_{I}=\begin{bmatrix}
270 | _{G}^{I}\textbf{q}^{T} & \textbf{b}_{g}^{T} & ^{G}\textbf{v}_{I}^{T} & \textbf{b}_{a}^{T} &
271 | ^{G}\textbf{p}_{I}^{T} & _{C}^{I}\textbf{q}^{T} & ^{I}\textbf{p}_{C}^{T}
272 | \end{bmatrix}^{T}
273 | \tag{4.1}
274 | $$
275 | 其中$_{G}^{I}\textbf{q}$表示的是从惯性系到机体IMU系的旋转变换, $^{G}\textbf{v}_{I}$和$^{G}\textbf{p}_{I}$分别表示机体在惯性系下的速度和位置,$\textbf{b}_{g}$和$\textbf{b}_{a}$分别代表IMU的bias,而$_{C}^{I}\textbf{q}$ 和$ ^{I}\textbf{p}_{C}$分别代表从相机坐标系到IMU坐标系的旋转与平移,其中以左相机为准. 但是我们注意到旋转实际上只有三个自由度,而且四元数必须是单位单位四元数,那这样额外的约束会使得协方差矩阵奇异,所以我们定义error IMU状态如下:
276 | $$
277 | \tilde{\textbf{x}}_{I}=\begin{bmatrix}
278 | _{G}^{I}\tilde{\theta }^{T} & \tilde{\textbf{b}}_{g}^{T} & ^{G}\tilde{\textbf{v}}_{I}^{T} & \tilde{\textbf{b}}_{a}^{T} &
279 | ^{G}\tilde{\textbf{p}}_{I}^{T} & _{C}^{I}\tilde{\textbf{q}}^{T} & ^{I}\tilde{\textbf{p}}_{C}^{T}
280 | \end{bmatrix}^{T}
281 | \tag{4.2}
282 | $$
283 |
284 | 所以IMU的状态一共是$3\cdot7=21$维向量,后6维度其实是相机与IMU的外参,这6个维度在MSCKF1.0版本是不在状态向量里边的. MSCKF的状态向量还包含另外一个组成部分,那就是N个相机的姿态,那么每个相机的姿态误差向量定义为 $\tilde{\textbf{x}}_{C_{i}}=\left ( \begin{matrix}_{G}^{C_{i}}\tilde{\theta }^{T} & ^{G}\tilde{\textbf{p}}_{C_{i}}^{T}\end{matrix} \right )^T$,所以当滑窗里边有N个相机姿态的时候,整个误差状态向量为:
285 | $$
286 | \tilde{\textbf{x}} = \begin{pmatrix}
287 | \tilde{\textbf{x}}_{I}^{T} & \tilde{\textbf{x}}_{C_{1}}^{T} & ... & \tilde{\textbf{x}}_{C_{N}}^{T}
288 | \end{pmatrix}
289 | \tag{4.3}
290 | $$
291 | 我们以标准error-state KF为准,其过程包含运动模型和观测模型.
292 |
293 | ##### A.运动模型
294 |
295 | 我们知道对于IMU的状态连续时间的运动学有:
296 | $$
297 | \begin{split}
298 | _{G}^{I}\dot{\hat{\textbf{q}}}=\frac{1}{2}\cdot_{G}^{I}\hat{\textbf{q}}\otimes\hat{w} =\frac{1}{2}\Omega (\hat{w})_{G}^{I}\hat{\textbf{q}},\\
299 | \dot{\hat{\textbf{b}}}_{g}=\textbf{0}_{3\times 1},\\
300 | ^{G}\dot{\hat{\textbf{v}}}=C(_{G}^{I}\hat{\textbf{q}})^{T}\hat{\textbf{a}}+^{G}\textbf{g},\\
301 | \dot{\hat{\textbf{b}}}_{a}=\textbf{0}_{3\times 1},\\
302 | ^{G}\dot{\hat{\textbf{p}}_{I}}=^{G}{\hat{\textbf{v}}},\\
303 | _{C}^{I}\dot{\hat{\textbf{q}}}=\textbf{0}_{3\times 1},\\
304 | ^{I}\dot{\hat{\textbf{p}}_{C}}=\textbf{0}_{3\times 1}
305 | \end{split}\tag{4.4}
306 | $$
307 | 其中 $\hat{w}$ 和 $\hat{a}$ 分别为角速度和加速度的估计值(测量值减去bias),即有:
308 | $$
309 | \hat{w}=w_{m}-\hat{b}_{g},\hat{a}=a_{m}-\hat{b}_{a}\tag{4.5}
310 | $$
311 | 其中有几点要说明,其中,
312 | $$
313 | \Omega (\hat{w})=\Omega (\hat{w})=\begin{pmatrix}
314 | -[\hat{w}_{\times }] & w\\
315 | -w^{T} & 0
316 | \end{pmatrix}
317 | \tag{4.6}
318 | $$
319 | 这个直接参考四元数乘法就可以了,然后 $[\hat{w}_{\times }]$ 是 $\hat{w}$的反对称矩阵; $C(\cdot)$表示四元数到旋转矩阵的转换,这个可以参照[1]和[2]. 那按照S-MSCKF的论文所述,我们可以得到以下式子,
320 | $$
321 | \dot{\tilde{\textbf{x}}}_{I}=\textbf{F}\tilde{\textbf{x}}_{I}+
322 | \textbf{G} \textbf{n}_{I}
323 | \tag{4.7}
324 | $$
325 | 其中
326 | $$
327 | \textbf{n}_{I}^{T}=\begin{pmatrix}
328 | \textbf{n}_{g}^{T}& \textbf{n}_{wg}^{T} & \textbf{n}_{a}^{T} & \textbf{n}_{wa}^{T}
329 | \end{pmatrix}^{T}
330 | \tag{4.8}
331 | $$
332 | $\textbf{n}_{g}$ 和 $\textbf{n}_{a}$ 分别代表角速度和加速度的测量噪声,服从高斯分布; $\textbf{n}_{wg}$ 和 $\textbf{n}_{wa}$ 分别代表角速度和加速度的bias的随机游走噪声. $\textbf{F} $ 是 $21\times21$ 大小矩阵, $\textbf{G}$ 是 $21\times12$ 大小的矩阵,其详细推到见附录A.
333 |
334 | 对于IMU的状态来说,我们可以采用RK4的积分方法根据(4.4)求得IMU的状态. 那么对于IMU的协方差矩阵呢,我们需要事先求取状态转移矩阵和离散的运动噪声协方差矩阵,如下:
335 | $$
336 | \Phi_{k}=\Phi(t_{k+1},t_{k})=\textbf{exp}(\int_{t_{k}}^{t_{k+1}} \textbf{F}(\tau)d\tau) \\
337 | \textbf{Q}_{k}=\int_{t_{k}}^{t_{k+1}}\Phi({t_{k+1},\tau})\textbf{GQG}\Phi({t_{k+1},\tau})^{T}d\tau
338 | $$
339 |
340 |
341 | #### 4.2三角化
342 |
343 | #### 4.2 能观测性分析
344 |
345 | ### 5.S-MSCKF代码流程
346 |
347 | ### 6.总结
348 |
349 | ### 附录
350 |
351 | #### A. F矩阵和G矩阵的推导
352 |
353 | #### B. H矩阵
354 |
355 | #### C. 三种常用积分方式:欧拉,中值,RK4积分
356 |
357 | ### 7.参考文献
358 |
359 |
360 |
361 |
362 |
363 |
364 |
365 |
366 |
367 |
--------------------------------------------------------------------------------
/notes/notes.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/TurtleZhong/msckf_notes/e70b4df5ebfb6b47b7769f010516f3ba309b0022/notes/notes.pdf
--------------------------------------------------------------------------------