VIO/VINS中关于能观性/可观性的详细讲解

以前大概写过一下，太烂了。。。也是没什么人写，再详细讲解一下

整体简洁一点尝试主要用文字说明这个比较关键的点，其实整体和多传感器融合也有很大的关联，无论是外感还是内感传感器，无外乎从运动出发或者从观测出发：

这个部分不需要扯到VIO里面非线性优化那块比较复杂的东西，直接列出线性高斯情况下的运动/观测方程：

运动方程：Xk=A(k-1)X(k-1)+Vk+Wk, k=1,....K

观测方程：Yk=CkXk+Nk， k=0,......,K

这2个式子大家应该都很熟悉了，随便敲的格式可能不是很好请忽略，重点是里面那一串矩阵，只列和能观性/可观性相关的核心矩阵：

A: 转移矩阵 C:观测矩阵

输入Vk其实还有个很重要的矩阵B，一般叫控制矩阵，理解为Vk=B*Uk

输入量Vk的噪声协方差矩阵Q，观测变量Yk的一次实现Ymeas协方差矩阵R

能观性/可观性实际上就是判断对应的运动/观测线性方程组是否有唯一解的过程。(VIO里的非线性优化也是一样，最终的方程组，无论是近似目标函数或者近似残差/误差函数，在能观性/可观性判断上是一致的，不要焦虑)。

判断是否有唯一解的标准条件就是：Q>0， R>0， rank(O)=N

首先除了运动和观测的噪声，系统一定还会有未知的偏差，比如视觉里程计的路标观测偏差，以及IMU的随机游走BIAS等，本身这些偏差分量的估计就是状态估计问题的一部分。通常有偏估计我们要加入一个积分器进行处理，在有偏差的运动与观测模型下，能观性矩阵O需进行增广处理，这里面的能观性矩阵O就是一个C，A与B构成的增广矩阵形式。

举2个最直观的二维小车的例子！(来自：“机器人学中的状态估计")

1. 针对运动方程，如果这个偏差来自于加速度，则系统能观/可观，运动方程有唯一解！ 如果偏差同时来自于速度v与加速度a，会使能观性矩阵O出现相同的列，导致O的秩不再等于N，系统不能观/不可观。

2. 针对观测方程，如果这个系统无偏，则其能观/可观；而当其路标产生偏差时，系统的能观性矩阵O将出现相同的列，导致不能观/不可观。这个非常好理解，即当小车和路标同时移动时，测量不变。

最后是结论：

对于整个机器人状态估计，如VIO，我们最终的目标是求解正定方程：Ax=b，它有3种情况，要么无解，要么唯一解，要么无穷多组解。而在我们的批量BA优化器中，实际是有无穷多组解。

回到VINS系统，4自由度不可观(3平移+yaw)，roll和pitch因重力存在而可观。因此我们的初始化才会如此重要，目的就是对状态进行一个初始的估计，确定Pbc,Qbc,GC0，Bias等，而我们在运行过程中的解依赖于我们对初始条件的选取，从另一个角度来说，初始时刻引入的偏置将一直存在于系统中。