huasheng007
泡芙3级
泡芙3级
  • UID8
  • 粉丝1
  • 关注0
  • 发帖数31
阅读:1134回复:1

【泡泡机器人原创专栏-VIO系列】(一)如何理解SLAM中的First Estimate Jacobian

楼主#
更多 发布于:2018-10-20 02:53
First Estimate Jacobian是Visual Inertial中的一个很重要的概念,今天,由泡泡机器人的王京和张腾为大家带来他们的一点思考。欢迎大家在下方说说自己对FEJ的想法和思考。
此篇文章属于泡泡机器人原创专栏的VIO系列中的第一篇,后面我们还会陆续对VIO的一些基础知识以及高级知识进行一系列的科普介绍,希望大家看后能够有所收货。

1.Observability能观性 对于一个系统,Observability性质(能观性)[1,2],决定了这个系统在进行状态估计时,哪些自由度是可以被估计出来的。并且其能观性是不受估计方法(Closed-form 方法、EKF、或者Nonlinear Optimization等等)改变的。
与自控理论中的可观性一样,能观性通过Observability Matrix(能观性矩阵)体现,系统Unobservable的状态维数是这个矩阵零空间的维数。(对于非线性系统,相关性质和矩阵比较特别些,会用到Locally Weakly Observable的概念或者其他的定义,未深究,感兴趣可以看[1]和它的参考文献[8])
比如,单目纯视觉SLAM里,尺度和6DOF的绝对位姿——总共7DOF——无法被估计。(绝对位姿的6个自由度可以通过固定某一帧位姿来进行固定,而剩下的尺度却无法通过纯视觉固定下来,这在误差累积下,会造成单目VSLAM的尺度漂移[3])
再比如Visual-Inertial系统里,在运动激励充分(足够多轴向有足够大的加速度/角速度)的情况下,尺度、滚转/俯仰角是可以被估计的,只剩下绝对偏航角、绝对位置无法获得[1],也就是说
对于Visual-Inertial系统在合适的运动模式下Unobservable的维数是4。(甚至相机-IMU外参和零偏也可以估计。)
Full observability requires the camera–IMU platform to undergo rotation about at least two IMU axes and acceleration along two IMU axes.

2.二维机器人EKF-SLAM中First Estimate Jacobian提出 Huang Guoquan老师在[4]中提出First Estimate Jacobian(之后简写成FEJ),论文是针对针对二维机器人3DOF的EKF-SLAM。
上述情况,离散化后EKF系统的传播和测量方程如下

对于3DOF的系统,其能观性矩阵如下
这种Observability性质的改变带来的影响,论文里称为inconsistency
Therefore, the filter gains “nonexistent” information about the robot’s global orientation. This leads to an unjustified reduction in the orientation uncertainty, which in turn, further reduces the uncertainty in all the state variables, causing inconsistency.


3.三维Visual-Inertial系统中FEJ的引入 Li Mingyang在MSCKF2.0[5]中采用FEJ来对MSCKF进行改进。
基本思路和上面一样,作者对标准MSCKF的能观性矩阵进行分析,发现矩阵的零空间维数是3。而前面[1]等已经指出,Visual-Inertial系统在一般情况下的Unobservable维数是4。
这里和二维EKF-SLAM中的yaw一样,对应于yaw的原本Unobservable的自由度,在标准MSCKF中错误的变成Observable了。
同样,这种能观性的改变也会带来inconsistency,而采用FEJ的方法可以修正这个影响。

4.Sliding Window方法中的FEJ 上面都是滤波类型方法的应用,对于优化的方法,OKVIS[6]使用了这一方式,DSO[7]参考OKVIS也采用了FEJ。
we employ first- estimate Jacobians, i.e. whenever linearization of a variable is employed, we fix the linearization point for any subsequent linearization involving that particular variable

