First Estimate Jacobian (FEJ) 如何理解SLAM中的First Estimate Jacobian

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.

tial系统中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

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时的线性化点，因为状态更新之后就不同了。