如何旋转协方差？

我正在开发EKF，并且对协方差矩阵的坐标框架转换有疑问。假设我用对应的6x6协方差矩阵一些测量。这种测量和在一些坐标框架中给出。我需要将测量值转换为另一个坐标系。转换测量本身并不容易，但我还需要转换其协方差，对吗？和之间的平移应该无关紧要，但是我仍然需要旋转它。如果我是正确的，我将如何做？对于，之间的协方差$(x, y, z, roll, pitch, yaw)$$C$$C$$G_1$$G_2$$G_1$$G_2$$x$$y$和，我的第一个想法是简单地应用3D旋转矩阵，但这仅适用于完整6x6协方差矩阵内的3x3子矩阵。我是否需要对所有四个块应用相同的旋转？$z$

协方差定义为

$\begin{align} C &= \mathbb{E}(XX^T) - \mathbb{E}(X)\mathbb{E}(X^T) \end{align}$

其中，在您的情况下，是您的状态向量，是您已经拥有的协方差矩阵。$X \in \mathbb{R}^6$$C$

对于转换后的状态，在您的情况下，$X'=R X$$R \in \mathbb{R}^{6\times 6}$

$\begin{align} C' &= \mathbb{E}(X' X'^T) - \mathbb{E}(X')\mathbb{E}(X'^T) \\ &= \mathbb{E}(R X X^T R^T) - \mathbb{E}(RX)\mathbb{E}(X^T R^T) \\ &= R ~\mathbb{E}(X X^T) ~R^T - R\mathbb{E}(X)\mathbb{E}(X^T)R^T \\ &= R \big(~\mathbb{E}(X X^T) - \mathbb{E}(X)\mathbb{E}(X^T)\big)R^T \\ &= R C R^T \end{align}$

请注意，注意欧拉角。这些通常在行为上不直观，因此您可能无法简单地使用与位置相同的旋转矩阵来旋转它们。请记住，它们通常是在局部坐标系下定义的（在机器人世界中），而位置通常是根据全局坐标系定义的。不过，我不记得他们是否需要特殊对待。

该MRPT库可以为你做到这一点。您需要使用a CPose3DPDFGaussian来表示您的姿势和协方差，然后使用+运算符。

在引擎盖下，它表示您的6DOF协方差为7DOF四元数基础协方差，其中数学更简单。

非常直观的解释，带有针对协方差及其分解的几何解释。

http://www.visiondummy.com/2014/04/geometric-interpretation-covariance-matrix/