通过能观性分析理解SLAM系统的可观维度

1 能观性分析大体理解

什么是能观性分析？
能观性分析通过计算可观性矩阵，分析它的零空间的秩，来分析系统哪些状态维度可观/不可观。可观性矩阵对应系统可观测的维度，零空间对应系统不可观的维度。
为什么要做能观性分析？
举个例子：真实VIO系统不能观的维度是4（位置和yaw角），而实际MSCKF不能观的维度变成了3，绕重力轴的旋转（yaw角）被错误地能观了，从而产生了不一致性（Inconsistency），系统误认为yaw角具有更多的信息从而将yaw对应的协方差设得比较小（Under Estimate），最终导致VIO估计精度的下降。能观性分析就是为了能让MSCKF系统不可观的维度与真实系统一致，从而提高VIO精度。

2 可观性定义

如果没有两个点x0, x1，∈M是不可区分的，或者对于所有可能的控制输入都共享相同的输入-输出映射，那么从全局意义上说，一个系统是可观察的。

直观地，这个定义表达了这样一个事实，即状态空间中的一个点与它的相邻点是可以瞬间区分的。总而言之，“如果状态不同，测量值也不同。”

SLAM系统不是一个完全可观的系统，它是一个局部弱可观的系统，对于这个局部弱可观的系统，系统状态可以从测量的输出和已知的控制输入估计出来。

现在，考虑一个系统S，设O为S的可观性矩阵，其行由h(x)的李代数导数的梯度构成。当O在x0处列秩满时，系统S在x0处是局部弱可观的;我们说S在x0处满足可观秩条件(Hermann &Krener 1977))。如果O对所有x， M具有全列秩，则S一般满足可观秩条件，且是局部弱可观的(Isidori 1995)。关于可观性矩阵后面用一个简单的例子来看。

 

以上参考文献：Visual-Inertial Sensor Fusion: Localization,Mapping and Sensor-to-Sensor Self-Calibration。

后面知道：可观性是系统固有属性，只和状态量观测量的定义有关，不受优化滤波等估计算法的影响，不受相机激光雷达等传感器类型的影响，当系统模型确定时，系统状态量的可观测性也就随之确定，无法变更。

对于 SLAM 系统而言（如单目 VO），当我们改变状态量时，测量不变意味着损失函数不会改变，更意味着求解最小二乘时对应的信息矩阵 Λ 存在着零空间。也就是局部是弱可观的。

①单目 SLAM 系统 7 自由度不可观：6 自由度姿态 + 尺度。
②单目 + IMU 系统是 4 自由度不可观：yaw 角 + 3 自由度位置不可观。
roll 和 pitch 由于重力的存在而可观，尺度因子由于加速度计的存在而
可观。

3 可观性矩阵

考虑最简单的情况，对于线性定常离散系统来说，有状态方程和观测方程：

 其中， u ( t ) 是输入量， Y ( t ) 是输出量(观测量)。 X ( t ) 是系统状态量，即待估计的未知量。
定义经过 k 时刻的状态转移矩阵，Φ ( k ) = F k \Phi(k) =F^k Φ(k)=Fk , 即 X ( t + k ) = Φ ( k ) X ( t ) X(t+k)=\Phi(k)X(t) X(t+k)=Φ(k)X(t)。
若 t 0 t_0 t0​ 时刻状态 X ( t 0 ) = X 0 X(t_0)=X_0 X(t0​)=X0​,那么就能够得到一系列的状态值和观测值：

将状态量和输入量分开，可得：

可见，该问题为 A x = b Ax=b Ax=b 的最小二乘问题。
可观性矩阵为：

当 Rank(M) =n时， X0各方向均有约束，全状态可观。当 R a n k ( M ) < n 时，则 X0存在不受约束的方向，这些方向构成了可观性矩阵M的零空间。从上面可以也看出，系统的可观性与输入量无关。

从这里可以看出，在SLAM系统优化问题中的可观性矩阵其实就是信息矩阵 H(Λ)，也可以进一步知道信息矩阵 H(Λ )不满秩。
 

 https://blog.csdn.net/Walking_roll/article/details/120612989