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等,而我们在运行过程中的解依赖于我们对初始条件的选取,从另一个角度来说,初始时刻引入的偏置将一直存在于系统中。

你可能感兴趣的:(VIO与多传感器融合融态,算法,矩阵,线性代数)