23、笛卡尔空间闭环反馈与卡尔曼滤波的位姿测量

笛卡尔空间闭环反馈与卡尔曼滤波的位姿测量

1. 笛卡尔空间闭环反馈相关矩阵与向量计算

在笛卡尔空间闭环反馈中，对于坐标系 FE3 中的矩阵 $\mathbf{M}_{3,c}$，可根据特定公式（这里对应 Eq. (6.11)）来获取。其具体表达式如下：
[
\begin{align }
\mathbf{M} {3,c}&=\begin{bmatrix}
\mathbf{m} {3,1}-\overline{\mathbf{m}} 2 & \mathbf{m} {3,2}-\overline{\mathbf{m}} 2 & \cdots & \mathbf{m} {3,n}-\overline{\mathbf{m}} 2
\end{bmatrix}\
&=\begin{bmatrix}
(\boldsymbol{\Lambda}^T\overline{\mathbf{m}} {0,1}+q)-(\boldsymbol{\Lambda}^T\overline{\mathbf{m}} 0 + q) & (\boldsymbol{\Lambda}^T\overline{\mathbf{m}} {0,2}+q)-(\boldsymbol{\Lambda}^T\overline{\mathbf{m}} 0 + q) & \cdots & (\boldsymbol{\Lambda}^T\overline{\mathbf{m}} {0,n}+q)-(\boldsy