观测方程(30)中的A是观测值(通常是二维图像坐标)对状态向量(包含相机位姿和尺度信息)的求导。B是观测值对特征点的位置(三维空间坐标)的求导。这些求导信息是用来更新卡尔曼滤波器的状态估计的,通常使用雅可比矩阵的形式来表示。
求导的具体推导过程是这样的:
假设有一个相机在三维空间中观测到了一个特征点,那么这个相机的位姿可以用一个旋转矩阵R和一个平移向量t表示,特征点的位置可以用一个三维坐标p表示。观测到的二维图像坐标可以用z表示。那么观测方程就是:
z = h(R, t, p)
其中h是一个转化函数,它把三维世界坐标转化成二维图像坐标。这个函数可以使用摄像机投影模型来推导,这里不再赘述。
接下来,我们需要计算观测方程的雅可比矩阵,也就是求导。我们分别求导z对R、t、p的偏导数。
对于观测方程中的旋转矩阵R,我们可以使用叉乘矩阵的形式来表示。假设ω是一个三维向量,那么叉乘矩阵\hat{ω}就是这样的:

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

其中I是单位矩阵。
这个式子是李群的幂级数展开式,可以表示旋转矩阵的形式。我们可以使用这个式子来求导。
对于观测方程中的平移向量t,它可以看作是一个三维向量,所以求导直接就是对每一维求导。
对于观测方程中的特征点p,它也是一个三维向量,所以求导同样是对每一维求导。
最后,我们将所有的偏导数拼接起来就得到了观测方程的雅可比矩阵A。
