关于RI-MSCKF不变卡尔曼滤波MSCKF论文中的公式推导疑问

Question

在学习RI-MSCKF的过程中，怎么算都不太理解观测方程（30）中的A是什么东西，是谁对谁的求导，B是特征点对状态向量中观测到此特征点的相机旋转和位移求导，那么A是什么呢，不太理解，另外是怎么推导出来的呢，请求赐教
论文出自：An invariant-EKF VINS algorithm for improving consistency，Kanzhi Wu, Teng Zhang

Answer 1

观测方程（30）中的A是观测值（通常是二维图像坐标）对状态向量（包含相机位姿和尺度信息）的求导。B是观测值对特征点的位置（三维空间坐标）的求导。这些求导信息是用来更新卡尔曼滤波器的状态估计的，通常使用雅可比矩阵的形式来表示。

求导的具体推导过程是这样的：

假设有一个相机在三维空间中观测到了一个特征点，那么这个相机的位姿可以用一个旋转矩阵R和一个平移向量t表示，特征点的位置可以用一个三维坐标p表示。观测到的二维图像坐标可以用z表示。那么观测方程就是：

z = h(R, t, p)

其中h是一个转化函数，它把三维世界坐标转化成二维图像坐标。这个函数可以使用摄像机投影模型来推导，这里不再赘述。

接下来，我们需要计算观测方程的雅可比矩阵，也就是求导。我们分别求导z对R、t、p的偏导数。
对于观测方程中的旋转矩阵R，我们可以使用叉乘矩阵的形式来表示。假设ω是一个三维向量，那么叉乘矩阵\hat{ω}就是这样的：

那么旋转矩阵R就可以表示为：

其中I是单位矩阵。

这个式子是李群的幂级数展开式，可以表示旋转矩阵的形式。我们可以使用这个式子来求导。

对于观测方程中的平移向量t，它可以看作是一个三维向量，所以求导直接就是对每一维求导。

对于观测方程中的特征点p，它也是一个三维向量，所以求导同样是对每一维求导。

最后，我们将所有的偏导数拼接起来就得到了观测方程的雅可比矩阵A。