23、笛卡尔空间闭环反馈与卡尔曼滤波在姿态测量中的应用

笛卡尔空间闭环反馈与卡尔曼滤波在姿态测量中的应用

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

在笛卡尔空间闭环反馈中，涉及到一些重要矩阵和向量的计算。以框架 FE3 为例，矩阵 $\mathbf{M}_{3,c}$ 可根据公式 (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)-(\boldsymb