在学习RI-MSCKF的过程中,怎么算都不太理解观测方程(30)中的A是什么东西,是谁对谁的求导,B是特征点对状态向量中观测到此特征点的相机旋转和位移求导,那么A是什么呢,不太理解,另外是怎么推导出来的呢,请求赐教
论文出自:An invariant-EKF VINS algorithm for improving consistency,Kanzhi Wu, Teng Zhang
观测方程(30)中的A是观测值(通常是二维图像坐标)对状态向量(包含相机位姿和尺度信息)的求导。B是观测值对特征点的位置(三维空间坐标)的求导。这些求导信息是用来更新卡尔曼滤波器的状态估计的,通常使用雅可比矩阵的形式来表示。
求导的具体推导过程是这样的:
假设有一个相机在三维空间中观测到了一个特征点,那么这个相机的位姿可以用一个旋转矩阵R和一个平移向量t表示,特征点的位置可以用一个三维坐标p表示。观测到的二维图像坐标可以用z表示。那么观测方程就是:
z = h(R, t, p)
其中h是一个转化函数,它把三维世界坐标转化成二维图像坐标。这个函数可以使用摄像机投影模型来推导,这里不再赘述。
接下来,我们需要计算观测方程的雅可比矩阵,也就是求导。我们分别求导z对R、t、p的偏导数。
对于观测方程中的旋转矩阵R,我们可以使用叉乘矩阵的形式来表示。假设ω是一个三维向量,那么叉乘矩阵\hat{ω}就是这样的:
那么旋转矩阵R就可以表示为:
其中I是单位矩阵。
这个式子是李群的幂级数展开式,可以表示旋转矩阵的形式。我们可以使用这个式子来求导。
对于观测方程中的平移向量t,它可以看作是一个三维向量,所以求导直接就是对每一维求导。
对于观测方程中的特征点p,它也是一个三维向量,所以求导同样是对每一维求导。
最后,我们将所有的偏导数拼接起来就得到了观测方程的雅可比矩阵A。
A是观测方程中的测量量$z_{i}$对状态向量$x$的求导,它表示的是当状态发生变化时,测量量的变化情况。
而B则是特征点对状态向量中观测到此特征点的相机旋转和位移求导,它表示的是当观测到的相机位置或姿态发生变化时,测量量的变化情况。
这两个量的组合构成了观测方程中的线性模型,可以用来表示测量量的变化规律。
关于如何推导出A和B,这个过程略微复杂,需要对深度图像投影模型、求导基本知识以及线性代数基础有一定了解。如果您还没有掌握这些基础知识,可以先学习一下,然后再尝试理解观测方程的推导过程。
一文看懂卡尔曼滤波(附全网最详细公式推导)
这个讲的很详细
https://zhuanlan.zhihu.com/p/433560568
参考一下https://blog.csdn.net/sinat_16643223/article/details/119012400