系统的可观性问题分析与FEJ的简要说明
1 系统可观性问题分析
1.1 数学基础
对于一个方程组：\(Ax=0\)，其中\(A\)维度为\(m \times n\)（其中列数为未知数的数量），\(x\)维度为\(n \times 1\)，则有
- 矩阵\(A\)是列满秩的（\(m \ge n\)，此处列数也是未知向量x的维数），则每个列向量之间线性无关，则上面方程的只存在0解（\(x\)对每列进行线性组合）；
- 矩阵\(A\)不是列满秩的，则上面方程存在非零解；
对于一个矩阵\(A\)，维度为\(m \times n\)，所有满足\(Ax=0\)的向量\(x\)（维度为\(n \times 1\)）组成的集合\(N\)，可以证明\(N\)包含零向量，并且对于线性运算是封闭的，因此\(N\)是一个向量子空间。这个向量子空间\(N\)就是矩阵\(A\)的零空间。一般来说零空间的自由度就是系统的不可观的状态维度。
矩阵\(A\)可以看作是由一组列向量\(C_1,C_2,...,C_n\)组成的，则如果这组列向量是线性无关的，则矩阵\(A\)是列满秩的，并且其零空间中只包含一个零向量；相反，如果这组列向量是线性相关的，则矩阵\(A\)不是列满秩的，即矩阵的零空间中会包含非零向量。
零空间和可观性的联系在于：我们将\(x\)看成当前系统的状态量，\(A\)看成系统的观测矩阵，则当\(x\)在零空间的维度上进行变化时，我们通过观测矩阵是发现不了的，也就是对于该观测系统，其零空间维度的状态变化是不可观的。
1.2 系统的可观性分析
由上面的描述可知，首先需要找到系统的A矩阵——可观性矩阵，在SLAM系统中，k时刻的观测对系统初始状态的可观性矩阵的定义为 \[ A_k = H_k \Phi(t_k,t_0) \] 其中，\(H_k\)为k时刻的测量雅克比矩阵（比如视觉测量对状态向量的雅克比），\(\Phi(t_k,t_0)\)为从0时刻到t时刻的状态转移矩阵。
在一些基于滤波的vio系统能观性推导中，都是将\(\Phi(t_k,t_{k-1})\)看做imu积分传播阶段的状态转移矩阵，\(\Phi(t_k,t_0)\)看做其累乘。这其实是不准确的，因为系统在k时刻是首先通过imu传播获得先验估计，然后再通过测量更新得到后验估计，所以这样会缺少从先验到后验的状态转移。但是对于\(H_k \Phi(t_k,t_{k-1})\)，因为imu传播获得先验估计之后，如果是在零空间维度上，\(H_k\)获得的测量会是0，此时先验到后验的状态转移（测量为0，则测量更新为单位变换）为单位变换，因此先验到后验的状态转移不会对系统的零空间产生影响，状态转移矩阵\(\Phi(t_k,t_{k-1})\)也可以近似的仅仅看做为imu积分传播阶段的状态转移矩阵。
举一个例子，在黄国权老师的一篇融合gnss测量的vio论文中，有过关于在vio坐标系下系统可观性的探讨。
- 1.首先确定k时刻的状态向量：在vio坐标系下imu的位姿、速度，还有一个视觉路标3d位置，vio坐标系与enu坐标系位姿的外参；
- 2.然后确定k时刻对系统初始状态的可观性矩阵\(A_k\)：\(H_k\)为视觉观测与gnss观测对状态向量的雅克比，\(\Phi(t_k,t_0)\)为依赖imu的从初始时刻到k时刻的状态转移矩阵；
- 3.在重力轴上VIO坐标系相对于ENU坐标系添加一个旋转扰动，获得初始状态的扰动向量，发现这个扰动向量乘以\(A_k\)等于0，因此可以断定这个扰动向量是系统的一个零空间向量；
- 4.在VIO坐标系相对于ENU坐标系的位移上添加一个扰动，获得初始状态的扰动向量，同样发现这个扰动向量乘以\(A_k\)等于0，又发现一个零空间向量；
- 5.因为4中的位移扰动有3个自由度，可以由此找到3个互不相关的扰动向量，由此可以至少确定一个自由度为4的零空间；
- 6.由5找到的零空间可以找到该系统的4个不可观方向；
此处多记录一下这篇论文，论文分析了gps与vio松耦合的定位系统的可观性。首先通过上述分析得到在vio坐标系下该系统不可观（我觉得主要是因为vio系下需要维护vio与enu坐标系之间的外参，这个是因为系统需要输出enu坐标，但是设备每次启动，vio和enu之间的外参有可能是不同的）的；而在enu坐标系下系统是完全可观的（额主要就是因为不需要维护vio与enu之间的外参了）
论文中提到关于在vio系下不可观问题的解决方案是：
- 系统开机后进行初始化：通过轨迹对齐非线性优化确定enu与vio坐标系之间外参；
- 将vio的状态根据1估计的外参转换到enu坐标系下；
- 在enu下进行状态传播与更新，不需要再维护vio与enu之间的外参；
FEJ简要说明
FEJ的思想是：对于一个状态x_k，在传播阶段得到了一个先验的估计，之后的测量更新都是在这个先验的估计上进行展开（这样做的原因是对于k时刻的状态，我们希望对其求雅克比时线性化点始终保持不变）。而在msckf的滑窗更新中，只有当前最新的帧状态才是先验传播估计的值，其他的帧都是测量更新之后的后验估计的值了，因此此时如果对滑窗中之前的帧进行测量更新，应该也是在fej也就是其先验估计的值处展开。否则展开点不同会导致系统零空间发生改变。
与上述零空间讨论一起看，对于系统k时刻的测量\(A_k = H_k \Phi(t_k,t_0)\)，如果使用FEJ，则系统状态转移矩阵\(\Phi(t_k,t_0)\)可以简单的看作\(\Phi(t_n,t_{n-1})\)的累乘，而且\(\Phi(t_n,t_{n-1})\)可以很简单的认为是传播矩阵与更新矩阵的乘积，整个系统完全可以看做一个线性系统。而如果不使用FEJ，这一条件就不再满足（因为每个状态的测量更新会在不同的线性化点进行）。
我不禁想到，优化的方法从原理上是不断迭代靠近目标点，从而每次可以获得更准确的线性化点，这与FEJ简直背道而驰。
参考文献
- [1]. Lee, Woosik & Eckenhoff, Kevin & Geneva, Patrick & Huang, Guoquan. (2020). Intermittent GPS-aided VIO: Online Initialization and Calibration. 10.1109/ICRA40945.2020.9197029.