对于Sliding window的方法,一般会采用Marginalization的方式把离开window的状态所带有的信息给保留下来。
(Marginalization可以看看贺一家博客和里面的参考文献
SLAM中的marginalization 和 Schur complement
如上图是OKVIS的window,在新增一个KeyFrame时,会把window中的较早的一个KeyFrame以及相关的一些landmark给marginalize掉。
(OKVIS为了保证稀疏性,不是所有的landmark都进行marg。OKVIS中old KeyFrame之间covisible的landmark——Landmarks visible in KF1 but not KF4——被marg,而能被最新的KF或者frame所看到的landmark直接drop,这样只会导致KF与KF之间被fill-in。)
(上面博客中提到,“要marg那些不被其他帧观测到的特征点”,是保证KF与KF之间也不会有fill-in。)
进行Marg时会用到相关状态的Jacobian,按照一般的思路,也是用最新的估计值作为线性化点来计算。以上图对KF1进行Marg为例,KF2/KF3和其他没有被Marg的状态,在这次进行Marg时计算Jacobian的线性化点,和Marg之后进行优化计算、或者对KF2进行Marg时的线性化点,因为状态更新之后就不同了。


最新喜欢:

happybaolianghappyb...
huasheng007
泡芙3级
泡芙3级
  • UID8
  • 粉丝1
  • 关注0
  • 发帖数31
沙发#
发布于:2018-10-20 02:53
******以下为个人理解+跑火车部分****** 个人理解,能观性是系统的固有特性,和采用EKF还是sliding window optimization的方法无关
直观上,Sliding window方式中Jacobian计算时线性化点的不同,和上面EKF-SLAM或者MSCKF中情况是类似的。在EKF/MSCKF中线性化点不同导致的能观性的改变,同样也会造成Sliding window方式中能观性的改变。 (不负责任的猜测:如果严格按照[1]中的方式,通过Lie Derivative来计算Observability矩阵,也会用到Jacobian,相比用真值计算,不同的线性化点可能也会引起矩阵零空间的改变。暂时没去找哪篇论文有证明,所以只是猜测。) 另一种理解,Marginalization的操作,把旧信息以prior的形式保留了下来,等价于一个EKF-SLAM系统,但是被marg的状态固定并且不再更新。它们的线性化点也就固定了,并且计算prior的时候关于未被marg的状态的Jacobian会有不同,也就造成了等价EKF-SLAM系统中Jacobian的不同。EKF-SLAM中采用FEJ来保证consistency,那么Sliding window这种也可以。
******以上为个人理解+跑火车部分******

疑问: 1.对于类似ORBSLAM这种保存有全局地图信息的系统是否需要???
2.线性化点的误差较大,对精度的影响多大?(论文里有说inconsistency的修正带来的好处比线性化点的误差更大点,整体性能提升。)
欢迎大家在下面回复进行讨论。

参考文献 [1]Visual-Inertial Sensor Fusion: Localization, Mapping and Sensor-to-Sensor Self-calibration
[2]Resolvability of Visual-Inertial Structure from Motion in Closed-form
[3]Scale Drift-Aware Large Scale Monocular SLAM
[4]A First-Estimates Jacobian EKF for Improving SLAM Consistency
[5]High-precision, consistent EKF-based visual–inertial odometry
[6]Keyframe-based visual–inertial odometry using nonlinear optimization
[7]Direct Sparse Odometry
[8]Nonlinear controllability and observability
---这里是张腾思考的分割线---
参考文献 RomaTeng/EKF-SLAM-on-Manifold

【版权声明】泡泡机器人SLAM的所有文章全部由泡泡机器人的成员花费大量心血制作而成的原创内容,希望大家珍惜我们的劳动成果,转载请务必注明出自【泡泡机器人SLAM】微信公众号,否则侵权必究!同时,我们也欢迎各位转载到自己的朋友圈,让更多的人能否进入到SLAM这个领域中,让我们共同为推进中国的SLAM事业而努力!
【注】商业转载请联系刘富强(liufuqiang_robot@hotmail.com)进行授权。普通个人转载,请保留版权声明,并且在文章下方放上“泡泡机器人SLAM”微信公众账号的二维码即可。
↑长按上方免费订阅【编辑】王超群,刘富强,吴博,李平,范帝楷,朱小英
游客

返回顶部